From 2f65e0d7f72e43b93e007c605531800d52215716 Mon Sep 17 00:00:00 2001 From: Maxim Khomenko Date: Sun, 6 Mar 2016 15:28:26 +0200 Subject: [PATCH 001/456] Some reworking of INDICATOR's and RING's functionality. * RING: phase start and direction of animation now depends on LED's X:Y coordinates on the grid (X+Y) * INDICATOR: it works only at LEDs with defined direction (North, South, East, West). Also the colors are changed: Orange for turns, Blue for acceleration, Red for decceleration. --- src/main/io/ledstrip.c | 112 ++++++++++++----------------------------- 1 file changed, 31 insertions(+), 81 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 701d29a0a5..1fd4bae7a7 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -558,42 +558,6 @@ void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledCon } -typedef enum { - QUADRANT_NORTH_EAST = 1, - QUADRANT_SOUTH_EAST, - QUADRANT_SOUTH_WEST, - QUADRANT_NORTH_WEST -} quadrant_e; - -void applyQuadrantColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const quadrant_e quadrant, const hsvColor_t *color) -{ - switch (quadrant) { - case QUADRANT_NORTH_EAST: - if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, color); - } - return; - - case QUADRANT_SOUTH_EAST: - if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, color); - } - return; - - case QUADRANT_SOUTH_WEST: - if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, color); - } - return; - - case QUADRANT_NORTH_WEST: - if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, color); - } - return; - } -} - void applyLedModeLayer(void) { const ledConfig_t *ledConfig; @@ -713,18 +677,24 @@ void applyLedWarningLayer(uint8_t updateNow) void applyLedIndicatorLayer(uint8_t indicatorFlashState) { const ledConfig_t *ledConfig; - static const hsvColor_t *flashColor; + static const hsvColor_t *turnColor; + static const hsvColor_t *accColor; + static const hsvColor_t *decColor; if (!rxIsReceivingSignal()) { return; } if (indicatorFlashState == 0) { - flashColor = &hsv_orange; + turnColor = &hsv_orange; + accColor = &hsv_blue; + decColor = &hsv_red; } else { - flashColor = &hsv_black; + turnColor = &hsv_black; + accColor = &hsv_black; + decColor = &hsv_black; } - + uint8_t ledIndex; for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { @@ -734,25 +704,31 @@ void applyLedIndicatorLayer(uint8_t indicatorFlashState) if (!(ledConfig->flags & LED_FUNCTION_INDICATOR)) { continue; } - + if (rcCommand[ROLL] > INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); + if ((ledConfig->flags & LED_DIRECTION_EAST)) { + setLedHsv(ledIndex, turnColor); + continue; + } } if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); + if ((ledConfig->flags & LED_DIRECTION_WEST)) { + setLedHsv(ledIndex, turnColor); + continue; + } } if (rcCommand[PITCH] > INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); + if ((ledConfig->flags & LED_DIRECTION_NORTH)) { + setLedHsv(ledIndex, accColor); + } } if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); + if ((ledConfig->flags & LED_DIRECTION_SOUTH)) { + setLedHsv(ledIndex, decColor); + } } } } @@ -790,11 +766,8 @@ void applyLedThrustRingLayer(void) uint8_t ledIndex; // initialised to special value instead of using more memory for a flag. - static uint8_t rotationSeqLedCount = RING_PATTERN_NOT_CALCULATED; static uint8_t rotationPhase = ROTATION_SEQUENCE_LED_COUNT; - bool nextLedOn = false; - uint8_t ledRingIndex = 0; for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; @@ -803,16 +776,15 @@ void applyLedThrustRingLayer(void) continue; } + uint8_t ledRingIndex = GET_LED_X(ledConfig) + GET_LED_Y(ledConfig); bool applyColor = false; + if (ARMING_FLAG(ARMED)) { - if ((ledRingIndex + rotationPhase) % rotationSeqLedCount < ROTATION_SEQUENCE_LED_WIDTH) { + if ((ledRingIndex + rotationPhase) % ROTATION_SEQUENCE_LED_COUNT < ROTATION_SEQUENCE_LED_WIDTH) { applyColor = true; } } else { - if (nextLedOn == false) { - applyColor = true; - } - nextLedOn = !nextLedOn; + applyColor = (ledRingIndex % 2) == 0; } if (applyColor) { @@ -822,33 +794,11 @@ void applyLedThrustRingLayer(void) } setLedHsv(ledIndex, &ringColor); - - ledRingIndex++; } - - uint8_t ledRingLedCount = ledRingIndex; - if (rotationSeqLedCount == RING_PATTERN_NOT_CALCULATED) { - // update ring pattern according to total number of ring leds found - - rotationSeqLedCount = ledRingLedCount; - - // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds - if ((ledRingLedCount % ROTATION_SEQUENCE_LED_COUNT) == 0) { - rotationSeqLedCount = ROTATION_SEQUENCE_LED_COUNT; - } else { - // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds - while ((rotationSeqLedCount > ROTATION_SEQUENCE_LED_COUNT) && ((rotationSeqLedCount % 2) == 0)) { - rotationSeqLedCount >>= 1; - } - } - - // trigger start over - rotationPhase = 1; - } - + rotationPhase--; if (rotationPhase == 0) { - rotationPhase = rotationSeqLedCount; + rotationPhase = ROTATION_SEQUENCE_LED_COUNT; } } From 86626acbdf118f783188e8ae42fdfea985d53e52 Mon Sep 17 00:00:00 2001 From: rav-rav Date: Sun, 3 Jul 2016 23:54:01 +0200 Subject: [PATCH 002/456] add notch filter configurable with gyro_notch_hz and gyro_notch_q --- src/main/blackbox/blackbox.c | 2 ++ src/main/common/filter.c | 39 +++++++++++++++++++++++---------- src/main/common/filter.h | 7 +++++- src/main/config/config.c | 4 +++- src/main/config/config_master.h | 6 +++-- src/main/debug.h | 1 + src/main/flight/mixer.c | 2 +- src/main/io/serial_cli.c | 3 +++ src/main/sensors/acceleration.c | 2 +- src/main/sensors/battery.c | 2 +- src/main/sensors/gyro.c | 25 +++++++++++++++++---- src/main/sensors/gyro.h | 2 +- 12 files changed, 71 insertions(+), 24 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 46bae19584..35c1ba8fc9 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1218,6 +1218,8 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d", (int)(masterConfig.gyro_soft_notch_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_q:%d", (int)(masterConfig.gyro_soft_notch_q * 10.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.acc_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware); diff --git a/src/main/common/filter.c b/src/main/common/filter.c index d059fe882f..98d9dc3c58 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -56,24 +56,39 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT) } /* sets up a biquad Filter */ -void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate) +void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate) +{ + biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); +} +void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType) { - const float sampleRate = 1 / ((float)refreshRate * 0.000001f); - // setup variables - const float omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate; + const float sampleRate = 1 / ((float)refreshRate * 0.000001f); + const float omega = 2 * M_PI_FLOAT * filterFreq / sampleRate; const float sn = sinf(omega); const float cs = cosf(omega); - //this is wrong, should be hyperbolic sine - //alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn); const float alpha = sn / (2 * BIQUAD_Q); - const float b0 = (1 - cs) / 2; - const float b1 = 1 - cs; - const float b2 = (1 - cs) / 2; - const float a0 = 1 + alpha; - const float a1 = -2 * cs; - const float a2 = 1 - alpha; + float b0, b1, b2, a0, a1, a2; + + switch (filterType) { + case FILTER_LPF: + b0 = (1 - cs) / 2; + b1 = 1 - cs; + b2 = (1 - cs) / 2; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; + break; + case FILTER_NOTCH: + b0 = 1; + b1 = -2 * cs; + b2 = 1; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; + break; + } // precompute the coefficients filter->b0 = b0 / a0; diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 66fcb53bb0..d97becc015 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -29,8 +29,13 @@ typedef struct biquadFilter_s { float d1, d2; } biquadFilter_t; +typedef enum { + FILTER_LPF, + FILTER_NOTCH +} filterType_e; -void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate); +void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); +void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType); float biquadFilterApply(biquadFilter_t *filter, float input); void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT); diff --git a/src/main/config/config.c b/src/main/config/config.c index 003b7c32a7..8726a23408 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -455,6 +455,8 @@ static void resetConf(void) masterConfig.gyro_sync_denom = 4; #endif masterConfig.gyro_soft_lpf_hz = 100; + masterConfig.gyro_soft_notch_hz = 0; + masterConfig.gyro_soft_notch_q = 5; masterConfig.pid_process_denom = 2; @@ -715,7 +717,7 @@ void activateConfig(void) ¤tProfile->pidProfile ); - gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz); + gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q); #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index c6f01b4029..9d68c0b267 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -56,9 +56,11 @@ typedef struct master_t { int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes. - uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. + uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_sync_denom; // Gyro sample divider - uint8_t gyro_soft_lpf_hz; // Biqyad gyro lpf hz + uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz + uint16_t gyro_soft_notch_hz; // Biquad gyro notch hz + uint8_t gyro_soft_notch_q; // Biquad gyro notch quality uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) diff --git a/src/main/debug.h b/src/main/debug.h index 4f6250b758..19124266d0 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -50,5 +50,6 @@ typedef enum { DEBUG_MIXER, DEBUG_AIRMODE, DEBUG_PIDLOOP, + DEBUG_NOTCH, DEBUG_COUNT } debugType_e; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b5da4a0170..942d63789a 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -955,7 +955,7 @@ void filterServos(void) if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { if (!servoFilterIsSet) { - biquadFilterInit(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime); + biquadFilterInitLPF(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime); servoFilterIsSet = true; } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e160869df0..35784d94ab 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -451,6 +451,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "MIXER", "AIRMODE", "PIDLOOP", + "NOTCH", }; #ifdef OSD static const char * const lookupTableOsdType[] = { @@ -696,6 +697,8 @@ const clivalue_t valueTable[] = { { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, + { "gyro_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } }, + { "gyro_notch_q", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_notch_q, .config.minmax = { 1, 100 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index da86d37eb4..b5e71f308e 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -192,7 +192,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) if (!accFilterInitialised) { if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime); + biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime); } accFilterInitialised = true; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 23159d87cd..30c03f130f 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -72,7 +72,7 @@ static void updateBatteryVoltage(void) if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample; if (!vbatFilterIsInitialised) { - biquadFilterInit(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update + biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update vbatFilterIsInitialised = true; } vbatSample = biquadFilterApply(&vbatFilter, vbatSample); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 4f425dc1d2..f5996e87af 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -27,6 +27,7 @@ #include "common/filter.h" #include "drivers/sensor.h" +#include "drivers/system.h" #include "drivers/accgyro.h" #include "sensors/sensors.h" #include "io/beeper.h" @@ -43,21 +44,27 @@ float gyroADCf[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; -static biquadFilter_t gyroFilter[XYZ_AXIS_COUNT]; +static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; +static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT]; +static uint16_t gyroSoftNotchHz; +static uint8_t gyroSoftNotchQ; static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz) +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q) { gyroConfig = gyroConfigToUse; gyroSoftLpfHz = gyro_soft_lpf_hz; + gyroSoftNotchHz = gyro_soft_notch_hz; + gyroSoftNotchQ = gyro_soft_notch_q; } void gyroInit(void) { if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - biquadFilterInit(&gyroFilter[axis], gyroSoftLpfHz, gyro.targetLooptime); + biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH); + biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); } } } @@ -157,7 +164,17 @@ void gyroUpdate(void) if (gyroSoftLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - gyroADCf[axis] = biquadFilterApply(&gyroFilter[axis], (float)gyroADC[axis]); + float sample = (float) gyroADC[axis]; + if (gyroSoftNotchHz) { + sample = biquadFilterApply(&gyroFilterNotch[axis], sample); + } + + if (debugMode == DEBUG_NOTCH && axis < 2){ + debug[axis*2 + 0] = gyroADC[axis]; + debug[axis*2 + 1] = lrintf(sample); + } + + gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);; gyroADC[axis] = lrintf(gyroADCf[axis]); } } else { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 2239eb6816..4d2153fa0d 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -40,7 +40,7 @@ typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. } gyroConfig_t; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz); +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q); void gyroSetCalibrationCycles(void); void gyroInit(void); void gyroUpdate(void); From b07c6c492af3d242edda9a3a9d29535cc0300ca7 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 11 Jul 2016 17:20:36 +0100 Subject: [PATCH 003/456] Tidied pwm_output and pwm_mapping --- src/main/drivers/pwm_mapping.c | 13 +-- src/main/drivers/pwm_mapping.h | 16 ---- src/main/drivers/pwm_output.c | 141 ++++++++++++++++++--------------- src/main/drivers/pwm_output.h | 6 ++ 4 files changed, 85 insertions(+), 91 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 80d2b01d92..d6af36104e 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -31,11 +31,6 @@ #include "pwm_rx.h" #include "pwm_mapping.h" -void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate); -void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); - /* Configuration maps @@ -314,14 +309,14 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } #endif if (init->useFastPwm) { - pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->pwmProtocolType, init->motorPwmRate, init->idlePulse); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT|PWM_PF_OUTPUT_PROTOCOL_PWM ; + pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->pwmProtocolType); + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_OUTPUT_PROTOCOL_ONESHOT; } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) { pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM; + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_MOTOR_MODE_BRUSHED; } else { pwmBrushlessMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM ; + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM; } pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].index = pwmOutputConfiguration.motorCount; pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].timerHardware = timerHardwarePtr; diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index ffb5540995..ffd16e74d6 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -30,24 +30,8 @@ #error Invalid motor/servo/port configuration #endif -#define PULSE_1MS (1000) // 1ms pulse width -#define MAX_INPUTS 8 #define PWM_TIMER_MHZ 1 -#if defined(STM32F40_41xxx) // must be multiples of timer clock -#define ONESHOT42_TIMER_MHZ 21 -#define ONESHOT125_TIMER_MHZ 12 -#define PWM_BRUSHED_TIMER_MHZ 21 -#define MULTISHOT_TIMER_MHZ 84 -#else -#define PWM_BRUSHED_TIMER_MHZ 24 -#define MULTISHOT_TIMER_MHZ 72 -#define ONESHOT42_TIMER_MHZ 24 -#define ONESHOT125_TIMER_MHZ 8 -#endif - -#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) -#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) typedef struct sonarIOConfig_s { ioTag_t triggerTag; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index fd8446584d..e46eacebab 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -17,21 +17,31 @@ #include #include - -#include #include #include "platform.h" #include "io.h" #include "timer.h" - -#include "flight/failsafe.h" // FIXME dependency into the main code from a driver - #include "pwm_mapping.h" - #include "pwm_output.h" +#if defined(STM32F40_41xxx) // must be multiples of timer clock +#define ONESHOT125_TIMER_MHZ 12 +#define ONESHOT42_TIMER_MHZ 21 +#define MULTISHOT_TIMER_MHZ 84 +#define PWM_BRUSHED_TIMER_MHZ 21 +#else +#define ONESHOT125_TIMER_MHZ 8 +#define ONESHOT42_TIMER_MHZ 24 +#define MULTISHOT_TIMER_MHZ 72 +#define PWM_BRUSHED_TIMER_MHZ 24 +#endif + +#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) +#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) + + typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors typedef struct { @@ -52,6 +62,8 @@ static pwmOutputPort_t *servos[MAX_PWM_SERVOS]; static uint8_t allocatedOutputPortCount = 0; static bool pwmMotorsEnabled = true; + + static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t ouputPolarity) { TIM_OCInitTypeDef TIM_OCInitStructure; @@ -65,22 +77,22 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; switch (channel) { - case TIM_Channel_1: - TIM_OC1Init(tim, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(tim, &TIM_OCInitStructure); - TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(tim, &TIM_OCInitStructure); - TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(tim, &TIM_OCInitStructure); - TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable); - break; + case TIM_Channel_1: + TIM_OC1Init(tim, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(tim, &TIM_OCInitStructure); + TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(tim, &TIM_OCInitStructure); + TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(tim, &TIM_OCInitStructure); + TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable); + break; } } @@ -90,7 +102,7 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 configTimeBase(timerHardware->tim, period, mhz); - IO_t io = IOGetByTag(timerHardware->tag); + const IO_t io = IOGetByTag(timerHardware->tag); IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount); IOConfigGPIO(io, IOCFG_AF_PP); @@ -102,18 +114,18 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 TIM_Cmd(timerHardware->tim, ENABLE); switch (timerHardware->channel) { - case TIM_Channel_1: - p->ccr = &timerHardware->tim->CCR1; - break; - case TIM_Channel_2: - p->ccr = &timerHardware->tim->CCR2; - break; - case TIM_Channel_3: - p->ccr = &timerHardware->tim->CCR3; - break; - case TIM_Channel_4: - p->ccr = &timerHardware->tim->CCR4; - break; + case TIM_Channel_1: + p->ccr = &timerHardware->tim->CCR1; + break; + case TIM_Channel_2: + p->ccr = &timerHardware->tim->CCR2; + break; + case TIM_Channel_3: + p->ccr = &timerHardware->tim->CCR3; + break; + case TIM_Channel_4: + p->ccr = &timerHardware->tim->CCR4; + break; } p->period = period; p->tim = timerHardware->tim; @@ -131,16 +143,16 @@ static void pwmWriteStandard(uint8_t index, uint16_t value) *motors[index]->ccr = value; } -static void pwmWriteOneShot42(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); -} - static void pwmWriteOneShot125(uint8_t index, uint16_t value) { *motors[index]->ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f)); } +static void pwmWriteOneShot42(uint8_t index, uint16_t value) +{ + *motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); +} + static void pwmWriteMultiShot(uint8_t index, uint16_t value) { *motors[index]->ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); @@ -148,15 +160,14 @@ static void pwmWriteMultiShot(uint8_t index, uint16_t value) void pwmWriteMotor(uint8_t index, uint16_t value) { - if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) + if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) { motors[index]->pwmWritePtr(index, value); + } } void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { - uint8_t index; - - for(index = 0; index < motorCount; index++){ + for(int index = 0; index < motorCount; index++){ // Set the compare register to 0, which stops the output pulsing if the timer overflows *motors[index]->ccr = 0; } @@ -174,17 +185,14 @@ void pwmEnableMotors(void) void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) { - uint8_t index; TIM_TypeDef *lastTimerPtr = NULL; - for (index = 0; index < motorCount; index++) { - + for (int index = 0; index < motorCount; index++) { // Force the timer to overflow if it's the first motor to output, or if we change timers if (motors[index]->tim != lastTimerPtr) { lastTimerPtr = motors[index]->tim; timerForceOverflow(motors[index]->tim); } - // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. *motors[index]->ccr = 0; @@ -193,44 +201,44 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate) { - uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; + const uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, 0); motors[motorIndex]->pwmWritePtr = pwmWriteBrushed; } void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) { - uint32_t hz = PWM_TIMER_MHZ * 1000000; + const uint32_t hz = PWM_TIMER_MHZ * 1000000; motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse); motors[motorIndex]->pwmWritePtr = pwmWriteStandard; } -void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint16_t idlePulse) +void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t fastPwmProtocolType) { uint32_t timerMhzCounter; 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; + default: + case (PWM_TYPE_ONESHOT125): + timerMhzCounter = ONESHOT125_TIMER_MHZ; + motors[motorIndex]->pwmWritePtr = pwmWriteOneShot125; + break; + case (PWM_TYPE_ONESHOT42): + timerMhzCounter = ONESHOT42_TIMER_MHZ; + motors[motorIndex]->pwmWritePtr = pwmWriteOneShot42; + break; + case (PWM_TYPE_MULTISHOT): + timerMhzCounter = MULTISHOT_TIMER_MHZ; + motors[motorIndex]->pwmWritePtr = pwmWriteMultiShot; + break; } if (motorPwmRate > 0) { - uint32_t hz = timerMhzCounter * 1000000; + const 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 : - ((fastPwmProtocolType == PWM_TYPE_ONESHOT125) ? pwmWriteOneShot125 : - pwmWriteOneShot42); } #ifdef USE_SERVOS @@ -241,7 +249,8 @@ void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, ui void pwmWriteServo(uint8_t index, uint16_t value) { - if (servos[index] && index < MAX_SERVOS) + if (servos[index] && index < MAX_SERVOS) { *servos[index]->ccr = value; + } } #endif diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index c89e7fa802..b9bc62c68d 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -25,6 +25,12 @@ typedef enum { PWM_TYPE_BRUSHED } motorPwmProtocolTypes_e; +struct timerHardware_s; +void pwmBrushedMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate); +void pwmBrushlessMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); +void pwmFastPwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t fastPwmProtocolType); +void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); + void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmCompleteOneshotMotorUpdate(uint8_t motorCount); From 32293908482017a8eba1d1ab1805bea624c6ca1c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 12 Jul 2016 00:07:41 +0100 Subject: [PATCH 004/456] Fixed setting of pwmWritePtr --- src/main/drivers/pwm_output.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index e46eacebab..de76be4b89 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -216,20 +216,21 @@ void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motor void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t fastPwmProtocolType) { uint32_t timerMhzCounter; + pwmWriteFuncPtr pwmWritePtr; switch (fastPwmProtocolType) { default: case (PWM_TYPE_ONESHOT125): timerMhzCounter = ONESHOT125_TIMER_MHZ; - motors[motorIndex]->pwmWritePtr = pwmWriteOneShot125; + pwmWritePtr = pwmWriteOneShot125; break; case (PWM_TYPE_ONESHOT42): timerMhzCounter = ONESHOT42_TIMER_MHZ; - motors[motorIndex]->pwmWritePtr = pwmWriteOneShot42; + pwmWritePtr = pwmWriteOneShot42; break; case (PWM_TYPE_MULTISHOT): timerMhzCounter = MULTISHOT_TIMER_MHZ; - motors[motorIndex]->pwmWritePtr = pwmWriteMultiShot; + pwmWritePtr = pwmWriteMultiShot; break; } @@ -239,6 +240,7 @@ void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIn } else { motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, 0xFFFF, 0); } + motors[motorIndex]->pwmWritePtr = pwmWritePtr; } #ifdef USE_SERVOS From 473d78790e03fad836c26fdd61f5636bf0de8158 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 12 Jul 2016 21:19:53 +1000 Subject: [PATCH 005/456] Disable IRQs whilst transferring bytes on VCP (F4) --- src/main/vcpf4/usbd_cdc_vcp.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 5672eeae55..42913891ea 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -223,6 +223,8 @@ static uint32_t rxPackets = 0; static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) { + __disable_irq(); + rxPackets++; for (uint32_t i = 0; i < Len; i++) { @@ -232,6 +234,7 @@ static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) rxTotalBytes++; } + __enable_irq(); if(receiveLength > (USB_RX_BUFSIZE-1)) return USBD_FAIL; From aab0a56068fc3404bf07032b6d8ec9ccec7f024e Mon Sep 17 00:00:00 2001 From: gaelj Date: Sat, 25 Jun 2016 13:03:38 +0200 Subject: [PATCH 006/456] CF new LED strip port Requires configurator update. LED strip Cleanup. New LED functions: - GPS (G) - Battery (L) - RSSI (S) - Larson Scanner (O) - Blink (B) - Blink on landing (N) Mode colors and special colors are configurable (mode_color in CLI, MSP_LED_STRIP_MODECOLOR & MSP_SET_LED_STRIP_MODECOLOR) --- src/main/common/utils.h | 11 + src/main/config/config.c | 4 +- src/main/config/config_master.h | 6 +- src/main/drivers/light_ws2811strip.h | 3 - src/main/io/ledstrip.c | 1603 ++++++++++++++------------ src/main/io/ledstrip.h | 206 +++- src/main/io/serial_cli.c | 58 +- src/main/io/serial_msp.c | 62 +- src/main/io/serial_msp.h | 2 + src/main/main.c | 4 +- src/main/sensors/battery.c | 2 +- 11 files changed, 1132 insertions(+), 829 deletions(-) diff --git a/src/main/common/utils.h b/src/main/common/utils.h index a5c0591918..27a51f3984 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -71,4 +71,15 @@ http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html static inline int16_t cmp16(uint16_t a, uint16_t b) { return a-b; } static inline int32_t cmp32(uint32_t a, uint32_t b) { return a-b; } +// using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions +// than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions) +#ifdef UNIT_TEST +// Call memcpy when building unittest - this is easier that asm symbol name mangling (symbols start with _underscore on win32) +#include +static inline void memcpy_fn ( void * destination, const void * source, size_t num ) { memcpy(destination, source, num); }; +#else +void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("memcpy"); +#endif + + #endif diff --git a/src/main/config/config.c b/src/main/config/config.c index 06ba7c9c09..f42a549862 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -612,8 +612,10 @@ static void resetConf(void) } #ifdef LED_STRIP - applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT); + applyDefaultColors(masterConfig.colors); applyDefaultLedStripConfig(masterConfig.ledConfigs); + applyDefaultModeColors(masterConfig.modeColors); + applyDefaultSpecialColors(&(masterConfig.specialColors)); masterConfig.ledstrip_visual_beeper = 0; #endif diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index c6f01b4029..329fd1f54c 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -117,8 +117,10 @@ typedef struct master_t { telemetryConfig_t telemetryConfig; #ifdef LED_STRIP - ledConfig_t ledConfigs[MAX_LED_STRIP_LENGTH]; - hsvColor_t colors[CONFIGURABLE_COLOR_COUNT]; + ledConfig_t ledConfigs[LED_MAX_STRIP_LENGTH]; + hsvColor_t colors[LED_CONFIGURABLE_COLOR_COUNT]; + modeColorIndexes_t modeColors[LED_MODE_COUNT]; + specialColorIndexes_t specialColors; uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on #endif diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 974c19c655..853ac7acbc 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -53,6 +53,3 @@ bool isWS2811LedStripReady(void); extern uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; extern volatile uint8_t ws2811LedDataTransferInProgress; - -extern const hsvColor_t hsv_white; -extern const hsvColor_t hsv_black; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index e74ca001a0..866198857c 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -42,6 +42,7 @@ #include #include "common/axis.h" +#include "common/utils.h" #include "io/rc_controls.h" @@ -61,6 +62,8 @@ #include "io/osd.h" #include "io/vtx.h" +#include "rx/rx.h" + #include "flight/failsafe.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -74,6 +77,13 @@ #include "config/config_profile.h" #include "config/config_master.h" +/* +PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0); +PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0); +PG_REGISTER_ARR_WITH_RESET_FN(modeColorIndexes_t, LED_MODE_COUNT, modeColors, PG_MODE_COLOR_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(specialColorIndexes_t, specialColors, PG_SPECIAL_COLOR_CONFIG, 0); +*/ + static bool ledStripInitialised = false; static bool ledStripEnabled = true; @@ -82,73 +92,13 @@ static void ledStripDisable(void); //#define USE_LED_ANIMATION //#define USE_LED_RING_DEFAULT_CONFIG -// timers -#ifdef USE_LED_ANIMATION -static uint32_t nextAnimationUpdateAt = 0; +#define LED_STRIP_HZ(hz) ((int32_t)((1000 * 1000) / (hz))) +#define LED_STRIP_MS(ms) ((int32_t)(1000 * (ms))) + +#if LED_MAX_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH +# error "Led strip length must match driver" #endif -static uint32_t nextIndicatorFlashAt = 0; -static uint32_t nextWarningFlashAt = 0; -static uint32_t nextRotationUpdateAt = 0; - -#define LED_STRIP_20HZ ((1000 * 1000) / 20) -#define LED_STRIP_10HZ ((1000 * 1000) / 10) -#define LED_STRIP_5HZ ((1000 * 1000) / 5) - -#if MAX_LED_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH -#error "Led strip length must match driver" -#endif - -// H S V -#define LED_BLACK { 0, 0, 0} -#define LED_WHITE { 0, 255, 255} -#define LED_RED { 0, 0, 255} -#define LED_ORANGE { 30, 0, 255} -#define LED_YELLOW { 60, 0, 255} -#define LED_LIME_GREEN { 90, 0, 255} -#define LED_GREEN {120, 0, 255} -#define LED_MINT_GREEN {150, 0, 255} -#define LED_CYAN {180, 0, 255} -#define LED_LIGHT_BLUE {210, 0, 255} -#define LED_BLUE {240, 0, 255} -#define LED_DARK_VIOLET {270, 0, 255} -#define LED_MAGENTA {300, 0, 255} -#define LED_DEEP_PINK {330, 0, 255} - -const hsvColor_t hsv_black = LED_BLACK; -const hsvColor_t hsv_white = LED_WHITE; -const hsvColor_t hsv_red = LED_RED; -const hsvColor_t hsv_orange = LED_ORANGE; -const hsvColor_t hsv_yellow = LED_YELLOW; -const hsvColor_t hsv_limeGreen = LED_LIME_GREEN; -const hsvColor_t hsv_green = LED_GREEN; -const hsvColor_t hsv_mintGreen = LED_MINT_GREEN; -const hsvColor_t hsv_cyan = LED_CYAN; -const hsvColor_t hsv_lightBlue = LED_LIGHT_BLUE; -const hsvColor_t hsv_blue = LED_BLUE; -const hsvColor_t hsv_darkViolet = LED_DARK_VIOLET; -const hsvColor_t hsv_magenta = LED_MAGENTA; -const hsvColor_t hsv_deepPink = LED_DEEP_PINK; - -#define LED_DIRECTION_COUNT 6 - -const hsvColor_t * const defaultColors[] = { - &hsv_black, - &hsv_white, - &hsv_red, - &hsv_orange, - &hsv_yellow, - &hsv_limeGreen, - &hsv_green, - &hsv_mintGreen, - &hsv_cyan, - &hsv_lightBlue, - &hsv_blue, - &hsv_darkViolet, - &hsv_magenta, - &hsv_deepPink -}; - typedef enum { COLOR_BLACK = 0, COLOR_WHITE, @@ -164,98 +114,134 @@ typedef enum { COLOR_DARK_VIOLET, COLOR_MAGENTA, COLOR_DEEP_PINK, -} colorIds; +} colorId_e; -typedef enum { - DIRECTION_NORTH = 0, - DIRECTION_EAST, - DIRECTION_SOUTH, - DIRECTION_WEST, - DIRECTION_UP, - DIRECTION_DOWN -} directionId_e; - -typedef struct modeColorIndexes_s { - uint8_t north; - uint8_t east; - uint8_t south; - uint8_t west; - uint8_t up; - uint8_t down; -} modeColorIndexes_t; - - -// Note, the color index used for the mode colors below refer to the default colors. -// if the colors are reconfigured the index is still valid but the displayed color might -// be different. -// See colors[] and defaultColors[] and applyDefaultColors[] - -static const modeColorIndexes_t orientationModeColors = { - COLOR_WHITE, - COLOR_DARK_VIOLET, - COLOR_RED, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE +const hsvColor_t hsv[] = { + // H S V + [COLOR_BLACK] = { 0, 0, 0}, + [COLOR_WHITE] = { 0, 255, 255}, + [COLOR_RED] = { 0, 0, 255}, + [COLOR_ORANGE] = { 30, 0, 255}, + [COLOR_YELLOW] = { 60, 0, 255}, + [COLOR_LIME_GREEN] = { 90, 0, 255}, + [COLOR_GREEN] = {120, 0, 255}, + [COLOR_MINT_GREEN] = {150, 0, 255}, + [COLOR_CYAN] = {180, 0, 255}, + [COLOR_LIGHT_BLUE] = {210, 0, 255}, + [COLOR_BLUE] = {240, 0, 255}, + [COLOR_DARK_VIOLET] = {270, 0, 255}, + [COLOR_MAGENTA] = {300, 0, 255}, + [COLOR_DEEP_PINK] = {330, 0, 255}, }; +// macro to save typing on default colors +#define HSV(color) (hsv[COLOR_ ## color]) -static const modeColorIndexes_t headfreeModeColors = { - COLOR_LIME_GREEN, - COLOR_DARK_VIOLET, - COLOR_ORANGE, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE +STATIC_UNIT_TESTED uint8_t ledGridWidth; +STATIC_UNIT_TESTED uint8_t ledGridHeight; +// grid offsets +STATIC_UNIT_TESTED uint8_t highestYValueForNorth; +STATIC_UNIT_TESTED uint8_t lowestYValueForSouth; +STATIC_UNIT_TESTED uint8_t highestXValueForWest; +STATIC_UNIT_TESTED uint8_t lowestXValueForEast; + +STATIC_UNIT_TESTED ledCounts_t ledCounts; + +// macro for initializer +#define LF(name) LED_FUNCTION_ ## name +#define LO(name) LED_FLAG_OVERLAY(LED_OVERLAY_ ## name) +#define LD(name) LED_FLAG_DIRECTION(LED_DIRECTION_ ## name) + +#ifdef USE_LED_RING_DEFAULT_CONFIG +static const ledConfig_t defaultLedStripConfig[] = { + DEFINE_LED( 2, 2, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 2, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 2, 0, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 0, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 0, 0, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 0, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 0, 2, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 2, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), }; +#else +static const ledConfig_t defaultLedStripConfig[] = { + DEFINE_LED(15, 15, 0, LD(SOUTH) | LD(EAST), LF(ARM_STATE), LO(INDICATOR), 0), -static const modeColorIndexes_t horizonModeColors = { - COLOR_BLUE, - COLOR_DARK_VIOLET, - COLOR_YELLOW, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE -}; + DEFINE_LED(15, 8, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED(15, 7, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0), -static const modeColorIndexes_t angleModeColors = { - COLOR_CYAN, - COLOR_DARK_VIOLET, - COLOR_YELLOW, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE -}; + DEFINE_LED(15, 0, 0, LD(NORTH) | LD(EAST), LF(ARM_STATE) , LO(INDICATOR), 0), + + DEFINE_LED( 8, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0), + DEFINE_LED( 7, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0), + + DEFINE_LED( 0, 0, 0, LD(NORTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0), + + DEFINE_LED( 0, 7, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 0, 8, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0), + + DEFINE_LED( 0, 15, 0, LD(SOUTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0), + + DEFINE_LED( 7, 15, 0, LD(SOUTH) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 8, 15, 0, LD(SOUTH) , LF(FLIGHT_MODE), LO(WARNING), 0), + + DEFINE_LED( 7, 7, 0, LD(UP) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 8, 7, 0, LD(UP) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 7, 8, 0, LD(DOWN) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 8, 8, 0, LD(DOWN) , LF(FLIGHT_MODE), LO(WARNING), 0), + + DEFINE_LED( 8, 9, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 9, 10, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED(10, 11, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED(10, 12, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 9, 13, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 8, 14, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 7, 14, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 6, 13, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 5, 12, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 5, 11, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 6, 10, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 7, 9, 3, 0, LF(THRUST_RING), 0, 0), -#ifdef MAG -static const modeColorIndexes_t magModeColors = { - COLOR_MINT_GREEN, - COLOR_DARK_VIOLET, - COLOR_ORANGE, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE }; #endif -static const modeColorIndexes_t baroModeColors = { - COLOR_LIGHT_BLUE, - COLOR_DARK_VIOLET, - COLOR_RED, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE +#undef LD +#undef LF +#undef LO + +static const modeColorIndexes_t defaultModeColors[] = { + // NORTH EAST SOUTH WEST UP DOWN + [LED_MODE_ORIENTATION] = {{ COLOR_WHITE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_HEADFREE] = {{ COLOR_LIME_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_HORIZON] = {{ COLOR_BLUE, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_ANGLE] = {{ COLOR_CYAN, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_MAG] = {{ COLOR_MINT_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_BARO] = {{ COLOR_LIGHT_BLUE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, +}; + +static const specialColorIndexes_t defaultSpecialColors[] = { + {{ [LED_SCOLOR_DISARMED] = COLOR_GREEN, + [LED_SCOLOR_ARMED] = COLOR_BLUE, + [LED_SCOLOR_ANIMATION] = COLOR_WHITE, + [LED_SCOLOR_BACKGROUND] = COLOR_BLACK, + [LED_SCOLOR_BLINKBACKGROUND] = COLOR_BLACK, + [LED_SCOLOR_GPSNOSATS] = COLOR_RED, + [LED_SCOLOR_GPSNOLOCK] = COLOR_ORANGE, + [LED_SCOLOR_GPSLOCKED] = COLOR_GREEN, + }} }; -uint8_t ledGridWidth; -uint8_t ledGridHeight; -uint8_t ledCount; -uint8_t ledsInRingCount; - -ledConfig_t *ledConfigs; -hsvColor_t *colors; +static int scaledThrottle; + + +/* #ifdef USE_LED_RING_DEFAULT_CONFIG const ledConfig_t defaultLedStripConfig[] = { { CALCULATE_LED_XY( 2, 2), 3, LED_FUNCTION_THRUST_RING}, @@ -273,16 +259,16 @@ const ledConfig_t defaultLedStripConfig[] = { }; #elif defined(USE_COLIBTI_RACE_LED_DEFAULT_CONFIG) const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, }; #else const ledConfig_t defaultLedStripConfig[] = { @@ -326,118 +312,58 @@ const ledConfig_t defaultLedStripConfig[] = { }; #endif +*/ -/* - * 6 coords @nn,nn - * 4 direction @## - * 6 modes @#### - * = 16 bytes per led - * 16 * 32 leds = 512 bytes storage needed worst case. - * = not efficient to store led configs as strings in flash. - * = becomes a problem to send all the data via cli due to serial/cli buffers - */ -typedef enum { - X_COORDINATE, - Y_COORDINATE, - DIRECTIONS, - FUNCTIONS, - RING_COLORS -} parseState_e; +static void updateLedRingCounts(void); -#define PARSE_STATE_COUNT 5 - -static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0' }; - -static const char directionCodes[] = { 'N', 'E', 'S', 'W', 'U', 'D' }; -#define DIRECTION_COUNT (sizeof(directionCodes) / sizeof(directionCodes[0])) -static const uint8_t directionMappings[DIRECTION_COUNT] = { - LED_DIRECTION_NORTH, - LED_DIRECTION_EAST, - LED_DIRECTION_SOUTH, - LED_DIRECTION_WEST, - LED_DIRECTION_UP, - LED_DIRECTION_DOWN -}; - -static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T', 'R', 'C' }; -#define FUNCTION_COUNT (sizeof(functionCodes) / sizeof(functionCodes[0])) -static const uint16_t functionMappings[FUNCTION_COUNT] = { - LED_FUNCTION_INDICATOR, - LED_FUNCTION_WARNING, - LED_FUNCTION_FLIGHT_MODE, - LED_FUNCTION_ARM_STATE, - LED_FUNCTION_THROTTLE, - LED_FUNCTION_THRUST_RING, - LED_FUNCTION_COLOR -}; - -// grid offsets -uint8_t highestYValueForNorth; -uint8_t lowestYValueForSouth; -uint8_t highestXValueForWest; -uint8_t lowestXValueForEast; - -void determineLedStripDimensions(void) +STATIC_UNIT_TESTED void determineLedStripDimensions(void) { - ledGridWidth = 0; - ledGridHeight = 0; + int maxX = 0; + int maxY = 0; - uint8_t ledIndex; - const ledConfig_t *ledConfig; + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - ledConfig = &ledConfigs[ledIndex]; - - if (GET_LED_X(ledConfig) >= ledGridWidth) { - ledGridWidth = GET_LED_X(ledConfig) + 1; - } - if (GET_LED_Y(ledConfig) >= ledGridHeight) { - ledGridHeight = GET_LED_Y(ledConfig) + 1; - } + maxX = MAX(ledGetX(ledConfig), maxX); + maxY = MAX(ledGetY(ledConfig), maxY); } + ledGridWidth = maxX + 1; + ledGridHeight = maxY + 1; } -void determineOrientationLimits(void) +STATIC_UNIT_TESTED void determineOrientationLimits(void) { - bool isOddHeight = (ledGridHeight & 1); - bool isOddWidth = (ledGridWidth & 1); - uint8_t heightModifier = isOddHeight ? 1 : 0; - uint8_t widthModifier = isOddWidth ? 1 : 0; - highestYValueForNorth = (ledGridHeight / 2) - 1; - lowestYValueForSouth = (ledGridHeight / 2) + heightModifier; + lowestYValueForSouth = ((ledGridHeight + 1) / 2); highestXValueForWest = (ledGridWidth / 2) - 1; - lowestXValueForEast = (ledGridWidth / 2) + widthModifier; + lowestXValueForEast = ((ledGridWidth + 1) / 2); } -void updateLedCount(void) +STATIC_UNIT_TESTED void updateLedCount(void) { - const ledConfig_t *ledConfig; - uint8_t ledIndex; - ledCount = 0; - ledsInRingCount = 0; + int count = 0, countRing = 0, countScanner= 0; - if( ledConfigs == 0 ){ - return; - } + for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - for (ledIndex = 0; ledIndex < MAX_LED_STRIP_LENGTH; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (ledConfig->flags == 0 && ledConfig->xy == 0) { + if (!(*ledConfig & (LED_POS_MASK | LED_FUNCTION_MASK | LED_OVERLAY_MASK))) break; - } - ledCount++; + count++; - if ((ledConfig->flags & LED_FUNCTION_THRUST_RING)) { - ledsInRingCount++; - } + if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) + countRing++; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) + countScanner++; } + + ledCounts.count = count; + ledCounts.ring = countRing; + ledCounts.larson = countScanner; } void reevaluateLedConfig(void) @@ -445,663 +371,890 @@ void reevaluateLedConfig(void) updateLedCount(); determineLedStripDimensions(); determineOrientationLimits(); + updateLedRingCounts(); } -#define CHUNK_BUFFER_SIZE 10 - -#define NEXT_PARSE_STATE(parseState) ((parseState + 1) % PARSE_STATE_COUNT) - - -bool parseLedStripConfig(uint8_t ledIndex, const char *config) +// get specialColor by index +static hsvColor_t* getSC(ledSpecialColorIds_e index) { - char chunk[CHUNK_BUFFER_SIZE]; - uint8_t chunkIndex; - uint8_t val; + return &masterConfig.colors[masterConfig.specialColors.color[index]]; +} - uint8_t parseState = X_COORDINATE; - bool ok = true; +static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' }; +static const char baseFunctionCodes[LED_BASEFUNCTION_COUNT] = { 'C', 'F', 'A', 'L', 'S', 'G', 'R' }; +static const char overlayCodes[LED_OVERLAY_COUNT] = { 'T', 'O', 'B', 'N', 'I', 'W' }; - if (ledIndex >= MAX_LED_STRIP_LENGTH) { - return !ok; - } +#define CHUNK_BUFFER_SIZE 11 - ledConfig_t *ledConfig = &ledConfigs[ledIndex]; +bool parseLedStripConfig(int ledIndex, const char *config) +{ + if (ledIndex >= LED_MAX_STRIP_LENGTH) + return false; + + enum parseState_e { + X_COORDINATE, + Y_COORDINATE, + DIRECTIONS, + FUNCTIONS, + RING_COLORS, + PARSE_STATE_COUNT + }; + static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0'}; + + ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; memset(ledConfig, 0, sizeof(ledConfig_t)); - while (ok) { + int x = 0, y = 0, color = 0; // initialize to prevent warnings + int baseFunction = 0; + int overlay_flags = 0; + int direction_flags = 0; - char chunkSeparator = chunkSeparators[parseState]; - - memset(&chunk, 0, sizeof(chunk)); - chunkIndex = 0; - - while (*config && chunkIndex < CHUNK_BUFFER_SIZE && *config != chunkSeparator) { - chunk[chunkIndex++] = *config++; + for (enum parseState_e parseState = 0; parseState < PARSE_STATE_COUNT; parseState++) { + char chunk[CHUNK_BUFFER_SIZE]; + { + char chunkSeparator = chunkSeparators[parseState]; + int chunkIndex = 0; + while (*config && *config != chunkSeparator && chunkIndex < (CHUNK_BUFFER_SIZE - 1)) { + chunk[chunkIndex++] = *config++; + } + chunk[chunkIndex++] = 0; // zero-terminate chunk + if (*config != chunkSeparator) { + return false; + } + config++; // skip separator } - - if (*config++ != chunkSeparator) { - ok = false; - break; - } - - switch((parseState_e)parseState) { + switch (parseState) { case X_COORDINATE: - val = atoi(chunk); - ledConfig->xy |= CALCULATE_LED_X(val); + x = atoi(chunk); break; case Y_COORDINATE: - val = atoi(chunk); - ledConfig->xy |= CALCULATE_LED_Y(val); + y = atoi(chunk); break; case DIRECTIONS: - for (chunkIndex = 0; chunk[chunkIndex] && chunkIndex < CHUNK_BUFFER_SIZE; chunkIndex++) { - for (uint8_t mappingIndex = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) { - if (directionCodes[mappingIndex] == chunk[chunkIndex]) { - ledConfig->flags |= directionMappings[mappingIndex]; + for (char *ch = chunk; *ch; ch++) { + for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) { + if (directionCodes[dir] == *ch) { + direction_flags |= LED_FLAG_DIRECTION(dir); break; } } } break; case FUNCTIONS: - for (chunkIndex = 0; chunk[chunkIndex] && chunkIndex < CHUNK_BUFFER_SIZE; chunkIndex++) { - for (uint8_t mappingIndex = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) { - if (functionCodes[mappingIndex] == chunk[chunkIndex]) { - ledConfig->flags |= functionMappings[mappingIndex]; + for (char *ch = chunk; *ch; ch++) { + for (ledBaseFunctionId_e fn = 0; fn < LED_BASEFUNCTION_COUNT; fn++) { + if (baseFunctionCodes[fn] == *ch) { + baseFunction = fn; + break; + } + } + + for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) { + if (overlayCodes[ol] == *ch) { + overlay_flags |= LED_FLAG_OVERLAY(ol); break; } } } break; case RING_COLORS: - if (atoi(chunk) < CONFIGURABLE_COLOR_COUNT) { - ledConfig->color = atoi(chunk); - } else { - ledConfig->color = 0; - } + color = atoi(chunk); + if (color >= LED_CONFIGURABLE_COLOR_COUNT) + color = 0; break; - default : - break; - } - - parseState++; - if (parseState >= PARSE_STATE_COUNT) { - break; + case PARSE_STATE_COUNT:; // prevent warning } } - if (!ok) { - memset(ledConfig, 0, sizeof(ledConfig_t)); - } + *ledConfig = DEFINE_LED(x, y, color, direction_flags, baseFunction, overlay_flags, 0); reevaluateLedConfig(); - return ok; + return true; } -void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize) +void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize) { - char functions[FUNCTION_COUNT]; - char directions[DIRECTION_COUNT]; - uint8_t index; - uint8_t mappingIndex; + char directions[LED_DIRECTION_COUNT + 1]; + char baseFunctionOverlays[LED_OVERLAY_COUNT + 2]; - ledConfig_t *ledConfig = &ledConfigs[ledIndex]; + ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; memset(ledConfigBuffer, 0, bufferSize); - memset(&functions, 0, sizeof(functions)); - memset(&directions, 0, sizeof(directions)); - for (mappingIndex = 0, index = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) { - if (ledConfig->flags & functionMappings[mappingIndex]) { - functions[index++] = functionCodes[mappingIndex]; + char *dptr = directions; + for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) { + if (ledGetDirectionBit(ledConfig, dir)) { + *dptr++ = directionCodes[dir]; } } + *dptr = 0; - for (mappingIndex = 0, index = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) { - if (ledConfig->flags & directionMappings[mappingIndex]) { - directions[index++] = directionCodes[mappingIndex]; + char *fptr = baseFunctionOverlays; + *fptr++ = baseFunctionCodes[ledGetFunction(ledConfig)]; + + for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) { + if (ledGetOverlayBit(ledConfig, ol)) { + *fptr++ = overlayCodes[ol]; } } + *fptr = 0; - sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions, ledConfig->color); + // TODO - check buffer length + sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig)); } -void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const modeColorIndexes_t *modeColors) -{ - // apply up/down colors regardless of quadrant. - if ((ledConfig->flags & LED_DIRECTION_UP)) { - setLedHsv(ledIndex, &colors[modeColors->up]); - } - - if ((ledConfig->flags & LED_DIRECTION_DOWN)) { - setLedHsv(ledIndex, &colors[modeColors->down]); - } - - // override with n/e/s/w colors to each n/s e/w half - bail at first match. - if ((ledConfig->flags & LED_DIRECTION_WEST) && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, &colors[modeColors->west]); - } - - if ((ledConfig->flags & LED_DIRECTION_EAST) && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, &colors[modeColors->east]); - } - - if ((ledConfig->flags & LED_DIRECTION_NORTH) && GET_LED_Y(ledConfig) <= highestYValueForNorth) { - setLedHsv(ledIndex, &colors[modeColors->north]); - } - - if ((ledConfig->flags & LED_DIRECTION_SOUTH) && GET_LED_Y(ledConfig) >= lowestYValueForSouth) { - setLedHsv(ledIndex, &colors[modeColors->south]); - } - -} typedef enum { - QUADRANT_NORTH_EAST = 1, - QUADRANT_SOUTH_EAST, - QUADRANT_SOUTH_WEST, - QUADRANT_NORTH_WEST + // the ordering is important, see below how NSEW is mapped to NE/SE/NW/SW + QUADRANT_NORTH = 1 << 0, + QUADRANT_SOUTH = 1 << 1, + QUADRANT_EAST = 1 << 2, + QUADRANT_WEST = 1 << 3, + QUADRANT_NORTH_EAST = 1 << 4, + QUADRANT_SOUTH_EAST = 1 << 5, + QUADRANT_NORTH_WEST = 1 << 6, + QUADRANT_SOUTH_WEST = 1 << 7, + QUADRANT_NONE = 1 << 8, + QUADRANT_NOTDIAG = 1 << 9, // not in NE/SE/NW/SW + // values for test + QUADRANT_ANY = QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST | QUADRANT_NONE, } quadrant_e; -void applyQuadrantColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const quadrant_e quadrant, const hsvColor_t *color) +static quadrant_e getLedQuadrant(const int ledIndex) { - switch (quadrant) { - case QUADRANT_NORTH_EAST: - if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, color); - } - return; + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - case QUADRANT_SOUTH_EAST: - if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, color); - } - return; + int x = ledGetX(ledConfig); + int y = ledGetY(ledConfig); - case QUADRANT_SOUTH_WEST: - if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, color); - } - return; + int quad = 0; + if (y <= highestYValueForNorth) + quad |= QUADRANT_NORTH; + else if (y >= lowestYValueForSouth) + quad |= QUADRANT_SOUTH; + if (x >= lowestXValueForEast) + quad |= QUADRANT_EAST; + else if (x <= highestXValueForWest) + quad |= QUADRANT_WEST; + + if ((quad & (QUADRANT_NORTH | QUADRANT_SOUTH)) + && (quad & (QUADRANT_EAST | QUADRANT_WEST)) ) { // is led in one of NE/SE/NW/SW? + quad |= 1 << (4 + ((quad & QUADRANT_SOUTH) ? 1 : 0) + ((quad & QUADRANT_WEST) ? 2 : 0)); + } else { + quad |= QUADRANT_NOTDIAG; + } + + if ((quad & (QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST)) == 0) + quad |= QUADRANT_NONE; + + return quad; +} + +static const struct { + uint8_t dir; // ledDirectionId_e + uint16_t quadrantMask; // quadrant_e +} directionQuadrantMap[] = { + {LED_DIRECTION_SOUTH, QUADRANT_SOUTH}, + {LED_DIRECTION_NORTH, QUADRANT_NORTH}, + {LED_DIRECTION_EAST, QUADRANT_EAST}, + {LED_DIRECTION_WEST, QUADRANT_WEST}, + {LED_DIRECTION_DOWN, QUADRANT_ANY}, + {LED_DIRECTION_UP, QUADRANT_ANY}, +}; + +static hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIndexes_t *modeColors) +{ + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + + quadrant_e quad = getLedQuadrant(ledIndex); + for (unsigned i = 0; i < ARRAYLEN(directionQuadrantMap); i++) { + ledDirectionId_e dir = directionQuadrantMap[i].dir; + quadrant_e quadMask = directionQuadrantMap[i].quadrantMask; + + if (ledGetDirectionBit(ledConfig, dir) && (quad & quadMask)) + return &masterConfig.colors[modeColors->color[dir]]; + } + return NULL; +} + + +// map flight mode to led mode, in order of priority +// flightMode == 0 is always active +static const struct { + uint16_t flightMode; + uint8_t ledMode; +} flightModeToLed[] = { + {HEADFREE_MODE, LED_MODE_HEADFREE}, +#ifdef MAG + {MAG_MODE, LED_MODE_MAG}, +#endif +#ifdef BARO + {BARO_MODE, LED_MODE_BARO}, +#endif + {HORIZON_MODE, LED_MODE_HORIZON}, + {ANGLE_MODE, LED_MODE_ANGLE}, + {0, LED_MODE_ORIENTATION}, +}; + +static void applyLedFixedLayers() +{ + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND); + + int fn = ledGetFunction(ledConfig); + int hOffset = HSV_HUE_MAX; + + switch (fn) { + case LED_FUNCTION_COLOR: + color = *&masterConfig.colors[ledGetColor(ledConfig)]; + break; + + case LED_FUNCTION_FLIGHT_MODE: + for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) + if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { + color = *getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]); + break; // stop on first match + } + break; + + case LED_FUNCTION_ARM_STATE: + color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED); + break; + + case LED_FUNCTION_BATTERY: + color = HSV(RED); + hOffset += scaleRange(calculateBatteryCapacityRemainingPercentage(), 0, 100, -30, 120); + break; + + case LED_FUNCTION_RSSI: + color = HSV(RED); + hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120); + break; + + default: + break; + } + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { + hOffset += ((scaledThrottle - 10) * 4) / 3; + } + + color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1); + + setLedHsv(ledIndex, &color); - case QUADRANT_NORTH_WEST: - if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, color); - } - return; } } -void applyLedModeLayer(void) +static void applyLedHsv(uint32_t mask, const hsvColor_t *color) { - const ledConfig_t *ledConfig; - - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (!(ledConfig->flags & LED_FUNCTION_THRUST_RING)) { - if (ledConfig->flags & LED_FUNCTION_COLOR) { - setLedHsv(ledIndex, &colors[ledConfig->color]); - } else { - setLedHsv(ledIndex, &hsv_black); - } - } - - if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) { - if (ledConfig->flags & LED_FUNCTION_ARM_STATE) { - if (!ARMING_FLAG(ARMED)) { - setLedHsv(ledIndex, &hsv_green); - } else { - setLedHsv(ledIndex, &hsv_blue); - } - } - continue; - } - - applyDirectionalModeColor(ledIndex, ledConfig, &orientationModeColors); - - if (FLIGHT_MODE(HEADFREE_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &headfreeModeColors); -#ifdef MAG - } else if (FLIGHT_MODE(MAG_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &magModeColors); -#endif -#ifdef BARO - } else if (FLIGHT_MODE(BARO_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &baroModeColors); -#endif - } else if (FLIGHT_MODE(HORIZON_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &horizonModeColors); - } else if (FLIGHT_MODE(ANGLE_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &angleModeColors); - } + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if ((*ledConfig & mask) == mask) + setLedHsv(ledIndex, color); } } typedef enum { - WARNING_FLAG_NONE = 0, - WARNING_FLAG_LOW_BATTERY = (1 << 0), - WARNING_FLAG_FAILSAFE = (1 << 1), - WARNING_FLAG_ARMING_DISABLED = (1 << 2) + WARNING_ARMING_DISABLED, + WARNING_LOW_BATTERY, + WARNING_FAILSAFE, } warningFlags_e; -static uint8_t warningFlags = WARNING_FLAG_NONE; -void applyLedWarningLayer(uint8_t updateNow) +static void applyLedWarningLayer(bool updateNow, uint32_t *timer) { - const ledConfig_t *ledConfig; - uint8_t ledIndex; static uint8_t warningFlashCounter = 0; + static uint8_t warningFlags = 0; // non-zero during blinks - if (updateNow && warningFlashCounter == 0) { - warningFlags = WARNING_FLAG_NONE; - if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) { - warningFlags |= WARNING_FLAG_LOW_BATTERY; - } - if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) { - warningFlags |= WARNING_FLAG_FAILSAFE; - } - if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) { - warningFlags |= WARNING_FLAG_ARMING_DISABLED; - } - } - - if (warningFlags || warningFlashCounter > 0) { - const hsvColor_t *warningColor = &hsv_black; - - if ((warningFlashCounter & 1) == 0) { - if (warningFlashCounter < 4 && (warningFlags & WARNING_FLAG_ARMING_DISABLED)) { - warningColor = &hsv_green; - } - if (warningFlashCounter >= 4 && warningFlashCounter < 12 && (warningFlags & WARNING_FLAG_LOW_BATTERY)) { - warningColor = &hsv_red; - } - if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) { - warningColor = &hsv_yellow; - } - } else { - if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) { - warningColor = &hsv_blue; - } - } - - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (!(ledConfig->flags & LED_FUNCTION_WARNING)) { - continue; - } - setLedHsv(ledIndex, warningColor); - } - } - - if (updateNow && (warningFlags || warningFlashCounter)) { + if (updateNow) { + // keep counter running, so it stays in sync with blink warningFlashCounter++; - if (warningFlashCounter == 20) { - warningFlashCounter = 0; + warningFlashCounter &= 0xF; + + if (warningFlashCounter == 0) { // update when old flags was processed + warningFlags = 0; + if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) + warningFlags |= 1 << WARNING_LOW_BATTERY; + if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) + warningFlags |= 1 << WARNING_FAILSAFE; + if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) + warningFlags |= 1 << WARNING_ARMING_DISABLED; } + *timer += LED_STRIP_HZ(10); + } + + if (warningFlags) { + const hsvColor_t *warningColor = NULL; + + bool colorOn = (warningFlashCounter % 2) == 0; // w_w_ + warningFlags_e warningId = warningFlashCounter / 4; + if (warningFlags & (1 << warningId)) { + switch (warningId) { + case WARNING_ARMING_DISABLED: + warningColor = colorOn ? &HSV(GREEN) : NULL; + break; + case WARNING_LOW_BATTERY: + warningColor = colorOn ? &HSV(RED) : NULL; + break; + case WARNING_FAILSAFE: + warningColor = colorOn ? &HSV(YELLOW) : &HSV(BLUE); + break; + default:; + } + } + if (warningColor) + applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor); } } +static void applyLedBatteryLayer(bool updateNow, uint32_t *timer) +{ + static bool flash = false; + + int state; + int timeOffset = 1; + + if (updateNow) { + state = getBatteryState(); + + switch (state) { + case BATTERY_OK: + flash = false; + timeOffset = 1; + break; + case BATTERY_WARNING: + timeOffset = 2; + break; + default: + timeOffset = 8; + break; + } + flash = !flash; + } + + *timer += LED_STRIP_HZ(timeOffset); + + if (!flash) { + hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); + applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_BATTERY), bgc); + } +} + +static void applyLedRssiLayer(bool updateNow, uint32_t *timer) +{ + static bool flash = false; + + int state; + int timeOffset = 0; + + if (updateNow) { + state = (rssi * 100) / 1023; + + if (state > 50) { + flash = false; + timeOffset = 1; + } else if (state > 20) { + timeOffset = 2; + } else { + timeOffset = 8; + } + flash = !flash; + } + + + *timer += LED_STRIP_HZ(timeOffset); + + if (!flash) { + hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); + applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_RSSI), bgc); + } +} + +#ifdef GPS +static void applyLedGpsLayer(bool updateNow, uint32_t *timer) +{ + static uint8_t gpsFlashCounter = 0; + static uint8_t gpsPauseCounter = 0; + const uint8_t blinkPauseLength = 4; + + if (updateNow) { + if (gpsPauseCounter > 0) { + gpsPauseCounter--; + } else if (gpsFlashCounter >= GPS_numSat) { + gpsFlashCounter = 0; + gpsPauseCounter = blinkPauseLength; + } else { + gpsFlashCounter++; + gpsPauseCounter = 1; + } + *timer += LED_STRIP_HZ(2.5); + } + + const hsvColor_t *gpsColor; + + if (GPS_numSat == 0 || !sensors(SENSOR_GPS)) { + gpsColor = getSC(LED_SCOLOR_GPSNOSATS); + } else { + bool colorOn = gpsPauseCounter == 0; // each interval starts with pause + if (STATE(GPS_FIX)) { + gpsColor = colorOn ? getSC(LED_SCOLOR_GPSLOCKED) : getSC(LED_SCOLOR_BACKGROUND); + } else { + gpsColor = colorOn ? getSC(LED_SCOLOR_GPSNOLOCK) : getSC(LED_SCOLOR_GPSNOSATS); + } + } + + applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_GPS), gpsColor); +} + +#endif + + #define INDICATOR_DEADBAND 25 -void applyLedIndicatorLayer(uint8_t indicatorFlashState) +static void applyLedIndicatorLayer(bool updateNow, uint32_t *timer) { - const ledConfig_t *ledConfig; - static const hsvColor_t *flashColor; + static bool flash = 0; - if (!rxIsReceivingSignal()) { + if (updateNow) { + if (rxIsReceivingSignal()) { + // calculate update frequency + int scale = MAX(ABS(rcCommand[ROLL]), ABS(rcCommand[PITCH])); // 0 - 500 + scale += (50 - INDICATOR_DEADBAND); // start increasing frequency right after deadband + *timer += LED_STRIP_HZ(5) * 50 / MAX(50, scale); // 5 - 50Hz update, 2.5 - 25Hz blink + + flash = !flash; + } else { + *timer += LED_STRIP_HZ(5); // try again soon + } + } + + if (!flash) return; + + const hsvColor_t *flashColor = &HSV(ORANGE); // TODO - use user color? + + quadrant_e quadrants = 0; + if (rcCommand[ROLL] > INDICATOR_DEADBAND) { + quadrants |= QUADRANT_NORTH_EAST | QUADRANT_SOUTH_EAST; + } else if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { + quadrants |= QUADRANT_NORTH_WEST | QUADRANT_SOUTH_WEST; + } + if (rcCommand[PITCH] > INDICATOR_DEADBAND) { + quadrants |= QUADRANT_NORTH_EAST | QUADRANT_NORTH_WEST; + } else if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { + quadrants |= QUADRANT_SOUTH_EAST | QUADRANT_SOUTH_WEST; } - if (indicatorFlashState == 0) { - flashColor = &hsv_orange; - } else { - flashColor = &hsv_black; - } - - - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (!(ledConfig->flags & LED_FUNCTION_INDICATOR)) { - continue; + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_INDICATOR)) { + if (getLedQuadrant(ledIndex) & quadrants) + setLedHsv(ledIndex, flashColor); } - - if (rcCommand[ROLL] > INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); - } - - if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); - } - - if (rcCommand[PITCH] > INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); - } - - if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); - } - } -} - -void applyLedThrottleLayer() -{ - const ledConfig_t *ledConfig; - hsvColor_t color; - - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - ledConfig = &ledConfigs[ledIndex]; - if (!(ledConfig->flags & LED_FUNCTION_THROTTLE)) { - continue; - } - - getLedHsv(ledIndex, &color); - - int scaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, -60, +60); - scaled += HSV_HUE_MAX; - color.h = (color.h + scaled) % HSV_HUE_MAX; - setLedHsv(ledIndex, &color); } } #define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off -#define ROTATION_SEQUENCE_LED_WIDTH 2 +#define ROTATION_SEQUENCE_LED_WIDTH 2 // 2 on -#define RING_PATTERN_NOT_CALCULATED 255 - -void applyLedThrustRingLayer(void) +static void updateLedRingCounts(void) { - const ledConfig_t *ledConfig; - hsvColor_t ringColor; - uint8_t ledIndex; - - // initialised to special value instead of using more memory for a flag. - static uint8_t rotationSeqLedCount = RING_PATTERN_NOT_CALCULATED; - static uint8_t rotationPhase = ROTATION_SEQUENCE_LED_COUNT; - bool nextLedOn = false; - - uint8_t ledRingIndex = 0; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if ((ledConfig->flags & LED_FUNCTION_THRUST_RING) == 0) { - continue; + int seqLen; + // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds + if ((ledCounts.ring % ROTATION_SEQUENCE_LED_COUNT) == 0) { + seqLen = ROTATION_SEQUENCE_LED_COUNT; + } else { + seqLen = ledCounts.ring; + // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds + // TODO - improve partitioning (15 leds -> 3x5) + while ((seqLen > ROTATION_SEQUENCE_LED_COUNT) && ((seqLen % 2) == 0)) { + seqLen /= 2; } + } + ledCounts.ringSeqLen = seqLen; +} - bool applyColor = false; - if (ARMING_FLAG(ARMED)) { - if ((ledRingIndex + rotationPhase) % rotationSeqLedCount < ROTATION_SEQUENCE_LED_WIDTH) { - applyColor = true; - } - } else { - if (nextLedOn == false) { - applyColor = true; - } - nextLedOn = !nextLedOn; - } +static void applyLedThrustRingLayer(bool updateNow, uint32_t *timer) +{ + static uint8_t rotationPhase; + int ledRingIndex = 0; - if (applyColor) { - ringColor = colors[ledConfig->color]; - } else { - ringColor = hsv_black; - } + if (updateNow) { + rotationPhase = rotationPhase > 0 ? rotationPhase - 1 : ledCounts.ringSeqLen - 1; - setLedHsv(ledIndex, &ringColor); - - ledRingIndex++; + int scale = scaledThrottle; // ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + *timer += LED_STRIP_HZ(5) * 10 / scale; // 5 - 50Hz update rate } - uint8_t ledRingLedCount = ledRingIndex; - if (rotationSeqLedCount == RING_PATTERN_NOT_CALCULATED) { - // update ring pattern according to total number of ring leds found + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) { - rotationSeqLedCount = ledRingLedCount; - - // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds - if ((ledRingLedCount % ROTATION_SEQUENCE_LED_COUNT) == 0) { - rotationSeqLedCount = ROTATION_SEQUENCE_LED_COUNT; - } else { - // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds - while ((rotationSeqLedCount > ROTATION_SEQUENCE_LED_COUNT) && ((rotationSeqLedCount % 2) == 0)) { - rotationSeqLedCount >>= 1; + bool applyColor; + if (ARMING_FLAG(ARMED)) { + applyColor = (ledRingIndex + rotationPhase) % ledCounts.ringSeqLen < ROTATION_SEQUENCE_LED_WIDTH; + } else { + applyColor = !(ledRingIndex % 2); // alternating pattern } + + if (applyColor) { + const hsvColor_t *ringColor = &masterConfig.colors[ledGetColor(ledConfig)]; + setLedHsv(ledIndex, ringColor); + } + + ledRingIndex++; } - - // trigger start over - rotationPhase = 1; - } - - rotationPhase--; - if (rotationPhase == 0) { - rotationPhase = rotationSeqLedCount; } } +typedef struct larsonParameters_s { + uint8_t currentBrightness; + int8_t currentIndex; + int8_t direction; +} larsonParameters_t; + +static int brightnessForLarsonIndex(larsonParameters_t *larsonParameters, uint8_t larsonIndex) +{ + int offset = larsonIndex - larsonParameters->currentIndex; + static const int larsonLowValue = 8; + + if (ABS(offset) > 1) + return (larsonLowValue); + + if (offset == 0) + return (larsonParameters->currentBrightness); + + if (larsonParameters->direction == offset) { + return (larsonParameters->currentBrightness - 127); + } + + return (255 - larsonParameters->currentBrightness); + +} + +static void larsonScannerNextStep(larsonParameters_t *larsonParameters, int delta) +{ + if (larsonParameters->currentBrightness > (255 - delta)) { + larsonParameters->currentBrightness = 127; + if (larsonParameters->currentIndex >= ledCounts.larson || larsonParameters->currentIndex < 0) { + larsonParameters->direction = -larsonParameters->direction; + } + larsonParameters->currentIndex += larsonParameters->direction; + } else { + larsonParameters->currentBrightness += delta; + } +} + +static void applyLarsonScannerLayer(bool updateNow, uint32_t *timer) +{ + static larsonParameters_t larsonParameters = { 0, 0, 1 }; + + if (updateNow) { + larsonScannerNextStep(&larsonParameters, 15); + *timer += LED_STRIP_HZ(60); + } + + int scannerLedIndex = 0; + for (unsigned i = 0; i < ledCounts.count; i++) { + + const ledConfig_t *ledConfig = &ledConfigs[i]; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) { + hsvColor_t ledColor; + getLedHsv(i, &ledColor); + ledColor.v = brightnessForLarsonIndex(&larsonParameters, scannerLedIndex); + setLedHsv(i, &ledColor); + scannerLedIndex++; + } + } +} + +// blink twice, then wait ; either always or just when landing +static void applyLedBlinkLayer(bool updateNow, uint32_t *timer) +{ + const uint16_t blinkPattern = 0x8005; // 0b1000000000000101; + static uint16_t blinkMask; + + if (updateNow) { + blinkMask = blinkMask >> 1; + if (blinkMask <= 1) + blinkMask = blinkPattern; + + *timer += LED_STRIP_HZ(10); + } + + bool ledOn = (blinkMask & 1); // b_b_____... + if (!ledOn) { + for (int i = 0; i < ledCounts.count; ++i) { + const ledConfig_t *ledConfig = &ledConfigs[i]; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_BLINK) || + (ledGetOverlayBit(ledConfig, LED_OVERLAY_LANDING_FLASH) && scaledThrottle < 55 && scaledThrottle > 10)) { + setLedHsv(i, getSC(LED_SCOLOR_BLINKBACKGROUND)); + } + } + } +} + + #ifdef USE_LED_ANIMATION -static uint8_t previousRow; -static uint8_t currentRow; -static uint8_t nextRow; -void updateLedAnimationState(void) +static void applyLedAnimationLayer(bool updateNow, uint32_t *timer) { static uint8_t frameCounter = 0; - - uint8_t animationFrames = ledGridHeight; - - previousRow = (frameCounter + animationFrames - 1) % animationFrames; - currentRow = frameCounter; - nextRow = (frameCounter + 1) % animationFrames; - - frameCounter = (frameCounter + 1) % animationFrames; -} - -static void applyLedAnimationLayer(void) -{ - const ledConfig_t *ledConfig; - - if (ARMING_FLAG(ARMED)) { - return; + const int animationFrames = ledGridHeight; + if(updateNow) { + frameCounter = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; + *timer += LED_STRIP_HZ(20); } - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { + if (ARMING_FLAG(ARMED)) + return; - ledConfig = &ledConfigs[ledIndex]; + int previousRow = frameCounter > 0 ? frameCounter - 1 : animationFrames - 1; + int currentRow = frameCounter; + int nextRow = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; - if (GET_LED_Y(ledConfig) == previousRow) { - setLedHsv(ledIndex, &hsv_white); + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + + if (ledGetY(ledConfig) == previousRow) { + setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION)); scaleLedValue(ledIndex, 50); - - } else if (GET_LED_Y(ledConfig) == currentRow) { - setLedHsv(ledIndex, &hsv_white); - } else if (GET_LED_Y(ledConfig) == nextRow) { + } else if (ledGetY(ledConfig) == currentRow) { + setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION)); + } else if (ledGetY(ledConfig) == nextRow) { scaleLedValue(ledIndex, 50); } } } #endif +typedef enum { + timBlink, + timLarson, + timBattery, + timRssi, +#ifdef GPS + timGps, +#endif + timWarning, + timIndicator, +#ifdef USE_LED_ANIMATION + timAnimation, +#endif + timRing, + timTimerCount +} timId_e; + +static uint32_t timerVal[timTimerCount]; + + +// function to apply layer. +// function must replan self using timer pointer +// when updateNow is true (timer triggered), state must be updated first, +// before calculating led state. Otherwise update started by different trigger +// may modify LED state. +typedef void applyLayerFn_timed(bool updateNow, uint32_t *timer); + + +static applyLayerFn_timed* layerTable[] = { + [timBlink] = &applyLedBlinkLayer, + [timLarson] = &applyLarsonScannerLayer, + [timBattery] = &applyLedBatteryLayer, + [timRssi] = &applyLedRssiLayer, +#ifdef GPS + [timGps] = &applyLedGpsLayer, +#endif + [timWarning] = &applyLedWarningLayer, + [timIndicator] = &applyLedIndicatorLayer, +#ifdef USE_LED_ANIMATION + [timAnimation] = &applyLedAnimationLayer, +#endif + [timRing] = &applyLedThrustRingLayer +}; + void updateLedStrip(void) { - if (!(ledStripInitialised && isWS2811LedStripReady())) { return; } - if (IS_RC_MODE_ACTIVE(BOXLEDLOW) && !(masterConfig.ledstrip_visual_beeper && isBeeperOn())) { + if (IS_RC_MODE_ACTIVE(BOXLEDLOW)) { if (ledStripEnabled) { ledStripDisable(); ledStripEnabled = false; } - } else { - ledStripEnabled = true; - } - - if (!ledStripEnabled){ return; } - + ledStripEnabled = true; uint32_t now = micros(); - bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; - bool warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L; - bool rotationUpdateNow = (int32_t)(now - nextRotationUpdateAt) >= 0L; -#ifdef USE_LED_ANIMATION - bool animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L; -#endif - if (!( - indicatorFlashNow || - rotationUpdateNow || - warningFlashNow -#ifdef USE_LED_ANIMATION - || animationUpdateNow -#endif - )) { - return; - } - - static uint8_t indicatorFlashState = 0; - - // LAYER 1 - applyLedModeLayer(); - applyLedThrottleLayer(); - - // LAYER 2 - - if (warningFlashNow) { - nextWarningFlashAt = now + LED_STRIP_10HZ; - } - applyLedWarningLayer(warningFlashNow); - - // LAYER 3 - - if (indicatorFlashNow) { - - uint8_t rollScale = ABS(rcCommand[ROLL]) / 50; - uint8_t pitchScale = ABS(rcCommand[PITCH]) / 50; - uint8_t scale = MAX(rollScale, pitchScale); - nextIndicatorFlashAt = now + (LED_STRIP_5HZ / MAX(1, scale)); - - if (indicatorFlashState == 0) { - indicatorFlashState = 1; - } else { - indicatorFlashState = 0; + // test all led timers, setting corresponding bits + uint32_t timActive = 0; + for (timId_e timId = 0; timId < timTimerCount; timId++) { + // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value. + // max delay is limited to 5s + int32_t delta = cmp32(now, timerVal[timId]); + if (delta < 0 && delta > -LED_STRIP_MS(5000)) + continue; // not ready yet + timActive |= 1 << timId; + if (delta >= LED_STRIP_MS(100) || delta < 0) { + timerVal[timId] = now; } } - applyLedIndicatorLayer(indicatorFlashState); + if (!timActive) + return; // no change this update, keep old state -#ifdef USE_LED_ANIMATION - if (animationUpdateNow) { - nextAnimationUpdateAt = now + LED_STRIP_20HZ; - updateLedAnimationState(); + // apply all layers; triggered timed functions has to update timers + + scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + + applyLedFixedLayers(); + + for (timId_e timId = 0; timId < ARRAYLEN(layerTable); timId++) { + uint32_t *timer = &timerVal[timId]; + bool updateNow = timActive & (1 << timId); + (*layerTable[timId])(updateNow, timer); } - applyLedAnimationLayer(); -#endif - - if (rotationUpdateNow) { - - applyLedThrustRingLayer(); - - uint8_t animationSpeedScale = 1; - - if (ARMING_FLAG(ARMED)) { - animationSpeedScale = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 1, 10); - } - - nextRotationUpdateAt = now + LED_STRIP_5HZ/animationSpeedScale; - } - ws2811UpdateStrip(); } -bool parseColor(uint8_t index, const char *colorConfig) +bool parseColor(int index, const char *colorConfig) { const char *remainingCharacters = colorConfig; - hsvColor_t *color = &colors[index]; + hsvColor_t *color = &masterConfig.colors[index]; - bool ok = true; - - uint8_t componentIndex; - for (componentIndex = 0; ok && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) { - uint16_t val = atoi(remainingCharacters); + bool result = true; + static const uint16_t hsv_limit[HSV_COLOR_COMPONENT_COUNT] = { + [HSV_HUE] = HSV_HUE_MAX, + [HSV_SATURATION] = HSV_SATURATION_MAX, + [HSV_VALUE] = HSV_VALUE_MAX, + }; + for (int componentIndex = 0; result && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) { + int val = atoi(remainingCharacters); + if(val > hsv_limit[componentIndex]) { + result = false; + break; + } switch (componentIndex) { case HSV_HUE: - if (val > HSV_HUE_MAX) { - ok = false; - continue; - - } - colors[index].h = val; + color->h = val; break; case HSV_SATURATION: - if (val > HSV_SATURATION_MAX) { - ok = false; - continue; - } - colors[index].s = (uint8_t)val; + color->s = val; break; case HSV_VALUE: - if (val > HSV_VALUE_MAX) { - ok = false; - continue; - } - colors[index].v = (uint8_t)val; + color->v = val; break; } - remainingCharacters = strstr(remainingCharacters, ","); + remainingCharacters = strchr(remainingCharacters, ','); if (remainingCharacters) { - remainingCharacters++; + remainingCharacters++; // skip separator } else { - if (componentIndex < 2) { - ok = false; + if (componentIndex < HSV_COLOR_COMPONENT_COUNT - 1) { + result = false; } } } - if (!ok) { - memset(color, 0, sizeof(hsvColor_t)); + if (!result) { + memset(color, 0, sizeof(*color)); } - return ok; + return result; } -void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount) +/* + * Redefine a color in a mode. + * */ +bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) { - memset(colors, 0, colorCount * sizeof(hsvColor_t)); - for (uint8_t colorIndex = 0; colorIndex < colorCount && colorIndex < (sizeof(defaultColors) / sizeof(defaultColors[0])); colorIndex++) { - *colors++ = *defaultColors[colorIndex]; + // check color + if (colorIndex < 0 || colorIndex >= LED_CONFIGURABLE_COLOR_COUNT) + return false; + if (modeIndex < LED_MODE_COUNT) { // modeIndex_e is unsigned, so one-sided test is enough + if(modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT) + return false; + masterConfig.modeColors[modeIndex].color[modeColorIndex] = colorIndex; + } else if (modeIndex == LED_SPECIAL) { + if (modeColorIndex < 0 || modeColorIndex >= LED_SPECIAL_COLOR_COUNT) + return false; + masterConfig.specialColors.color[modeColorIndex] = colorIndex; + } else { + return false; + } + return true; +} + +/* +void pgResetFn_ledConfigs(ledConfig_t *instance) +{ + memcpy_fn(instance, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); +} + +void pgResetFn_colors(hsvColor_t *instance) +{ + // copy hsv colors as default + BUILD_BUG_ON(ARRAYLEN(*colors_arr()) < ARRAYLEN(hsv)); + + for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { + *instance++ = hsv[colorIndex]; } } +void pgResetFn_modeColors(modeColorIndexes_t *instance) +{ + memcpy_fn(instance, &defaultModeColors, sizeof(defaultModeColors)); +} + +void pgResetFn_specialColors(specialColorIndexes_t *instance) +{ + memcpy_fn(instance, &defaultSpecialColors, sizeof(defaultSpecialColors)); +} +*/ + void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) { - memset(ledConfigs, 0, MAX_LED_STRIP_LENGTH * sizeof(ledConfig_t)); + memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); reevaluateLedConfig(); } -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse) +void applyDefaultColors(hsvColor_t *colors) +{ + // copy hsv colors as default + memset(colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t)); + for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { + *colors++ = hsv[colorIndex]; + } +} + +void applyDefaultModeColors(modeColorIndexes_t *modeColors) +{ + memcpy_fn(modeColors, &defaultModeColors, sizeof(defaultModeColors)); +} + +void applyDefaultSpecialColors(specialColorIndexes_t *specialColors) +{ + memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors)); +} + + + +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse) { ledConfigs = ledConfigsToUse; colors = colorsToUse; + modeColors = modeColorsToUse; + specialColors = *specialColorsToUse; ledStripInitialised = false; } @@ -1115,8 +1268,8 @@ void ledStripEnable(void) static void ledStripDisable(void) { - setStripColor(&hsv_black); - - ws2811UpdateStrip(); + setStripColor(&HSV(BLACK)); + + ws2811UpdateStrip(); } #endif diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 18915229b7..14df5edbbd 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -17,81 +17,163 @@ #pragma once -#define MAX_LED_STRIP_LENGTH 32 -#define CONFIGURABLE_COLOR_COUNT 16 +#define LED_MAX_STRIP_LENGTH 32 +#define LED_CONFIGURABLE_COLOR_COUNT 16 +#define LED_MODE_COUNT 6 +#define LED_DIRECTION_COUNT 6 +#define LED_BASEFUNCTION_COUNT 7 +#define LED_OVERLAY_COUNT 6 +#define LED_SPECIAL_COLOR_COUNT 11 + +#define LED_POS_OFFSET 0 +#define LED_FUNCTION_OFFSET 8 +#define LED_OVERLAY_OFFSET 12 +#define LED_COLOR_OFFSET 18 +#define LED_DIRECTION_OFFSET 22 +#define LED_PARAMS_OFFSET 28 + +#define LED_POS_BITCNT 8 +#define LED_FUNCTION_BITCNT 4 +#define LED_OVERLAY_BITCNT 6 +#define LED_COLOR_BITCNT 4 +#define LED_DIRECTION_BITCNT 6 +#define LED_PARAMS_BITCNT 4 + +#define LED_FLAG_OVERLAY_MASK ((1 << LED_OVERLAY_BITCNT) - 1) +#define LED_FLAG_DIRECTION_MASK ((1 << LED_DIRECTION_BITCNT) - 1) + +#define LED_MOV_POS(pos) ((pos) << LED_POS_OFFSET) +#define LED_MOV_FUNCTION(func) ((func) << LED_FUNCTION_OFFSET) +#define LED_MOV_OVERLAY(overlay) ((overlay) << LED_OVERLAY_OFFSET) +#define LED_MOV_COLOR(colorId) ((colorId) << LED_COLOR_OFFSET) +#define LED_MOV_DIRECTION(direction) ((direction) << LED_DIRECTION_OFFSET) +#define LED_MOV_PARAMS(param) ((param) << LED_PARAMS_OFFSET) + +#define LED_BIT_MASK(len) ((1 << (len)) - 1) + +#define LED_POS_MASK LED_MOV_POS(((1 << LED_POS_BITCNT) - 1)) +#define LED_FUNCTION_MASK LED_MOV_FUNCTION(((1 << LED_FUNCTION_BITCNT) - 1)) +#define LED_OVERLAY_MASK LED_MOV_OVERLAY(LED_FLAG_OVERLAY_MASK) +#define LED_COLOR_MASK LED_MOV_COLOR(((1 << LED_COLOR_BITCNT) - 1)) +#define LED_DIRECTION_MASK LED_MOV_DIRECTION(LED_FLAG_DIRECTION_MASK) +#define LED_PARAMS_MASK LED_MOV_PARAMS(((1 << LED_PARAMS_BITCNT) - 1)) + +#define LED_FLAG_OVERLAY(id) (1 << (id)) +#define LED_FLAG_DIRECTION(id) (1 << (id)) #define LED_X_BIT_OFFSET 4 #define LED_Y_BIT_OFFSET 0 - -#define LED_XY_MASK (0x0F) - -#define GET_LED_X(ledConfig) ((ledConfig->xy >> LED_X_BIT_OFFSET) & LED_XY_MASK) -#define GET_LED_Y(ledConfig) ((ledConfig->xy >> LED_Y_BIT_OFFSET) & LED_XY_MASK) - -#define CALCULATE_LED_X(x) ((x & LED_XY_MASK) << LED_X_BIT_OFFSET) -#define CALCULATE_LED_Y(y) ((y & LED_XY_MASK) << LED_Y_BIT_OFFSET) - - -#define CALCULATE_LED_XY(x,y) (CALCULATE_LED_X(x) | CALCULATE_LED_Y(y)) +#define LED_XY_MASK 0x0F +#define CALCULATE_LED_XY(x, y) ((((x) & LED_XY_MASK) << LED_X_BIT_OFFSET) | (((y) & LED_XY_MASK) << LED_Y_BIT_OFFSET)) typedef enum { - LED_DISABLED = 0, - LED_DIRECTION_NORTH = (1 << 0), - LED_DIRECTION_EAST = (1 << 1), - LED_DIRECTION_SOUTH = (1 << 2), - LED_DIRECTION_WEST = (1 << 3), - LED_DIRECTION_UP = (1 << 4), - LED_DIRECTION_DOWN = (1 << 5), - LED_FUNCTION_INDICATOR = (1 << 6), - LED_FUNCTION_WARNING = (1 << 7), - LED_FUNCTION_FLIGHT_MODE = (1 << 8), - LED_FUNCTION_ARM_STATE = (1 << 9), - LED_FUNCTION_THROTTLE = (1 << 10), - LED_FUNCTION_THRUST_RING = (1 << 11), - LED_FUNCTION_COLOR = (1 << 12), -} ledFlag_e; + LED_MODE_ORIENTATION = 0, + LED_MODE_HEADFREE, + LED_MODE_HORIZON, + LED_MODE_ANGLE, + LED_MODE_MAG, + LED_MODE_BARO, + LED_SPECIAL +} ledModeIndex_e; -#define LED_DIRECTION_BIT_OFFSET 0 -#define LED_DIRECTION_MASK ( \ - LED_DIRECTION_NORTH | \ - LED_DIRECTION_EAST | \ - LED_DIRECTION_SOUTH | \ - LED_DIRECTION_WEST | \ - LED_DIRECTION_UP | \ - LED_DIRECTION_DOWN \ -) -#define LED_FUNCTION_BIT_OFFSET 6 -#define LED_FUNCTION_MASK ( \ - LED_FUNCTION_INDICATOR | \ - LED_FUNCTION_WARNING | \ - LED_FUNCTION_FLIGHT_MODE | \ - LED_FUNCTION_ARM_STATE | \ - LED_FUNCTION_THROTTLE | \ - LED_FUNCTION_THRUST_RING | \ - LED_FUNCTION_COLOR \ -) +typedef enum { + LED_SCOLOR_DISARMED = 0, + LED_SCOLOR_ARMED, + LED_SCOLOR_ANIMATION, + LED_SCOLOR_BACKGROUND, + LED_SCOLOR_BLINKBACKGROUND, + LED_SCOLOR_GPSNOSATS, + LED_SCOLOR_GPSNOLOCK, + LED_SCOLOR_GPSLOCKED +} ledSpecialColorIds_e; + +typedef enum { + LED_DIRECTION_NORTH = 0, + LED_DIRECTION_EAST, + LED_DIRECTION_SOUTH, + LED_DIRECTION_WEST, + LED_DIRECTION_UP, + LED_DIRECTION_DOWN +} ledDirectionId_e; + +typedef enum { + LED_FUNCTION_COLOR, + LED_FUNCTION_FLIGHT_MODE, + LED_FUNCTION_ARM_STATE, + LED_FUNCTION_BATTERY, + LED_FUNCTION_RSSI, + LED_FUNCTION_GPS, + LED_FUNCTION_THRUST_RING, +} ledBaseFunctionId_e; + +typedef enum { + LED_OVERLAY_THROTTLE, + LED_OVERLAY_LARSON_SCANNER, + LED_OVERLAY_BLINK, + LED_OVERLAY_LANDING_FLASH, + LED_OVERLAY_INDICATOR, + LED_OVERLAY_WARNING, +} ledOverlayId_e; + +typedef struct modeColorIndexes_s { + uint8_t color[LED_DIRECTION_COUNT]; +} modeColorIndexes_t; + +typedef struct specialColorIndexes_s { + uint8_t color[LED_SPECIAL_COLOR_COUNT]; +} specialColorIndexes_t; + +typedef uint32_t ledConfig_t; + +typedef struct ledCounts_s { + uint8_t count; + uint8_t ring; + uint8_t larson; + uint8_t ringSeqLen; +} ledCounts_t; -typedef struct ledConfig_s { - uint8_t xy; // see LED_X/Y_MASK defines - uint8_t color; // see colors (config_master) - uint16_t flags; // see ledFlag_e -} ledConfig_t; +ledConfig_t *ledConfigs; +hsvColor_t *colors; +modeColorIndexes_t *modeColors; +specialColorIndexes_t specialColors; -extern uint8_t ledCount; -extern uint8_t ledsInRingCount; +#define DEFINE_LED(x, y, col, dir, func, ol, params) (LED_MOV_POS(CALCULATE_LED_XY(x, y)) | LED_MOV_COLOR(col) | LED_MOV_DIRECTION(dir) | LED_MOV_FUNCTION(func) | LED_MOV_OVERLAY(ol) | LED_MOV_PARAMS(params)) +static inline uint8_t ledGetXY(const ledConfig_t *lcfg) { return ((*lcfg >> LED_POS_OFFSET) & LED_BIT_MASK(LED_POS_BITCNT)); } +static inline uint8_t ledGetX(const ledConfig_t *lcfg) { return ((*lcfg >> (LED_POS_OFFSET + LED_X_BIT_OFFSET)) & LED_XY_MASK); } +static inline uint8_t ledGetY(const ledConfig_t *lcfg) { return ((*lcfg >> (LED_POS_OFFSET + LED_Y_BIT_OFFSET)) & LED_XY_MASK); } +static inline uint8_t ledGetFunction(const ledConfig_t *lcfg) { return ((*lcfg >> LED_FUNCTION_OFFSET) & LED_BIT_MASK(LED_FUNCTION_BITCNT)); } +static inline uint8_t ledGetOverlay(const ledConfig_t *lcfg) { return ((*lcfg >> LED_OVERLAY_OFFSET) & LED_BIT_MASK(LED_OVERLAY_BITCNT)); } +static inline uint8_t ledGetColor(const ledConfig_t *lcfg) { return ((*lcfg >> LED_COLOR_OFFSET) & LED_BIT_MASK(LED_COLOR_BITCNT)); } +static inline uint8_t ledGetDirection(const ledConfig_t *lcfg) { return ((*lcfg >> LED_DIRECTION_OFFSET) & LED_BIT_MASK(LED_DIRECTION_BITCNT)); } +static inline uint8_t ledGetParams(const ledConfig_t *lcfg) { return ((*lcfg >> LED_PARAMS_OFFSET) & LED_BIT_MASK(LED_PARAMS_BITCNT)); } +static inline bool ledGetOverlayBit(const ledConfig_t *lcfg, int id) { return ((ledGetOverlay(lcfg) >> id) & 1); } +static inline bool ledGetDirectionBit(const ledConfig_t *lcfg, int id) { return ((ledGetDirection(lcfg) >> id) & 1); } +/* +PG_DECLARE_ARR(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs); +PG_DECLARE_ARR(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors); +PG_DECLARE_ARR(modeColorIndexes_t, LED_MODE_COUNT, modeColors); +PG_DECLARE(specialColorIndexes_t, specialColors); +*/ +bool parseColor(int index, const char *colorConfig); -bool parseLedStripConfig(uint8_t ledIndex, const char *config); +bool parseLedStripConfig(int ledIndex, const char *config); +void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize); +void reevalulateLedConfig(void); + +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); +void ledStripEnable(void); void updateLedStrip(void); -void updateLedRing(void); + +bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex); + +extern uint16_t rssi; // FIXME dependency on mw.c + void applyDefaultLedStripConfig(ledConfig_t *ledConfig); -void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize); +void applyDefaultColors(hsvColor_t *colors); +void applyDefaultModeColors(modeColorIndexes_t *modeColors); +void applyDefaultSpecialColors(specialColorIndexes_t *specialColors); -bool parseColor(uint8_t index, const char *colorConfig); -void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount); - -void ledStripEnable(void); -void reevaluateLedConfig(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index c0f3a8c417..cf29d81e08 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -155,6 +155,7 @@ static void cliMap(char *cmdline); #ifdef LED_STRIP static void cliLed(char *cmdline); static void cliColor(char *cmdline); +static void cliModeColor(char *cmdline); #endif #ifndef USE_QUAD_MIXER_ONLY @@ -263,6 +264,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux), #ifdef LED_STRIP CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), + CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor), #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), @@ -1399,20 +1401,20 @@ static void cliLed(char *cmdline) char ledConfigBuffer[20]; if (isEmpty(cmdline)) { - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { + for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); cliPrintf("led %u %s\r\n", i, ledConfigBuffer); } } else { ptr = cmdline; i = atoi(ptr); - if (i < MAX_LED_STRIP_LENGTH) { + if (i < LED_MAX_STRIP_LENGTH) { ptr = strchr(cmdline, ' '); if (!parseLedStripConfig(i, ++ptr)) { cliShowParseError(); } } else { - cliShowArgumentRangeError("index", 0, MAX_LED_STRIP_LENGTH - 1); + cliShowArgumentRangeError("index", 0, LED_MAX_STRIP_LENGTH - 1); } } } @@ -1423,7 +1425,7 @@ static void cliColor(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { cliPrintf("color %u %d,%u,%u\r\n", i, masterConfig.colors[i].h, @@ -1434,16 +1436,57 @@ static void cliColor(char *cmdline) } else { ptr = cmdline; i = atoi(ptr); - if (i < CONFIGURABLE_COLOR_COUNT) { + if (i < LED_CONFIGURABLE_COLOR_COUNT) { ptr = strchr(cmdline, ' '); if (!parseColor(i, ++ptr)) { cliShowParseError(); } } else { - cliShowArgumentRangeError("index", 0, CONFIGURABLE_COLOR_COUNT - 1); + cliShowArgumentRangeError("index", 0, LED_CONFIGURABLE_COLOR_COUNT - 1); } } } + +static void cliModeColor(char *cmdline) +{ + if (isEmpty(cmdline)) { + for (int i = 0; i < LED_MODE_COUNT; i++) { + for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + int colorIndex = modeColors[i].color[j]; + cliPrintf("mode_color %u %u %u\r\n", i, j, colorIndex); + } + } + + for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + int colorIndex = specialColors.color[j]; + cliPrintf("mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex); + } + } else { + enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; + int args[ARGS_COUNT]; + int argNo = 0; + char* ptr = strtok(cmdline, " "); + while (ptr && argNo < ARGS_COUNT) { + args[argNo++] = atoi(ptr); + ptr = strtok(NULL, " "); + } + + if (ptr != NULL || argNo != ARGS_COUNT) { + cliShowParseError(); + return; + } + + int modeIdx = args[MODE]; + int funIdx = args[FUNCTION]; + int color = args[COLOR]; + if(!setModeColor(modeIdx, funIdx, color)) { + cliShowParseError(); + return; + } + // values are validated + cliPrintf("mode_color %u %u %u\r\n", modeIdx, funIdx, color); + } +} #endif #ifdef USE_SERVOS @@ -2066,6 +2109,9 @@ static void cliDump(char *cmdline) cliPrint("\r\n\r\n# color\r\n"); cliColor(""); + + cliPrint("\r\n\r\n# mode_color\r\n"); + cliModeColor(""); #endif cliPrint("\r\n# aux\r\n"); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 80e4717fde..882b95dfa9 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1142,8 +1142,8 @@ static bool processOutCommand(uint8_t cmdMSP) #ifdef LED_STRIP case MSP_LED_COLORS: - headSerialReply(CONFIGURABLE_COLOR_COUNT * 4); - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + headSerialReply(LED_CONFIGURABLE_COLOR_COUNT * 4); + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; serialize16(color->h); serialize8(color->s); @@ -1152,14 +1152,27 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_LED_STRIP_CONFIG: - headSerialReply(MAX_LED_STRIP_LENGTH * 7); - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { + headSerialReply(LED_MAX_STRIP_LENGTH * 4); + for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - serialize16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET); - serialize16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET); - serialize8(GET_LED_X(ledConfig)); - serialize8(GET_LED_Y(ledConfig)); - serialize8(ledConfig->color); + serialize32(*ledConfig); + } + break; + + case MSP_LED_STRIP_MODECOLOR: + headSerialReply(((LED_MODE_COUNT * LED_DIRECTION_COUNT) + LED_SPECIAL_COLOR_COUNT) * 3); + for (int i = 0; i < LED_MODE_COUNT; i++) { + for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + serialize8(i); + serialize8(j); + serialize8(masterConfig.modeColors[i].color[j]); + } + } + + for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + serialize8(LED_MODE_COUNT); + serialize8(j); + serialize8(masterConfig.specialColors.color[j]); } break; #endif @@ -1774,7 +1787,7 @@ static bool processInCommand(void) #ifdef LED_STRIP case MSP_SET_LED_COLORS: - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; color->h = read16(); color->s = read8(); @@ -1785,29 +1798,24 @@ static bool processInCommand(void) case MSP_SET_LED_STRIP_CONFIG: { i = read8(); - if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != (1 + 7)) { + if (i >= LED_MAX_STRIP_LENGTH || currentPort->dataSize != (1 + 4)) { headSerialError(0); break; } ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - uint16_t mask; - // currently we're storing directions and functions in a uint16 (flags) - // the msp uses 2 x uint16_t to cater for future expansion - mask = read16(); - ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; + *ledConfig = read32(); + reevalulateLedConfig(); + } + break; - mask = read16(); - ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; + case MSP_SET_LED_STRIP_MODECOLOR: + { + ledModeIndex_e modeIdx = read8(); + int funIdx = read8(); + int color = read8(); - mask = read8(); - ledConfig->xy = CALCULATE_LED_X(mask); - - mask = read8(); - ledConfig->xy |= CALCULATE_LED_Y(mask); - - ledConfig->color = read8(); - - reevaluateLedConfig(); + if (!setModeColor(modeIdx, funIdx, color)) + return false; } break; #endif diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 1a63b2b20d..bd135f1180 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -242,6 +242,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_3D 124 //out message Settings needed for reversible ESCs #define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll #define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag +#define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed @@ -262,6 +263,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll #define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults #define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag +#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings // #define MSP_BIND 240 //in message no param // #define MSP_ALARMS 242 diff --git a/src/main/main.c b/src/main/main.c index a411117617..cdb67a0076 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -134,7 +134,7 @@ void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); void imuInit(void); void displayInit(rxConfig_t *intialRxConfig); -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); void spektrumBind(rxConfig_t *rxConfig); const sonarHardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor); void osdInit(void); @@ -544,7 +544,7 @@ void init(void) #endif #ifdef LED_STRIP - ledStripInit(masterConfig.ledConfigs, masterConfig.colors); + ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 59fa7ef9ae..3be1984939 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -224,7 +224,7 @@ fix12_t calculateVbatPidCompensation(void) { uint8_t calculateBatteryPercentage(void) { - return (((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount); + return constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100); } uint8_t calculateBatteryCapacityRemainingPercentage(void) From f8906956ad59fd823773eebeaee402da0c050791 Mon Sep 17 00:00:00 2001 From: gaelj Date: Sun, 26 Jun 2016 02:17:10 +0200 Subject: [PATCH 007/456] Fix freeze if LED 0 is color only --- src/main/io/ledstrip.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 866198857c..7f632b3547 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -349,7 +349,7 @@ STATIC_UNIT_TESTED void updateLedCount(void) for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - if (!(*ledConfig & (LED_POS_MASK | LED_FUNCTION_MASK | LED_OVERLAY_MASK))) + if (!(*ledConfig)) break; count++; @@ -608,7 +608,7 @@ static void applyLedFixedLayers() switch (fn) { case LED_FUNCTION_COLOR: - color = *&masterConfig.colors[ledGetColor(ledConfig)]; + color = masterConfig.colors[ledGetColor(ledConfig)]; break; case LED_FUNCTION_FLIGHT_MODE: From 59632f472f807181a21501a451f89c7e62671af4 Mon Sep 17 00:00:00 2001 From: gaelj Date: Sun, 26 Jun 2016 15:16:13 +0200 Subject: [PATCH 008/456] MSP to 20 + typo Set MSP to 20 Fix typo in reevaluateLedConfig --- src/main/io/ledstrip.h | 2 +- src/main/io/serial_msp.c | 2 +- src/main/io/serial_msp.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 14df5edbbd..39b3cad6d2 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -161,7 +161,7 @@ bool parseColor(int index, const char *colorConfig); bool parseLedStripConfig(int ledIndex, const char *config); void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize); -void reevalulateLedConfig(void); +void reevaluateLedConfig(void); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); void ledStripEnable(void); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 882b95dfa9..334607f632 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1804,7 +1804,7 @@ static bool processInCommand(void) } ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; *ledConfig = read32(); - reevalulateLedConfig(); + reevaluateLedConfig(); } break; diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index bd135f1180..7326a390b1 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 17 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 20 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 From 99110b2b8ba8eb0b8da7d900be4c9e7d3311bd74 Mon Sep 17 00:00:00 2001 From: gaelj Date: Wed, 29 Jun 2016 00:59:26 +0200 Subject: [PATCH 009/456] visual beeper got left out --- src/main/io/ledstrip.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 7f632b3547..4ab463bd1c 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -1083,7 +1083,7 @@ void updateLedStrip(void) return; } - if (IS_RC_MODE_ACTIVE(BOXLEDLOW)) { + if (IS_RC_MODE_ACTIVE(BOXLEDLOW) && !(masterConfig.ledstrip_visual_beeper && isBeeperOn())) { if (ledStripEnabled) { ledStripDisable(); ledStripEnabled = false; From 0083e9bd84d8b981e71ffffb9ab6b1297dda6669 Mon Sep 17 00:00:00 2001 From: gaelj Date: Sun, 26 Jun 2016 22:17:39 +0200 Subject: [PATCH 010/456] Move deadband3d_throttle from MSP_3D to MSP_RC_DEADBAND Only for MSP framing compatibility with CF. Does not include to change from global to profile. --- src/main/io/serial_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 80e4717fde..a74e152ad4 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1241,7 +1241,6 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(masterConfig.flight3DConfig.deadband3d_low); serialize16(masterConfig.flight3DConfig.deadband3d_high); serialize16(masterConfig.flight3DConfig.neutral3d); - serialize16(masterConfig.flight3DConfig.deadband3d_throttle); break; case MSP_RC_DEADBAND: @@ -1249,6 +1248,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.rcControlsConfig.deadband); serialize8(masterConfig.rcControlsConfig.yaw_deadband); serialize8(masterConfig.rcControlsConfig.alt_hold_deadband); + serialize16(masterConfig.flight3DConfig.deadband3d_throttle); break; case MSP_SENSOR_ALIGNMENT: headSerialReply(3); @@ -1504,13 +1504,13 @@ static bool processInCommand(void) masterConfig.flight3DConfig.deadband3d_low = read16(); masterConfig.flight3DConfig.deadband3d_high = read16(); masterConfig.flight3DConfig.neutral3d = read16(); - masterConfig.flight3DConfig.deadband3d_throttle = read16(); break; case MSP_SET_RC_DEADBAND: masterConfig.rcControlsConfig.deadband = read8(); masterConfig.rcControlsConfig.yaw_deadband = read8(); masterConfig.rcControlsConfig.alt_hold_deadband = read8(); + masterConfig.flight3DConfig.deadband3d_throttle = read16(); break; case MSP_SET_RESET_CURR_PID: From 1204c772966f02183a5585e7054f29e837db8c43 Mon Sep 17 00:00:00 2001 From: nathan Date: Wed, 13 Jul 2016 00:30:59 -0700 Subject: [PATCH 011/456] omnibus pinout updates, osd dma mapping --- src/main/target/OMNIBUS/target.c | 22 ++++--- src/main/target/OMNIBUS/target.h | 96 ++++++++++++++----------------- src/main/target/OMNIBUS/target.mk | 8 +-- 3 files changed, 55 insertions(+), 71 deletions(-) diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index a41891d696..977d623f85 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -85,20 +85,18 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent - // Used by SPI1, MAX7456 - //{ TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - //{ TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - - { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 - { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM5 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM6 - PA3 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - PA0 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA1 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM4 - PA3 // UART3 RX/TX - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM9 - PB10 - TIM2_CH3 / UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PB11 - TIM2_CH4 / UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PB11 - TIM2_CH4 / UART3_RX (AF7) + + // SDA / SCL + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - PB7 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM8 - PB6 // LED Strip Pad { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index bab30522f7..7f59fed66f 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -30,39 +30,38 @@ #define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect -#define USE_EXTI -#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW - -#define USE_MAG_DATA_READY_SIGNAL -#define ENSURE_MAG_DATA_READY_IS_HIGH +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) +#define MPU_INT_EXTI PC13 +#define USE_EXTI +#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 #define GYRO -//#define USE_FAKE_GYRO -#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW90_DEG #define ACC -//#define USE_FAKE_ACC -#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6000 +#define GYRO_MPU6500_ALIGN CW90_DEG -#define ACC_MPU6500_ALIGN CW180_DEG -#define GYRO_MPU6500_ALIGN CW180_DEG +#define BMP280_SPI_INSTANCE SPI1 +#define BMP280_CS_PIN PA13 #define BARO #define USE_BARO_BMP280 +#define USE_BARO_SPI_BMP280 -#define MAG -#define USE_MPU9250_MAG // Enables bypass configuration -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 // External +//#define MAG +//#define USE_MAG_AK8975 +//#define USE_MAG_HMC5883 // External +// +//#define MAG_AK8975_ALIGN CW90_DEG_FLIP -#define MAG_AK8975_ALIGN CW90_DEG_FLIP - -#define SONAR -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN PB0 +//#define SONAR +//#define SONAR_ECHO_PIN PB1 +//#define SONAR_TRIGGER_PIN PB0 #define USB_IO #define USB_CABLE_DETECTION @@ -73,8 +72,7 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define USE_SOFTSERIAL1 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 4 #define UART1_TX_PIN PA9 // PA9 #define UART1_RX_PIN PA10 // PA10 @@ -85,13 +83,8 @@ #define UART3_TX_PIN PB10 // PB10 (AF7) #define UART3_RX_PIN PB11 // PB11 (AF7) -#define SOFTSERIAL_1_TIMER TIM2 -#define SOFTSERIAL_1_TIMER_RX_HARDWARE 9 // PA0 / PAD3 -#define SOFTSERIAL_1_TIMER_TX_HARDWARE 10 // PA1 / PAD4 - -#define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA - +//#define USE_I2C +//#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_1 @@ -108,14 +101,13 @@ // include the max7456 driver #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 -#define MAX7456_SPI_CS_PIN SPI1_NSS_PIN -#define MAX7456_DMA_CHANNEL_TX DMA1_Channel3 -//#define MAX7456_DMA_CHANNEL_RX DMA1_Channel2 // <- Conflicts with WS2811 DMA -#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_CH3_HANDLER +#define MAX7456_SPI_CS_PIN PB1 +//#define MAX7456_DMA_CHANNEL_TX DMA1_Channel3 +//#define MAX7456_DMA_CHANNEL_RX DMA1_Channel2 +//#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_CH3_HANDLER #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define SPI2_NSS_PIN PB12 #define SPI2_SCK_PIN PB13 @@ -144,21 +136,23 @@ // #define AFATFS_USE_INTROSPECTIVE_LOGGING #define USE_ADC -#define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +//#define BOARD_HAS_VOLTAGE_DIVIDER +#define VBAT_ADC_PIN PA0 +#define CURRENT_METER_ADC_PIN PA1 +#define ADC_INSTANCE ADC1 + +//#define RSSI_ADC_PIN PB1 +//#define ADC_INSTANCE ADC3 -#define LED_STRIP - -#define WS2811_PIN PA8 -#define WS2811_TIMER TIM1 -#define WS2811_DMA_CHANNEL DMA1_Channel2 -#define WS2811_IRQ DMA1_Channel2_IRQn -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER +//#define LED_STRIP +// +//#define WS2811_PIN PA8 +//#define WS2811_TIMER TIM1 +//#define WS2811_DMA_CHANNEL DMA1_Channel2 +//#define WS2811_IRQ DMA1_Channel2_IRQn +//#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +//#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER //#define TRANSPONDER @@ -178,7 +172,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES FEATURE_BLACKBOX +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_ONESHOT125) #define BUTTONS #define BUTTON_A_PORT GPIOB @@ -201,7 +195,3 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) - -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_APB2_PERIPHERALS (RCC_APB2Periph_TIM1 | RCC_APB2Periph_TIM15) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 1cb198e263..26a246adcd 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -3,15 +3,11 @@ FEATURES = VCP SDCARD TARGET_SRC = \ drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6000.c \ drivers/barometer_bmp280.c \ - drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c \ - drivers/flash_m25p16.c \ + drivers/barometer_spi_bmp280.c \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_softserial.c \ drivers/serial_usb_vcp.c \ - drivers/sonar_hcsr04.c \ drivers/max7456.c \ io/osd.c From 7c72816d7bc77064029328b5d0129843479b4bfd Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 13 Jul 2016 19:20:01 +1000 Subject: [PATCH 012/456] Removed tabs --- src/main/vcpf4/usbd_cdc_vcp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 42913891ea..61e26a1131 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -223,7 +223,7 @@ static uint32_t rxPackets = 0; static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) { - __disable_irq(); + __disable_irq(); rxPackets++; @@ -234,7 +234,7 @@ static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) rxTotalBytes++; } - __enable_irq(); + __enable_irq(); if(receiveLength > (USB_RX_BUFSIZE-1)) return USBD_FAIL; From 0e3afca946e947f5009b1aaee9cafc3f684f6fe1 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 13 Jul 2016 19:25:19 +1000 Subject: [PATCH 013/456] fix for OneShot42 issue. --- src/main/drivers/pwm_output.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index fd8446584d..cc5a6073c2 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -118,6 +118,7 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 p->period = period; p->tim = timerHardware->tim; + *p->ccr = 0; return p; } From 7ca18b52ca0da2f3e5079e74d51ae45e9372e83e Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 13 Jul 2016 19:39:03 +1000 Subject: [PATCH 014/456] Added sdcard not present check --- src/main/drivers/sdcard.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 3813fed74b..7e0b6f5a91 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -192,6 +192,11 @@ static void sdcard_deselect(void) */ static void sdcard_reset(void) { + if (!sdcard_isInserted()) { + sdcard.state = SDCARD_STATE_NOT_PRESENT; + return; + } + if (sdcard.state >= SDCARD_STATE_READY) { spiSetDivisor(SDCARD_SPI_INSTANCE, SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER); } From 7f4a02b73570e0a0f17acd50dd8110eb131f03d5 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 13 Jul 2016 19:42:08 +1000 Subject: [PATCH 015/456] tabs to spaces --- src/main/drivers/sdcard.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 7e0b6f5a91..a81af703ff 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -193,8 +193,8 @@ static void sdcard_deselect(void) static void sdcard_reset(void) { if (!sdcard_isInserted()) { - sdcard.state = SDCARD_STATE_NOT_PRESENT; - return; + sdcard.state = SDCARD_STATE_NOT_PRESENT; + return; } if (sdcard.state >= SDCARD_STATE_READY) { From e4f9118e82d9e288195c68c9c5e2f5ab2bcae4eb Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 13 Jul 2016 14:07:32 +0200 Subject: [PATCH 016/456] ident --- src/main/drivers/pwm_output.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index cc5a6073c2..056ee17faa 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -118,7 +118,8 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 p->period = period; p->tim = timerHardware->tim; - *p->ccr = 0; + *p->ccr = 0; + return p; } From d3cfc5f1cbe6ceb71a694c5b17afc03184e83741 Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Wed, 13 Jul 2016 19:37:36 +0200 Subject: [PATCH 017/456] Added name command to CLI --- src/main/config/config.c | 2 ++ src/main/config/config.h | 2 ++ src/main/config/config_master.h | 3 +++ src/main/io/serial_cli.c | 19 +++++++++++++++++++ 4 files changed, 26 insertions(+) diff --git a/src/main/config/config.c b/src/main/config/config.c index 06ba7c9c09..7841ef9328 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -441,6 +441,7 @@ static void resetConf(void) featureSet(FEATURE_VBAT); #endif + masterConfig.version = EEPROM_CONF_VERSION; masterConfig.mixerMode = MIXER_QUADX; @@ -657,6 +658,7 @@ static void resetConf(void) targetConfiguration(); #endif + // copy first profile into remaining profile for (int i = 1; i < MAX_PROFILE_COUNT; i++) { memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); diff --git a/src/main/config/config.h b/src/main/config/config.h index 5a46a78c33..ffcbea1691 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -24,6 +24,8 @@ #endif #define MAX_RATEPROFILES 3 #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 +#define MAX_NAME_LENGTH 32 + typedef enum { FEATURE_RX_PPM = 1 << 0, diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index c6f01b4029..37ecaa8258 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -161,6 +161,9 @@ typedef struct master_t { uint8_t magic_ef; // magic number, should be 0xEF uint8_t chk; // XOR checksum + + char name[MAX_NAME_LENGTH]; + } master_t; extern master_t masterConfig; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index c0f3a8c417..5624fd1b7c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -119,6 +119,7 @@ void cliDumpRateProfile(uint8_t rateProfileIndex) ; static void cliExit(char *cmdline); static void cliFeature(char *cmdline); static void cliMotor(char *cmdline); +static void cliName(char *cmdline); static void cliPlaySound(char *cmdline); static void cliProfile(char *cmdline); static void cliRateProfile(char *cmdline); @@ -340,6 +341,7 @@ const clicmd_t cmdTable[] = { #ifdef VTX CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx), #endif + CLI_COMMAND_DEF("name", "Name of vessel", NULL, cliName), }; #define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t)) @@ -1953,6 +1955,7 @@ static void cliDump(char *cmdline) cliPrint("\r\n# version\r\n"); cliVersion(NULL); + cliName(""); cliPrint("\r\n# dump master\r\n"); cliPrint("\r\n# mixer\r\n"); @@ -2494,6 +2497,22 @@ static void cliMotor(char *cmdline) cliPrintf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]); } +static void cliName(char *cmdline) +{ + + uint32_t len = strlen(cmdline); + if (isEmpty(cmdline)) { + cliPrintf("name %s\r\n", masterConfig.name); + } else if (len <= MAX_NAME_LENGTH) { + strcpy(masterConfig.name, cmdline); + cliPrintf("name %s\r\n", masterConfig.name); + } else { + cliPrintf("Max allowed name size is %d\r\n", MAX_NAME_LENGTH); + } + + return; +} + static void cliPlaySound(char *cmdline) { #if FLASH_SIZE <= 64 From fbdd6547f2af3d7076f78a9aebadd8735ba981be Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Wed, 13 Jul 2016 19:44:11 +0200 Subject: [PATCH 018/456] MSP cleanup, commands should really be in order --- src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.h | 33 +++++++++++++++++---------------- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5624fd1b7c..54e99fd7b3 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2507,7 +2507,7 @@ static void cliName(char *cmdline) strcpy(masterConfig.name, cmdline); cliPrintf("name %s\r\n", masterConfig.name); } else { - cliPrintf("Max allowed name size is %d\r\n", MAX_NAME_LENGTH); + cliPrintf("Max allowed name length is %d\r\n", MAX_NAME_LENGTH); } return; diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 1a63b2b20d..1287955d02 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -147,6 +147,23 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_ARMING_CONFIG 61 //out message Returns auto_disarm_delay and disarm_kill_switch parameters #define MSP_SET_ARMING_CONFIG 62 //in message Sets auto_disarm_delay and disarm_kill_switch parameters +// +// Baseflight MSP commands (if enabled they exist in Cleanflight) +// +#define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total) +#define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP + +// FIXME - Provided for backwards compatibility with configurator code until configurator is updated. +// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. +#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere +#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save + +#define MSP_REBOOT 68 //in message reboot settings + +// DEPRECATED - Use MSP_BUILD_INFO instead +#define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion + + #define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip #define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip #define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip @@ -177,22 +194,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_VTX_CONFIG 88 //in message Get vtx settings #define MSP_SET_VTX_CONFIG 89 //out message Set vtx settings -// -// Baseflight MSP commands (if enabled they exist in Cleanflight) -// -#define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total) -#define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP - -// FIXME - Provided for backwards compatibility with configurator code until configurator is updated. -// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. -#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere -#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save - -#define MSP_REBOOT 68 //in message reboot settings - -// DEPRECATED - Use MSP_BUILD_INFO instead -#define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion - // Betaflight Additional Commands #define MSP_PID_ADVANCED_CONFIG 90 #define MSP_SET_PID_ADVANCED_CONFIG 91 From 1e4a0c72fcea680ea9a67fb4cd7a74eaa8f69f7f Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Wed, 13 Jul 2016 20:47:38 +0200 Subject: [PATCH 019/456] MSP_NAME --- src/main/io/serial_msp.c | 7 +++++++ src/main/io/serial_msp.h | 3 +++ 2 files changed, 10 insertions(+) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 80e4717fde..5fb8003fb5 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -750,6 +750,13 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(constrain(averageSystemLoadPercent, 0, 100)); break; + case MSP_NAME: + headSerialReply(MAX_NAME_LENGTH); + for (uint8_t i=0; i Date: Wed, 13 Jul 2016 23:29:51 +0200 Subject: [PATCH 020/456] MSP_SET_NAME --- src/main/io/serial_cli.c | 3 +++ src/main/io/serial_msp.c | 6 ++++++ src/main/io/serial_msp.h | 1 + 3 files changed, 10 insertions(+) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 54e99fd7b3..945b93ea1d 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2505,6 +2505,9 @@ static void cliName(char *cmdline) cliPrintf("name %s\r\n", masterConfig.name); } else if (len <= MAX_NAME_LENGTH) { strcpy(masterConfig.name, cmdline); + for (uint8_t i = len; i Date: Wed, 13 Jul 2016 21:40:03 -0700 Subject: [PATCH 021/456] mpu6000 acc not 6500 gyro --- src/main/target/OMNIBUS/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 7f59fed66f..6821d67cbe 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -44,7 +44,7 @@ #define ACC #define USE_ACC_SPI_MPU6000 -#define GYRO_MPU6500_ALIGN CW90_DEG +#define ACC_MPU6000_ALIGN CW90_DEG #define BMP280_SPI_INSTANCE SPI1 #define BMP280_CS_PIN PA13 From 26ee39666e6440dcc71a26f47d8df224a5b69028 Mon Sep 17 00:00:00 2001 From: nathan Date: Wed, 13 Jul 2016 21:40:17 -0700 Subject: [PATCH 022/456] re-enable transponder and ws2811 --- src/main/target/OMNIBUS/target.h | 42 +++++++++++++++---------------- src/main/target/OMNIBUS/target.mk | 3 +++ 2 files changed, 23 insertions(+), 22 deletions(-) diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 6821d67cbe..b9119c5fd3 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -145,30 +145,28 @@ //#define ADC_INSTANCE ADC3 -//#define LED_STRIP -// -//#define WS2811_PIN PA8 -//#define WS2811_TIMER TIM1 -//#define WS2811_DMA_CHANNEL DMA1_Channel2 -//#define WS2811_IRQ DMA1_Channel2_IRQn -//#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 -//#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER +#define LED_STRIP +#define WS2811_PIN PA8 +#define WS2811_TIMER TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER +#define TRANSPONDER +#define TRANSPONDER_GPIO GPIOA +#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define TRANSPONDER_GPIO_AF GPIO_AF_6 +#define TRANSPONDER_PIN GPIO_Pin_8 +#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8 +#define TRANSPONDER_TIMER TIM1 +#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +#define TRANSPONDER_DMA_CHANNEL DMA1_Channel2 +#define TRANSPONDER_IRQ DMA1_Channel2_IRQn +#define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC2 +#define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER -//#define TRANSPONDER -//#define TRANSPONDER_GPIO GPIOA -//#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -//#define TRANSPONDER_GPIO_AF GPIO_AF_6 -//#define TRANSPONDER_PIN GPIO_Pin_8 -//#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8 -//#define TRANSPONDER_TIMER TIM1 -//#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 -//#define TRANSPONDER_DMA_CHANNEL DMA1_Channel2 -//#define TRANSPONDER_IRQ DMA1_Channel2_IRQn -//#define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC2 -//#define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER - -//#define REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT +#define REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 26a246adcd..82cde62266 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -9,5 +9,8 @@ TARGET_SRC = \ drivers/light_ws2811strip.c \ drivers/light_ws2811strip_stm32f30x.c \ drivers/serial_usb_vcp.c \ + drivers/transponder_ir.c \ + drivers/transponder_ir_stm32f30x.c \ + io/transponder_ir.c \ drivers/max7456.c \ io/osd.c From f6794f926ed5f720ad4616f54c11ad40f0b28b34 Mon Sep 17 00:00:00 2001 From: blckmn Date: Thu, 14 Jul 2016 17:27:04 +1000 Subject: [PATCH 023/456] Added SD Card output to the status command if applicable to target --- src/main/io/serial_cli.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index c0f3a8c417..283f905c09 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2904,6 +2904,10 @@ static void cliStatus(char *cmdline) #endif cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t)); + +#ifdef USE_SDCARD + cliSdInfo(NULL); +#endif } #ifndef SKIP_TASK_STATISTICS From 4d69682c9649d48e09ca97c3ec6f23213fdacc82 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 14 Jul 2016 10:33:05 +0100 Subject: [PATCH 024/456] Use ccache if it is installed on the system. --- Makefile | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index e41d2d19e4..90a221d2a7 100644 --- a/Makefile +++ b/Makefile @@ -546,10 +546,17 @@ VPATH := $(VPATH):$(STDPERIPH_DIR)/src # Things that might need changing to use different tools # +# Find out if ccache is installed on the system +CCACHE := ccache +RESULT = $(shell which $(CCACHE)) +ifeq ($(RESULT),) +CCACHE := +endif + # Tool names -CC = arm-none-eabi-gcc -OBJCOPY = arm-none-eabi-objcopy -SIZE = arm-none-eabi-size +CC := $(CCACHE) arm-none-eabi-gcc +OBJCOPY := arm-none-eabi-objcopy +SIZE := arm-none-eabi-size # # Tool options. From 72ba8ebccf44f6877a4e24ff9846e081f8b3a0a3 Mon Sep 17 00:00:00 2001 From: gaelj Date: Thu, 14 Jul 2016 16:28:33 +0200 Subject: [PATCH 025/456] Fix white color undefined in led drivers --- src/main/drivers/light_ws2811strip_stm32f10x.c | 1 + src/main/drivers/light_ws2811strip_stm32f30x.c | 1 + src/main/drivers/light_ws2811strip_stm32f4xx.c | 1 + 3 files changed, 3 insertions(+) diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 96bb85fd26..b4a9b54268 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -105,6 +105,7 @@ void ws2811LedStripHardwareInit(void) DMA_ITConfig(DMA1_Channel6, DMA_IT_TC, ENABLE); + const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index ff52a64836..722dd9a2e4 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -111,6 +111,7 @@ void ws2811LedStripHardwareInit(void) DMA_ITConfig(WS2811_DMA_CHANNEL, DMA_IT_TC, ENABLE); + const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index 3a888bd21b..af764801f7 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -154,6 +154,7 @@ void ws2811LedStripHardwareInit(void) dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); + const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); From 1ee8224706cd8185ee1ec76ca6e87ae880d6e19a Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Thu, 14 Jul 2016 21:45:17 +0200 Subject: [PATCH 026/456] Fixes after feedback --- src/main/config/config.h | 2 +- src/main/config/config_master.h | 2 +- src/main/io/serial_cli.c | 20 ++++++++++---------- src/main/io/serial_msp.c | 13 +++++++++---- 4 files changed, 21 insertions(+), 16 deletions(-) diff --git a/src/main/config/config.h b/src/main/config/config.h index ffcbea1691..b18febe8cd 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -24,7 +24,7 @@ #endif #define MAX_RATEPROFILES 3 #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 -#define MAX_NAME_LENGTH 32 +#define MAX_NAME_LENGTH 16 typedef enum { diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 37ecaa8258..4f8f4903fb 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -162,7 +162,7 @@ typedef struct master_t { uint8_t magic_ef; // magic number, should be 0xEF uint8_t chk; // XOR checksum - char name[MAX_NAME_LENGTH]; + char name[MAX_NAME_LENGTH+1]; } master_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 945b93ea1d..4d1fdb9364 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -341,7 +341,7 @@ const clicmd_t cmdTable[] = { #ifdef VTX CLI_COMMAND_DEF("vtx", "vtx channels on switch", NULL, cliVtx), #endif - CLI_COMMAND_DEF("name", "Name of vessel", NULL, cliName), + CLI_COMMAND_DEF("name", "Name of craft", NULL, cliName), }; #define CMD_COUNT (sizeof(cmdTable) / sizeof(clicmd_t)) @@ -1955,7 +1955,8 @@ static void cliDump(char *cmdline) cliPrint("\r\n# version\r\n"); cliVersion(NULL); - cliName(""); + cliPrint("\r\n# name\r\n"); + cliName(NULL); cliPrint("\r\n# dump master\r\n"); cliPrint("\r\n# mixer\r\n"); @@ -2501,16 +2502,15 @@ static void cliName(char *cmdline) { uint32_t len = strlen(cmdline); - if (isEmpty(cmdline)) { - cliPrintf("name %s\r\n", masterConfig.name); - } else if (len <= MAX_NAME_LENGTH) { - strcpy(masterConfig.name, cmdline); - for (uint8_t i = len; idataSize); i++) { masterConfig.name[i] = read8(); } + if (masterConfig.name[0] == '-') { + memset(masterConfig.name, '\0', MAX_NAME_LENGTH); + } break; default: // we do not know how to handle the (valid) message, indicate error MSP $M! From 9836eca375a073a2333cbb074b90da2a2c5bd228 Mon Sep 17 00:00:00 2001 From: nathan Date: Fri, 15 Jul 2016 00:07:03 -0700 Subject: [PATCH 027/456] f4 updates for max7456 dma and a new target (revo variant) with the osd chip --- src/main/drivers/dma.c | 31 ++++++ src/main/drivers/dma.h | 4 +- src/main/drivers/dma_stm32f4xx.c | 4 + src/main/drivers/max7456.c | 18 +++- src/main/target/OMNIBUSF4/REVO_OPBL.mk | 0 src/main/target/OMNIBUSF4/target.c | 105 +++++++++++++++++++ src/main/target/OMNIBUSF4/target.h | 133 +++++++++++++++++++++++++ src/main/target/OMNIBUSF4/target.mk | 10 ++ 8 files changed, 300 insertions(+), 5 deletions(-) create mode 100644 src/main/target/OMNIBUSF4/REVO_OPBL.mk create mode 100644 src/main/target/OMNIBUSF4/target.c create mode 100644 src/main/target/OMNIBUSF4/target.h create mode 100644 src/main/target/OMNIBUSF4/target.mk diff --git a/src/main/drivers/dma.c b/src/main/drivers/dma.c index 0ad411312e..cc7540601f 100644 --- a/src/main/drivers/dma.c +++ b/src/main/drivers/dma.c @@ -27,6 +27,26 @@ /* * DMA descriptors. */ +#ifdef STM32F4 +static dmaChannelDescriptor_t dmaDescriptors[] = { + DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream0, 0, DMA1_Stream0_IRQn, RCC_AHBPeriph_DMA1), + DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream1, 4, DMA1_Stream1_IRQn, RCC_AHBPeriph_DMA1), + DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream2, 8, DMA1_Stream2_IRQn, RCC_AHBPeriph_DMA1), + DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream3, 12, DMA1_Stream3_IRQn, RCC_AHBPeriph_DMA1), + DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream4, 16, DMA1_Stream4_IRQn, RCC_AHBPeriph_DMA1), + DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream5, 20, DMA1_Stream5_IRQn, RCC_AHBPeriph_DMA1), + DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream6, 24, DMA1_Stream6_IRQn, RCC_AHBPeriph_DMA1), + DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream7, 28, DMA1_Stream7_IRQn, RCC_AHBPeriph_DMA1), + DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream0, 0, DMA2_Stream0_IRQn, RCC_AHBPeriph_DMA2), + DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream1, 4, DMA2_Stream1_IRQn, RCC_AHBPeriph_DMA2), + DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream2, 8, DMA2_Stream2_IRQn, RCC_AHBPeriph_DMA2), + DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream3, 12, DMA2_Stream3_IRQn, RCC_AHBPeriph_DMA2), + DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream4, 16, DMA2_Stream4_IRQn, RCC_AHBPeriph_DMA2), + DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream5, 20, DMA2_Stream5_IRQn, RCC_AHBPeriph_DMA2), + DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream6, 24, DMA2_Stream5_IRQn, RCC_AHBPeriph_DMA2), + DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream7, 28, DMA2_Stream5_IRQn, RCC_AHBPeriph_DMA2), +}; +#else static dmaChannelDescriptor_t dmaDescriptors[] = { DEFINE_DMA_CHANNEL(DMA1, DMA1_Channel1, 0, DMA1_Channel1_IRQn, RCC_AHBPeriph_DMA1), DEFINE_DMA_CHANNEL(DMA1, DMA1_Channel2, 4, DMA1_Channel2_IRQn, RCC_AHBPeriph_DMA1), @@ -43,10 +63,14 @@ static dmaChannelDescriptor_t dmaDescriptors[] = { DEFINE_DMA_CHANNEL(DMA2, DMA2_Channel5, 16, DMA2_Channel5_IRQn, RCC_AHBPeriph_DMA2), #endif }; +#endif /* * DMA IRQ Handlers */ +#ifdef STM32F4 +DEFINE_DMA_IRQ_HANDLER(1, 0, DMA1_CH0_HANDLER) +#endif DEFINE_DMA_IRQ_HANDLER(1, 1, DMA1_CH1_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 2, DMA1_CH2_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 3, DMA1_CH3_HANDLER) @@ -55,6 +79,9 @@ DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_CH5_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_CH6_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_CH7_HANDLER) +#ifdef STM32F4 +DEFINE_DMA_IRQ_HANDLER(2, 0, DMA2_CH0_HANDLER) +#endif #if defined(STM32F3) || defined(STM32F10X_CL) DEFINE_DMA_IRQ_HANDLER(2, 1, DMA2_CH1_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_CH2_HANDLER) @@ -62,6 +89,10 @@ DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_CH3_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_CH4_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_CH5_HANDLER) #endif +#ifdef STM32F4 +DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_CH6_HANDLER) +DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_CH7_HANDLER) +#endif void dmaInit(void) diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index 4f0dabdfff..1665af5bd9 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -22,13 +22,15 @@ typedef void (*dmaCallbackHandlerFuncPtr)(struct dmaChannelDescriptor_s *channel #ifdef STM32F4 typedef enum { - DMA1_ST1_HANDLER = 0, + DMA1_ST0_HANDLER = 0, + DMA1_ST1_HANDLER, DMA1_ST2_HANDLER, DMA1_ST3_HANDLER, DMA1_ST4_HANDLER, DMA1_ST5_HANDLER, DMA1_ST6_HANDLER, DMA1_ST7_HANDLER, + DMA2_ST0_HANDLER, DMA2_ST1_HANDLER, DMA2_ST2_HANDLER, DMA2_ST3_HANDLER, diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index ce960ab4c2..a7dfd47045 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -51,6 +51,7 @@ static dmaChannelDescriptor_t dmaDescriptors[] = { /* * DMA IRQ Handlers */ +DEFINE_DMA_IRQ_HANDLER(1, 0, DMA1_ST0_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 1, DMA1_ST1_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 2, DMA1_ST2_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 3, DMA1_ST3_HANDLER) @@ -58,11 +59,14 @@ DEFINE_DMA_IRQ_HANDLER(1, 4, DMA1_ST4_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_ST5_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_ST6_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_ST7_HANDLER) +DEFINE_DMA_IRQ_HANDLER(2, 0, DMA2_ST0_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 1, DMA2_ST1_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_ST2_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_ST3_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_ST4_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_ST5_HANDLER) +DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER) +DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER) void dmaInit(void) diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index b9f521273c..035d665f34 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -75,6 +75,7 @@ static void max7456_send_dma(void* tx_buffer, void* rx_buffer, uint16_t buffer_s #endif // Common to both channels + DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(&(MAX7456_SPI_INSTANCE->DR)); DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; @@ -82,22 +83,31 @@ static void max7456_send_dma(void* tx_buffer, void* rx_buffer, uint16_t buffer_s DMA_InitStructure.DMA_BufferSize = buffer_size; DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; DMA_InitStructure.DMA_Priority = DMA_Priority_Low; - DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; #ifdef MAX7456_DMA_CHANNEL_RX // Rx Channel +#ifdef STM32F4 + DMA_InitStructure.DMA_Memory0BaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy); + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; +#else DMA_InitStructure.DMA_MemoryBaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy); - DMA_InitStructure.DMA_MemoryInc = rx_buffer ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; +#endif + DMA_InitStructure.DMA_MemoryInc = rx_buffer ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable; DMA_Init(MAX7456_DMA_CHANNEL_RX, &DMA_InitStructure); DMA_Cmd(MAX7456_DMA_CHANNEL_RX, ENABLE); #endif // Tx channel +#ifdef STM32F4 + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)tx_buffer; //max7456_screen; + DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; +#else DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)tx_buffer; //max7456_screen; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; +#endif + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_Init(MAX7456_DMA_CHANNEL_TX, &DMA_InitStructure); DMA_Cmd(MAX7456_DMA_CHANNEL_TX, ENABLE); @@ -247,7 +257,7 @@ void max7456_draw_screen(void) { max7456_send(MAX7456ADD_DMM, 1); for (xx = 0; xx < max_screen_size; ++xx) { max7456_send(MAX7456ADD_DMDI, SCREEN_BUFFER[xx]); - SCREEN_BUFFER[xx] = MAX7456_CHAR(0); + SCREEN_BUFFER[xx] = MAX7456_CHAR(' '); } max7456_send(MAX7456ADD_DMDI, 0xFF); max7456_send(MAX7456ADD_DMM, 0); diff --git a/src/main/target/OMNIBUSF4/REVO_OPBL.mk b/src/main/target/OMNIBUSF4/REVO_OPBL.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c new file mode 100644 index 0000000000..03f60516d4 --- /dev/null +++ b/src/main/target/OMNIBUSF4/target.c @@ -0,0 +1,105 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM1 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + 0xFFFF +}; + + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S6_IN + + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5 }, // S6_OUT +}; + + diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h new file mode 100644 index 0000000000..fccca3016b --- /dev/null +++ b/src/main/target/OMNIBUSF4/target.h @@ -0,0 +1,133 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "OBF4" + +#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) + +#define USBD_PRODUCT_STRING "OmnibusF4" +#ifdef OPBL +#define USBD_SERIALNUMBER_STRING "0x8020000" +#endif + +#define LED0 PB5 +#define LED1 PB4 +#define BEEPER PB4 +#define INVERTER PC0 // PC0 used as inverter select GPIO +#define INVERTER_USART USART1 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW270_DEG + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW270_DEG + +// MPU6000 interrupts +#define USE_MPU_DATA_READY_SIGNAL +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) +#define MPU_INT_EXTI PC4 +#define USE_EXTI + +#define MAG +#define USE_MAG_HMC5883 +#define MAG_HMC5883_ALIGN CW90_DEG + +//#define USE_MAG_NAZA +//#define MAG_NAZA_ALIGN CW180_DEG_FLIP + +#define BARO +#define USE_BARO_MS5611 + +#define OSD +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PA15 + +#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5 +#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0 +#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER + +//#define PITOT +//#define USE_PITOT_MS4525 +//#define MS4525_BUS I2C_DEVICE_EXT + +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_INSTANCE SPI3 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define USABLE_TIMER_CHANNEL_COUNT 12 + +#define USE_VCP +#define VBUS_SENSING_PIN PC5 + +#define USE_USART1 +#define USART1_RX_PIN PA10 +#define USART1_TX_PIN PA9 +#define USART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 + +#define USE_USART3 +#define USART3_RX_PIN PB11 +#define USART3_TX_PIN PB10 + +#define USE_USART6 +#define USART6_RX_PIN PC7 +#define USART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 + +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PB3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) + +#define USE_ADC +#define CURRENT_METER_ADC_PIN PC1 +#define VBAT_ADC_PIN PC2 +#define RSSI_ADC_GPIO_PIN PA0 + + +#define SENSORS_SET (SENSOR_ACC) + + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125 | FEATURE_RX_SERIAL) + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk new file mode 100644 index 0000000000..18034c1332 --- /dev/null +++ b/src/main/target/OMNIBUSF4/target.mk @@ -0,0 +1,10 @@ +F405_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_spi_mpu6000.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ + drivers/max7456.c \ + io/osd.c + From eec8bbd85a663166e0f9d512ad0a4d034398ae49 Mon Sep 17 00:00:00 2001 From: nathan Date: Fri, 15 Jul 2016 00:11:36 -0700 Subject: [PATCH 028/456] dont need the opbl omnibusf4 makefile --- src/main/target/OMNIBUSF4/REVO_OPBL.mk | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/main/target/OMNIBUSF4/REVO_OPBL.mk diff --git a/src/main/target/OMNIBUSF4/REVO_OPBL.mk b/src/main/target/OMNIBUSF4/REVO_OPBL.mk deleted file mode 100644 index e69de29bb2..0000000000 From 59adc362c35f542fd634b55c23f6b500be98fe26 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 15 Jul 2016 08:28:16 +0100 Subject: [PATCH 029/456] Whitespace and variable initialisation tidy --- src/main/drivers/adc_stm32f10x.c | 10 +-- src/main/drivers/adc_stm32f30x.c | 8 +- src/main/drivers/adc_stm32f4xx.c | 2 - src/main/drivers/light_led.c | 11 ++- src/main/drivers/resource.h | 2 +- src/main/sensors/gyro.c | 5 +- src/main/sensors/initialisation.c | 16 ++-- src/main/target/AIR32/target.h | 67 +++++++------- src/main/target/ALIENFLIGHTF3/target.h | 68 +++++++------- src/main/target/ALIENFLIGHTF4/target.h | 104 +++++++++++----------- src/main/target/BLUEJAYF4/target.h | 64 ++++++------- src/main/target/CC3D/target.h | 58 ++++++------ src/main/target/CHEBUZZF3/target.h | 10 +-- src/main/target/CJMCU/target.h | 18 ++-- src/main/target/COLIBRI_RACE/target.h | 64 ++++++------- src/main/target/EUSTM32F103RC/target.h | 8 +- src/main/target/LUX_RACE/target.h | 52 +++++------ src/main/target/MOTOLAB/target.h | 10 +-- src/main/target/NAZE/target.h | 88 +++++++++--------- src/main/target/OLIMEXINO/target.h | 8 +- src/main/target/PORT103R/target.h | 8 +- src/main/target/REVO/target.h | 58 ++++++------ src/main/target/REVONANO/target.h | 49 +++++----- src/main/target/RMDO/target.h | 10 +-- src/main/target/SPARKY/target.h | 54 ++++++----- src/main/target/SPRACINGF3/target.h | 55 ++++++------ src/main/target/SPRACINGF3EVO/target.h | 11 ++- src/main/target/STM32F3DISCOVERY/target.h | 59 ++++++------ 28 files changed, 477 insertions(+), 500 deletions(-) diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 451c9296c6..bc23c2b96c 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -24,9 +24,6 @@ #include "build_config.h" #include "system.h" - -#include "sensors/sensors.h" // FIXME dependency into the main code - #include "sensor.h" #include "accgyro.h" #include "adc.h" @@ -84,7 +81,6 @@ void adcInit(drv_adc_config_t *init) UNUSED(init); #endif - uint8_t i; uint8_t configuredAdcChannels = 0; memset(&adcConfig, 0, sizeof(adcConfig)); @@ -117,9 +113,9 @@ void adcInit(drv_adc_config_t *init) if (device == ADCINVALID) return; - adcDevice_t adc = adcHardware[device]; + const adcDevice_t adc = adcHardware[device]; - for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) { + for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].tag) continue; @@ -163,7 +159,7 @@ void adcInit(drv_adc_config_t *init) ADC_Init(adc.ADCx, &ADC_InitStructure); uint8_t rank = 1; - for (i = 0; i < ADC_CHANNEL_COUNT; i++) { + for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].enabled) { continue; } diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 6297c2bd16..5cbe50607c 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -21,7 +21,6 @@ #include "platform.h" #include "system.h" -#include "common/utils.h" #include "gpio.h" #include "sensor.h" @@ -32,6 +31,8 @@ #include "io.h" #include "rcc.h" +#include "common/utils.h" + #ifndef ADC_INSTANCE #define ADC_INSTANCE ADC1 #endif @@ -100,7 +101,6 @@ void adcInit(drv_adc_config_t *init) ADC_InitTypeDef ADC_InitStructure; DMA_InitTypeDef DMA_InitStructure; - uint8_t i; uint8_t adcChannelCount = 0; memset(&adcConfig, 0, sizeof(adcConfig)); @@ -135,7 +135,7 @@ void adcInit(drv_adc_config_t *init) adcDevice_t adc = adcHardware[device]; - for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) { + for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].tag) continue; @@ -203,7 +203,7 @@ void adcInit(drv_adc_config_t *init) ADC_Init(adc.ADCx, &ADC_InitStructure); uint8_t rank = 1; - for (i = 0; i < ADC_CHANNEL_COUNT; i++) { + for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].enabled) { continue; } diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index e1bb1ae617..197ff12999 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -26,8 +26,6 @@ #include "io_impl.h" #include "rcc.h" -#include "sensors/sensors.h" // FIXME dependency into the main code - #include "sensor.h" #include "accgyro.h" diff --git a/src/main/drivers/light_led.c b/src/main/drivers/light_led.c index 5fe73a1664..8b1bdc5ab4 100644 --- a/src/main/drivers/light_led.c +++ b/src/main/drivers/light_led.c @@ -78,15 +78,14 @@ uint8_t ledPolarity = 0 #endif ; -uint8_t ledOffset = 0; +static uint8_t ledOffset = 0; void ledInit(bool alternative_led) { - uint32_t i; - #if defined(LED0_A) || defined(LED1_A) || defined(LED2_A) - if (alternative_led) + if (alternative_led) { ledOffset = LED_NUMBER; + } #else UNUSED(alternative_led); #endif @@ -95,7 +94,7 @@ void ledInit(bool alternative_led) LED1_OFF; LED2_OFF; - for (i = 0; i < LED_NUMBER; i++) { + for (int i = 0; i < LED_NUMBER; i++) { if (leds[i + ledOffset]) { IOInit(leds[i + ledOffset], OWNER_LED, RESOURCE_OUTPUT, RESOURCE_INDEX(i)); IOConfigGPIO(leds[i + ledOffset], IOCFG_OUT_PP); @@ -114,6 +113,6 @@ void ledToggle(int led) void ledSet(int led, bool on) { - bool inverted = (1 << (led + ledOffset)) & ledPolarity; + const bool inverted = (1 << (led + ledOffset)) & ledPolarity; IOWrite(leds[led + ledOffset], on ? inverted : !inverted); } diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index a177dab1da..f9430d2844 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -1,7 +1,7 @@ #pragma once -#define RESOURCE_INDEX(x) x + 1 +#define RESOURCE_INDEX(x) (x + 1) typedef enum { OWNER_FREE = 0, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 4f425dc1d2..67e56b43d0 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -28,11 +28,12 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" -#include "sensors/sensors.h" + #include "io/beeper.h" #include "io/statusindicator.h" -#include "sensors/boardalignment.h" +#include "sensors/sensors.h" +#include "sensors/boardalignment.h" #include "sensors/gyro.h" gyro_t gyro; // gyro access functions diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index eb206627b4..3ba1f439a2 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -584,10 +584,14 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) } } -bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator) +bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, + uint8_t accHardwareToUse, + uint8_t magHardwareToUse, + uint8_t baroHardwareToUse, + int16_t magDeclinationFromConfig, + uint8_t gyroLpf, + uint8_t gyroSyncDenominator) { - int16_t deg, min; - memset(&acc, 0, sizeof(acc)); memset(&gyro, 0, sizeof(gyro)); @@ -605,7 +609,6 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a detectAcc(accHardwareToUse); detectBaro(baroHardwareToUse); - // Now time to init things, acc first if (sensors(SENSOR_ACC)) { acc.acc_1G = 256; // set default @@ -623,9 +626,8 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c if (sensors(SENSOR_MAG)) { // calculate magnetic declination - deg = magDeclinationFromConfig / 100; - min = magDeclinationFromConfig % 100; - + const int16_t deg = magDeclinationFromConfig / 100; + const int16_t min = magDeclinationFromConfig % 100; magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units } else { magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index 3e60de5031..253af13be0 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -22,17 +22,15 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB5 // Blue LED - PB5 - - -#define BEEPER PA0 +#define LED0 PB5 // Blue LED - PB5 +#define BEEPER PA0 #define USABLE_TIMER_CHANNEL_COUNT 9 // MPU6050 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define MPU_INT_EXTI PA15 #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL //#define ENSURE_MPU_DATA_READY_IS_LOW @@ -41,20 +39,18 @@ #define ACC #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW180_DEG +#define GYRO_MPU6050_ALIGN CW180_DEG #define USE_ACC_MPU6050 - -#define ACC_MPU6050_ALIGN CW180_DEG +#define ACC_MPU6050_ALIGN CW180_DEG #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_MPU6000_ALIGN CW180_DEG -#define MPU6000_CS_GPIO GPIOB -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_INSTANCE SPI2 //#define BARO //#define USE_BARO_MS5611 @@ -66,31 +62,30 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PB6 -#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 -#define UART2_TX_PIN PB3 -#define UART2_RX_PIN PB4 +#define UART2_TX_PIN PB3 +#define UART2_RX_PIN PB4 - -#define UART3_TX_PIN PB10 //(AF7) -#define UART3_RX_PIN PB11 //(AF7) +#define UART3_TX_PIN PB10 //(AF7) +#define UART3_RX_PIN PB11 //(AF7) #define USE_I2C #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) -#define I2C2_SCL PA9 -#define I2C2_SDA PA10 +#define I2C2_SCL PA9 +#define I2C2_SDA PA10 #define USE_SPI #define USE_SPI_DEVICE_2 -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_INSTANCE SPI2 +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 //#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) -#define SENSORS_SET (SENSOR_ACC) +#define SENSORS_SET (SENSOR_ACC) #undef GPS #define DISPLAY @@ -99,10 +94,10 @@ #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA5 -//#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA5 +//#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP #if 1 @@ -135,17 +130,17 @@ #define SPEKTRUM_BIND // USART2, PB4 -#define BIND_PIN PB4 +#define BIND_PIN PB4 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) // #define TARGET_IO_PORTF (BIT(0)|BIT(1)) // !!TODO - check the following line is correct -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index b05cdb33a4..8ce5876683 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -23,17 +23,17 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define USE_HARDWARE_REVISION_DETECTION -#define HW_PIN PB2 +#define HW_PIN PB2 // LED's V1 -#define LED0 PB4 // LED - PB4 -#define LED1 PB5 // LED - PB5 +#define LED0 PB4 +#define LED1 PB5 // LED's V2 -#define LED0_A PB8 // LED - PB8 -#define LED1_A PB9 // LED - PB9 +#define LED0_A PB8 +#define LED1_A PB9 -#define BEEPER PA5 // LED - PA5 +#define BEEPER PA5 #define USABLE_TIMER_CHANNEL_COUNT 11 @@ -48,15 +48,15 @@ #define USE_GYRO_MPU6050 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6050_ALIGN CW270_DEG -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_MPU6500_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6050 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6050_ALIGN CW270_DEG -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_MPU6500_ALIGN CW270_DEG // No baro support. //#define BARO @@ -65,29 +65,28 @@ // option to use MPU9150 or MPU9250 integrated AK89xx Mag #define MAG #define USE_MAG_AK8963 - -#define MAG_AK8963_ALIGN CW0_DEG_FLIP +#define MAG_AK8963_ALIGN CW0_DEG_FLIP #define USE_VCP #define USE_UART1 // Not connected - TX (PB6) RX PB7 (AF7) #define USE_UART2 // Receiver - RX (PA3) #define USE_UART3 // Not connected - 10/RX (PB11) 11/TX (PB10) -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PB6 // PB6 -#define UART1_RX_PIN PB7 // PB7 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 -#define UART2_TX_PIN PA2 // PA2 -#define UART2_RX_PIN PA3 // PA3 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 #define USE_I2C #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) -#define I2C2_SCL PA9 -#define I2C2_SDA PA10 +#define I2C2_SCL PA9 +#define I2C2_SDA PA10 // SPI3 // PA15 38 SPI3_NSS @@ -98,22 +97,22 @@ #define USE_SPI #define USE_SPI_DEVICE_3 -#define MPU6500_CS_PIN PA15 -#define MPU6500_SPI_INSTANCE SPI3 +#define MPU6500_CS_PIN PA15 +#define MPU6500_SPI_INSTANCE SPI3 #define USE_ADC -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define VBAT_SCALE_DEFAULT 20 +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define VBAT_SCALE_DEFAULT 20 #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 #define HARDWARE_BIND_PLUG // Hardware bind plug at PB12 (Pin 25) -#define BINDPLUG_PIN PB12 +#define BINDPLUG_PIN PB12 #define BRUSHED_MOTORS #define DEFAULT_FEATURES FEATURE_MOTOR_STOP @@ -124,12 +123,11 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - assuming 303 in 64pin package, TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 909026c021..45a40fb0f1 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -23,39 +23,39 @@ #define USBD_PRODUCT_STRING "AlienFlight F4" -#define LED0 PC12 -#define LED1 PD2 +#define LED0 PC12 +#define LED1 PD2 -#define BEEPER PC13 +#define BEEPER PC13 -#define INVERTER PC15 -#define INVERTER_USART USART2 +#define INVERTER PC15 +#define INVERTER_USART USART2 // MPU interrupt //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready -#define MPU_INT_EXTI PC14 +#define MPU_INT_EXTI PC14 #define USE_EXTI -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_MPU6500_ALIGN CW270_DEG #define GYRO #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_MPU6500_ALIGN CW270_DEG #define MAG #define USE_MAG_HMC5883 #define USE_MAG_AK8963 -#define MAG_HMC5883_ALIGN CW180_DEG -#define MAG_AK8963_ALIGN CW270_DEG +#define MAG_HMC5883_ALIGN CW180_DEG +#define MAG_AK8963_ALIGN CW270_DEG #define BARO #define USE_BARO_MS5611 @@ -65,14 +65,14 @@ //#define SDCARD_DETECT_INVERTED -#define SDCARD_DETECT_PIN PB10 -#define SDCARD_DETECT_EXTI_LINE EXTI_Line10 -#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource10 -#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB -#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn +#define SDCARD_DETECT_PIN PB10 +#define SDCARD_DETECT_EXTI_LINE EXTI_Line10 +#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource10 +#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOB +#define SDCARD_DETECT_EXTI_IRQn EXTI15_10_IRQn -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN // SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz @@ -93,57 +93,55 @@ //#define USE_FLASHFS //#define USE_FLASH_M25P16 -#define USABLE_TIMER_CHANNEL_COUNT 13 +#define USABLE_TIMER_CHANNEL_COUNT 13 #define USE_VCP #define USE_UART1 -#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART2 -#define UART2_RX_PIN PA3 -#define UART2_TX_PIN PA2 //inverter +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 //inverter //#define USE_UART3 -//#define UART3_RX_PIN PB11 -//#define UART3_TX_PIN PB10 +//#define UART3_RX_PIN PB11 +//#define UART3_TX_PIN PB10 #define USE_UART4 -#define UART4_RX_PIN PC10 -#define UART4_TX_PIN PC11 +#define UART4_RX_PIN PC10 +#define UART4_TX_PIN PC11 //#define USE_UART5 -//#define UART5_RX_PIN PD2 -//#define UART5_TX_PIN PC12 +//#define UART5_RX_PIN PD2 +//#define UART5_TX_PIN PC12 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 #define USE_SPI - #define USE_SPI_DEVICE_1 - #define USE_SPI_DEVICE_2 -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PC2 -#define SPI2_MOSI_PIN PC3 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 #define USE_SPI_DEVICE_3 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) -//#define I2C_DEVICE_EXT (I2CDEV_2) -#define I2C1_SCL PB6 -#define I2C1_SDA PB7 +#define I2C_DEVICE (I2CDEV_1) +//#define I2C_DEVICE_EXT (I2CDEV_2) +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 #define USE_ADC //#define BOARD_HAS_VOLTAGE_DIVIDER -#define VBAT_ADC_PIN PC0 -#define CURRENT_METER_ADC_PIN PC1 -#define RSSI_ADC_PIN PC4 -#define EXTERNAL1_ADC_GPIO_PIN PC5 +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PC4 +#define EXTERNAL1_ADC_GPIO_PIN PC5 // LED strip configuration using RC5 pin. //#define LED_STRIP @@ -156,11 +154,11 @@ #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 #define HARDWARE_BIND_PLUG // Hardware bind plug at PB2 (Pin 28) -#define BINDPLUG_PIN PB2 +#define BINDPLUG_PIN PB2 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT @@ -172,10 +170,10 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) -#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) ) diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index e83499615a..6bad2fee16 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -20,40 +20,40 @@ #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define USBD_PRODUCT_STRING "BlueJayF4" +#define USBD_PRODUCT_STRING "BlueJayF4" #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_EXTI -#define INVERTER PB15 -#define INVERTER_USART USART6 +#define LED0 PB6 +#define LED1 PB5 +#define LED2 PB4 -#define BEEPER PB7 +#define BEEPER PB7 #define BEEPER_INVERTED -#define LED0 PB6 -#define LED1 PB5 -#define LED2 PB4 +#define INVERTER PB15 +#define INVERTER_USART USART6 -#define MPU6500_CS_PIN PC4 -#define MPU6500_SPI_INSTANCE SPI1 +#define MPU6500_CS_PIN PC4 +#define MPU6500_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG #define GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG //#define MAG //#define USE_MAG_AK8963 #define BARO #define USE_BARO_MS5611 -#define MS5611_I2C_INSTANCE I2CDEV_1 +#define MS5611_I2C_INSTANCE I2CDEV_1 #define USE_SDCARD @@ -76,8 +76,8 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -//#define M25P16_CS_PIN PB3 -//#define M25P16_SPI_INSTANCE SPI3 +//#define M25P16_CS_PIN PB3 +//#define M25P16_SPI_INSTANCE SPI3 //#define USE_FLASHFS //#define USE_FLASH_M25P16 @@ -96,22 +96,22 @@ //#define VBUS_SENSING_ENABLED #define USE_UART1 -#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 #define USE_UART6 -#define UART6_RX_PIN PC7 -#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 #define USE_SOFTSERIAL1 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 5 -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 @@ -134,11 +134,11 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) +#define I2C_DEVICE (I2CDEV_1) #define USE_I2C_PULLUP #define USE_ADC -#define VBAT_ADC_PIN PC3 +#define VBAT_ADC_PIN PC3 #define LED_STRIP // LED Strip can run off Pin 6 (PB1) of the ESC outputs. @@ -155,15 +155,15 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES FEATURE_BLACKBOX +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index a1425bc538..0e7b290780 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -17,25 +17,25 @@ #define TARGET_BOARD_IDENTIFIER "CC3D" // CopterControl 3D -#define LED0 PB3 // PB3 (LED) +#define LED0 PB3 -#define INVERTER PB2 // PB2 (BOOT1) used as inverter select GPIO -#define INVERTER_USART USART1 +#define INVERTER PB2 // PB2 (BOOT1) used as inverter select GPIO +#define INVERTER_USART USART1 -#define BEEPER PB15 // PB15 (Beeper) -#define BEEPER_OPT PB2 // PB15 (Beeper) +#define BEEPER PB15 +#define BEEPER_OPT PB2 #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PA3 +#define MPU_INT_EXTI PA3 -#define MPU6000_CS_GPIO GPIOA -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_GPIO GPIOA +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 -#define M25P16_CS_GPIO GPIOB -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_INSTANCE SPI2 +#define M25P16_CS_GPIO GPIOB +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -72,18 +72,18 @@ #define USE_UART1 #define USE_UART3 #define USE_SOFTSERIAL1 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 #ifdef USE_UART1_RX_DMA #undef USE_UART1_RX_DMA #endif -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 1 // PWM 2 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 2 // PWM 3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 #define USE_SPI #define USE_SPI_DEVICE_1 @@ -93,15 +93,15 @@ #define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11 #define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA0 -#define RSSI_ADC_PIN PB0 +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA0 +#define RSSI_ADC_PIN PB0 #define LED_STRIP -#define WS2811_PIN PB4 -#define WS2811_TIMER TIM3 -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER +#define WS2811_PIN PB4 +#define WS2811_TIMER TIM3 +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER #define SPEKTRUM_BIND // USART3, PB11 (Flexport) @@ -111,8 +111,8 @@ #define DISPLAY #define SONAR -#define SONAR_ECHO_PIN PB0 -#define SONAR_TRIGGER_PIN PB5 +#define SONAR_ECHO_PIN PB0 +#define SONAR_TRIGGER_PIN PB5 #undef GPS @@ -128,8 +128,8 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM // IO - from schematics -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC ( BIT(14) ) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC ( BIT(14) ) -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index 52bea95ba2..f60a7b7b2a 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -101,11 +101,11 @@ #define I2C_DEVICE (I2CDEV_1) #define USE_ADC -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PC0 -#define CURRENT_METER_ADC_PIN PC1 -#define RSSI_ADC_PIN PC2 -#define EXTERNAL1_ADC_PIN PC3 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define EXTERNAL1_ADC_PIN PC3 // IO - assuming 303 in 64pin package, TODO #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index d84e98eae1..4af75b2b2e 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -20,9 +20,9 @@ #define TARGET_BOARD_IDENTIFIER "CJM1" // CJMCU #define USE_HARDWARE_REVISION_DETECTION -#define LED0 PC14 // PC14 (LED) -#define LED1 PC13 // PC13 (LED) -#define LED2 PC15 // PC15 (LED) +#define LED0 PC14 +#define LED1 PC13 +#define LED2 PC15 #define ACC #define USE_ACC_MPU6050 @@ -38,7 +38,7 @@ #define USE_UART1 #define USE_UART2 -#define SERIAL_PORT_COUNT 2 +#define SERIAL_PORT_COUNT 2 #define USE_I2C #define I2C_DEVICE (I2CDEV_1) @@ -49,7 +49,7 @@ #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 #if (FLASH_SIZE > 64) @@ -64,8 +64,8 @@ #endif // IO - assuming all IOs on 48pin package TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 65ab62d5d6..a40ba0083b 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -18,17 +18,17 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "CLBR" -#define BST_DEVICE_NAME "COLIBRI RACE" -#define BST_DEVICE_NAME_LENGTH 12 +#define BST_DEVICE_NAME "COLIBRI RACE" +#define BST_DEVICE_NAME_LENGTH 12 #define TARGET_CONFIG #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PC15 -#define LED1 PC14 -#define LED2 PC13 +#define LED0 PC15 +#define LED1 PC14 +#define LED2 PC13 -#define BEEPER PB13 +#define BEEPER PB13 #define BEEPER_INVERTED #define USE_EXTI @@ -53,17 +53,17 @@ #define GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_MPU6000_ALIGN CW270_DEG #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_MPU6500_ALIGN CW270_DEG #define ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_MPU6000_ALIGN CW270_DEG #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_MPU6500_ALIGN CW270_DEG #define BARO #define USE_BARO_MS5611 @@ -80,34 +80,34 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PC4 -#define UART1_RX_PIN PC5 +#define UART1_TX_PIN PC4 +#define UART1_RX_PIN PC5 -#define UART2_TX_PIN PA14 -#define UART2_RX_PIN PA15 +#define UART2_TX_PIN PA14 +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 #define USE_I2C #define I2C_DEVICE (I2CDEV_2) -#define I2C2_SCL_PIN PA9 -#define I2C2_SDA_PIN PA10 +#define I2C2_SCL_PIN PA9 +#define I2C2_SDA_PIN PA10 #define USE_BST #define BST_DEVICE (BSTDEV_1) /* Configure the CRC peripheral to use the polynomial x8 + x7 + x6 + x4 + x2 + 1 */ -#define BST_CRC_POLYNOM 0xD5 +#define BST_CRC_POLYNOM 0xD5 #define USE_ADC -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PC0 -#define CURRENT_METER_ADC_PIN PC1 -#define RSSI_ADC_PIN PC2 -#define EXTERNAL1_ADC_PIN PC3 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP #define USE_COLIBTI_RACE_LED_DEFAULT_CONFIG @@ -122,7 +122,7 @@ // MPU6500 interrupt #define USE_EXTI -#define MPU_INT_EXTI PA5 +#define MPU_INT_EXTI PA5 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -135,11 +135,11 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - assuming 303 in 64pin package, TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index ee893670dd..747b9b03bb 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -97,10 +97,10 @@ // #define SOFT_I2C_PB67 #define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA4 -#define RSSI_ADC_PIN PA1 -#define EXTERNAL1_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA4 +#define RSSI_ADC_PIN PA1 +#define EXTERNAL1_ADC_PIN PA5 #define SPEKTRUM_BIND // USART2, PA3 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index ac7d2c06dd..81fb3b3cee 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -22,11 +22,11 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PC15 -#define LED1 PC14 -#define LED2 PC13 +#define LED0 PC15 +#define LED1 PC14 +#define LED2 PC13 -#define BEEPER PB13 +#define BEEPER PB13 #define BEEPER_INVERTED #define USE_SPI @@ -47,12 +47,12 @@ #define GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG +#define GYRO_MPU6500_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG +#define ACC_MPU6500_ALIGN CW270_DEG #define USB_IO @@ -60,26 +60,26 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PC4 -#define UART1_RX_PIN PC5 +#define UART1_TX_PIN PC4 +#define UART1_RX_PIN PC5 -#define UART2_TX_PIN PA14 -#define UART2_RX_PIN PA15 +#define UART2_TX_PIN PA14 +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 #define USE_I2C #define I2C_DEVICE (I2CDEV_2) #define USE_ADC -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PC0 -#define CURRENT_METER_ADC_PIN PC1 -#define RSSI_ADC_PIN PC2 -#define EXTERNAL1_ADC_PIN PC3 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP #define WS2811_PIN PA6 // TIM16_CH1 @@ -97,20 +97,20 @@ #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SPEKTRUM_BIND // USART1, PC5 -#define BIND_PIN PC5 +#define BIND_PIN PC5 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - assuming 303 in 64pin package, TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 958924fabe..092059f5c3 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -96,12 +96,12 @@ #define USE_FLASHFS #define USE_FLASH_M25P16 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA5 -//#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA5 +//#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP #if 1 diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index adca1659a7..a5a91199b2 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -22,19 +22,19 @@ #define BOARD_HAS_VOLTAGE_DIVIDER -#define LED0 PB3 // PB3 (LED) -#define LED1 PB4 // PB4 (LED) +#define LED0 PB3 +#define LED1 PB4 -#define BEEPER PA12 // PA12 (Beeper) +#define BEEPER PA12 #ifdef AFROMINI #define BEEPER_INVERTED #endif -#define BARO_XCLR_PIN PC13 -#define BARO_EOC_PIN PC14 +#define BARO_XCLR_PIN PC13 +#define BARO_EOC_PIN PC14 -#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO -#define INVERTER_USART USART2 +#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO +#define INVERTER_USART USART2 #define USE_EXTI @@ -47,24 +47,22 @@ #define USE_SPI #define USE_SPI_DEVICE_2 -#define NAZE_SPI_INSTANCE SPI2 -#define NAZE_SPI_CS_GPIO GPIOB -#define NAZE_SPI_CS_PIN PB12 +#define NAZE_SPI_INSTANCE SPI2 +#define NAZE_SPI_CS_GPIO GPIOB +#define NAZE_SPI_CS_PIN PB12 #define NAZE_CS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOB // We either have this 16mbit flash chip on SPI or the MPU6500 acc/gyro depending on board revision: -#define M25P16_CS_GPIO NAZE_SPI_CS_GPIO -#define M25P16_CS_PIN NAZE_SPI_CS_PIN -#define M25P16_SPI_INSTANCE NAZE_SPI_INSTANCE - -#define MPU6500_CS_GPIO_CLK_PERIPHERAL NAZE_CS_GPIO_CLK_PERIPHERAL -#define MPU6500_CS_GPIO NAZE_SPI_CS_GPIO -#define MPU6500_CS_PIN NAZE_SPI_CS_PIN -#define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE +#define M25P16_CS_GPIO NAZE_SPI_CS_GPIO +#define M25P16_CS_PIN NAZE_SPI_CS_PIN +#define M25P16_SPI_INSTANCE NAZE_SPI_INSTANCE +#define MPU6500_CS_GPIO_CLK_PERIPHERAL NAZE_CS_GPIO_CLK_PERIPHERAL +#define MPU6500_CS_GPIO NAZE_SPI_CS_GPIO +#define MPU6500_CS_PIN NAZE_SPI_CS_PIN +#define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE #define USE_FLASHFS - #define USE_FLASH_M25P16 #define EXTI_CALLBACK_HANDLER_COUNT 3 // MPU data ready, MAG data ready, BMP085 EOC @@ -82,10 +80,9 @@ #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 - -#define GYRO_MPU3050_ALIGN CW0_DEG -#define GYRO_MPU6050_ALIGN CW0_DEG -#define GYRO_MPU6500_ALIGN CW0_DEG +#define GYRO_MPU3050_ALIGN CW0_DEG +#define GYRO_MPU6050_ALIGN CW0_DEG +#define GYRO_MPU6500_ALIGN CW0_DEG #define ACC #define USE_ACC_ADXL345 @@ -95,11 +92,11 @@ #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_ADXL345_ALIGN CW270_DEG -#define ACC_MPU6050_ALIGN CW0_DEG -#define ACC_MMA8452_ALIGN CW90_DEG -#define ACC_BMA280_ALIGN CW0_DEG -#define ACC_MPU6500_ALIGN CW0_DEG +#define ACC_ADXL345_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW0_DEG +#define ACC_MMA8452_ALIGN CW90_DEG +#define ACC_BMA280_ALIGN CW0_DEG +#define ACC_MPU6500_ALIGN CW0_DEG #define BARO #define USE_BARO_MS5611 @@ -108,8 +105,7 @@ #define MAG #define USE_MAG_HMC5883 - -#define MAG_HMC5883_ALIGN CW180_DEG +#define MAG_HMC5883_ALIGN CW180_DEG #define SONAR #define SONAR_TRIGGER_PIN PB0 @@ -124,7 +120,7 @@ #define USE_UART3 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 5 #define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 @@ -134,8 +130,8 @@ #define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 // USART3 only on NAZE32_SP - Flex Port -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 #define USE_I2C #define I2C_DEVICE (I2CDEV_2) @@ -145,23 +141,23 @@ // #define SOFT_I2C_PB67 #define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA4 -#define RSSI_ADC_PIN PA1 -#define EXTERNAL1_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA4 +#define RSSI_ADC_PIN PA1 +#define EXTERNAL1_ADC_PIN PA5 #define LED_STRIP -#define WS2811_TIMER TIM3 -#define WS2811_PIN PA6 -#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER +#define WS2811_TIMER TIM3 +#define WS2811_PIN PA6 +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER #undef GPS #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 #define USE_SERIAL_4WAY_BLHELI_INTERFACE @@ -186,8 +182,8 @@ #endif // ALIENFLIGHTF1 // IO - assuming all IOs on 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index c526310333..d795a22415 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -79,10 +79,10 @@ #define I2C_DEVICE (I2CDEV_2) #define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA4 -#define RSSI_ADC_PIN PA1 -#define EXTERNAL1_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA4 +#define RSSI_ADC_PIN PA1 +#define EXTERNAL1_ADC_PIN PA5 // IO - assuming all IOs on smt32f103rb LQFP64 package diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 417f889c9f..22e5a396da 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -110,10 +110,10 @@ // #define SOFT_I2C_PB67 #define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA4 -#define RSSI_ADC_PIN PA1 -#define EXTERNAL1_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA4 +#define RSSI_ADC_PIN PA1 +#define EXTERNAL1_ADC_PIN PA5 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 05c58aa979..fcf4ae395f 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -26,27 +26,27 @@ #define USBD_SERIALNUMBER_STRING "0x8020000" #endif -#define LED0 PB5 -#define LED1 PB4 -#define BEEPER PB4 -#define INVERTER PC0 // PC0 used as inverter select GPIO -#define INVERTER_USART USART1 +#define LED0 PB5 +#define LED1 PB4 +#define BEEPER PB4 +#define INVERTER PC0 // PC0 used as inverter select GPIO +#define INVERTER_USART USART1 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_MPU6000_ALIGN CW270_DEG #define GYRO #define USE_GYRO_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_MPU6000_ALIGN CW270_DEG // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PC4 +#define MPU_INT_EXTI PC4 #define USE_EXTI #define MAG @@ -63,8 +63,8 @@ //#define USE_PITOT_MS4525 //#define MS4525_BUS I2C_DEVICE_EXT -#define M25P16_CS_PIN PB3 -#define M25P16_SPI_INSTANCE SPI3 +#define M25P16_CS_PIN PB3 +#define M25P16_SPI_INSTANCE SPI3 #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -72,22 +72,22 @@ #define USABLE_TIMER_CHANNEL_COUNT 12 #define USE_VCP -#define VBUS_SENSING_PIN PC5 +#define VBUS_SENSING_PIN PC5 #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 #define USE_UART6 -#define UART6_RX_PIN PC7 -#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 -#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 +#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 #define USE_SPI @@ -103,22 +103,22 @@ #define I2C_DEVICE (I2CDEV_1) #define USE_ADC -#define CURRENT_METER_ADC_PIN PC1 -#define VBAT_ADC_PIN PC2 -#define RSSI_ADC_GPIO_PIN PA0 +#define CURRENT_METER_ADC_PIN PC1 +#define VBAT_ADC_PIN PC2 +#define RSSI_ADC_GPIO_PIN PA0 #define SENSORS_SET (SENSOR_ACC) -#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125) #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 6e461c5eb2..a298c3ed9d 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -25,24 +25,26 @@ #define USBD_SERIALNUMBER_STRING "0x8010000" #endif -#define LED0 PC14 -#define LED1 PC13 -#define BEEPER PC13 -#define INVERTER PC15 -#define INVERTER_USART USART2 //Sbus on USART 2 of nano. +#define LED0 PC14 +#define LED1 PC13 -#define MPU9250_CS_PIN PB12 -#define MPU9250_SPI_INSTANCE SPI2 +#define BEEPER PC13 + +#define INVERTER PC15 +#define INVERTER_USART USART2 //Sbus on USART 2 of nano. + +#define MPU9250_CS_PIN PB12 +#define MPU9250_SPI_INSTANCE SPI2 #define ACC #define USE_ACC_MPU9250 #define USE_ACC_SPI_MPU9250 -#define ACC_MPU9250_ALIGN CW270_DEG +#define ACC_MPU9250_ALIGN CW270_DEG #define GYRO #define USE_GYRO_MPU9250 #define USE_GYRO_SPI_MPU9250 -#define GYRO_MPU9250_ALIGN CW270_DEG +#define GYRO_MPU9250_ALIGN CW270_DEG //#define MAG //#define USE_MAG_HMC5883 @@ -54,23 +56,23 @@ #define USE_MPU_DATA_READY_SIGNAL //#define ENSURE_MPU_DATA_READY_IS_LOW #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PA15 +#define MPU_INT_EXTI PA15 #define USE_EXTI #define USABLE_TIMER_CHANNEL_COUNT 12 #define USE_VCP -#define VBUS_SENSING_PIN PA9 +#define VBUS_SENSING_PIN PA9 #define USE_UART1 -#define UART1_RX_PIN PB7 -#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 #define USE_UART2 -#define UART2_RX_PIN PA3 -#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 -#define SERIAL_PORT_COUNT 3 //VCP, USART1, USART2 +#define SERIAL_PORT_COUNT 3 //VCP, USART1, USART2 #define USE_SPI //#define USE_SPI_DEVICE_1 @@ -81,9 +83,9 @@ #define I2C_DEVICE (I2CDEV_3) #define USE_ADC -#define CURRENT_METER_ADC_PIN PA7 -#define VBAT_ADC_PIN PA6 -#define RSSI_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PA7 +#define VBAT_ADC_PIN PA6 +#define RSSI_ADC_PIN PA5 #define GPS #define BLACKBOX @@ -92,10 +94,9 @@ #define USE_SERVOS #define USE_CLI -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff - -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) ) diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 14fd760c8e..983c7d55b7 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -88,12 +88,12 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index f82be07689..db0528e984 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -21,30 +21,28 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB4 // Blue (Rev 1 & 2) - PB4 -#define LED1 PB5 // Green (Rev 1) / Red (Rev 2) - PB5 +#define LED0 PB4 // Blue (Rev 1 & 2) - PB4 +#define LED1 PB5 // Green (Rev 1) / Red (Rev 2) - PB5 -#define BEEPER PA1 +#define BEEPER PA1 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 11 // MPU6050 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define MPU_INT_EXTI PA15 #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL // MPU 9150 INT connected to PA15, pulled up to VCC by 10K Resistor, contains MPU6050 and AK8975 in single component. #define GYRO #define USE_GYRO_MPU6050 - -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_MPU6050_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6050 - -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW270_DEG #define BARO #define USE_BARO_MS5611 @@ -53,22 +51,22 @@ #define MAG #define USE_MAG_AK8975 -#define MAG_AK8975_ALIGN CW180_DEG_FLIP +#define MAG_AK8975_ALIGN CW180_DEG_FLIP #define USE_VCP #define USE_UART1 // Conn 1 - TX (PB6) RX PB7 (AF7) #define USE_UART2 // Input - RX (PA3) #define USE_UART3 // Servo out - 10/RX (PB11) 11/TX (PB10) -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PB6 // PB6 -#define UART1_RX_PIN PB7 // PB7 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 -#define UART2_TX_PIN PA2 // PA2 - Clashes with PWM6 input. -#define UART2_RX_PIN PA3 // PA3 +#define UART2_TX_PIN PA2 // PA2 - Clashes with PWM6 input. +#define UART2_RX_PIN PA3 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) // Note: PA5 and PA0 are N/C on the sparky - potentially use for ADC or LED STRIP? @@ -76,11 +74,11 @@ #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) #define USE_ADC -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA7 +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA7 -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define LED_STRIP #if 1 @@ -115,21 +113,19 @@ #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 //#define SONAR -//#define SONAR_ECHO_PIN PB1 -//#define SONAR_TRIGGER_PIN PA2 +//#define SONAR_ECHO_PIN PB1 +//#define SONAR_TRIGGER_PIN PA2 // available IO pins (from schematics) //#define TARGET_IO_PORTA (BIT(1)|BIT(2)|BIT(3)|BIT(4)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(10)|BIT(11)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) //#define TARGET_IO_PORTB (BIT(0)|BIT(1)|BIT(10)|BIT(11)|BIT(14)|BIT(15)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)) // !!TODO - check following lines are correct -#define TARGET_IO_PORTA (BIT(1)|BIT(2)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(10)|BIT(11)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTB (BIT(0)|BIT(1)|BIT(6)|BIT(10)|BIT(11)|BIT(14)|BIT(15)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(12)|BIT(13)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) +#define TARGET_IO_PORTA (BIT(1)|BIT(2)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(10)|BIT(11)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTB (BIT(0)|BIT(1)|BIT(6)|BIT(10)|BIT(11)|BIT(14)|BIT(15)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(12)|BIT(13)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 2380fd1008..c68e8b2b21 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -21,9 +21,9 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0 PB3 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 17 @@ -37,11 +37,11 @@ #define GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_MPU6050_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW270_DEG #define BARO #define USE_BARO_MS5611 @@ -51,37 +51,37 @@ #define MAG #define USE_MAG_AK8975 #define USE_MAG_HMC5883 -#define MAG_HMC5883_ALIGN CW270_DEG +#define MAG_HMC5883_ALIGN CW270_DEG #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH -#define MAG_INT_EXTI PC14 +#define MAG_INT_EXTI PC14 #define USE_FLASHFS #define USE_FLASH_M25P16 #define SONAR -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN PB0 #define USE_UART1 #define USE_UART2 #define USE_UART3 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 5 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 #define SOFTSERIAL_2_TIMER TIM3 @@ -97,12 +97,12 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP @@ -121,16 +121,15 @@ #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) - -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 0bae984252..39ee765f8d 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -117,13 +117,12 @@ #define MPU6500_CS_PIN PB9 #define MPU6500_SPI_INSTANCE SPI1 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP #define USE_LED_STRIP_ON_DMA1_CHANNEL2 diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 8ac7eca05f..b5e9ae0e0f 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -31,12 +31,12 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PE8 // Blue LEDs - PE8/PE12 +#define LED0 PE8 // Blue LEDs - PE8/PE12 #define LED0_INVERTED -#define LED1 PE10 // Orange LEDs - PE10/PE14 +#define LED1 PE10 // Orange LEDs - PE10/PE14 #define LED1_INVERTED -#define BEEPER PE9 // Red LEDs - PE9/PE13 +#define BEEPER PE9 // Red LEDs - PE9/PE13 #define BEEPER_INVERTED #define USE_SPI @@ -50,9 +50,9 @@ //#define USE_SD_CARD // -//#define SD_DETECT_PIN PC14 -//#define SD_CS_PIN PB12 -//#define SD_SPI_INSTANCE SPI2 +//#define SD_DETECT_PIN PC14 +//#define SD_CS_PIN PB12 +//#define SD_SPI_INSTANCE SPI2 //#define USE_FLASHFS //#define USE_FLASH_M25P16 @@ -74,9 +74,9 @@ #define GYRO #define USE_GYRO_L3GD20 -#define L3GD20_SPI SPI1 -#define L3GD20_CS_PIN PE3 -#define GYRO_L3GD20_ALIGN CW270_DEG +#define L3GD20_SPI SPI1 +#define L3GD20_CS_PIN PE3 +#define GYRO_L3GD20_ALIGN CW270_DEG // Support the GY-91 MPU9250 dev board #define USE_GYRO_MPU6500 @@ -92,21 +92,21 @@ #define ACC_MPU6500_ALIGN CW270_DEG_FLIP //#define BARO -//#define BMP280_CS_PIN PB12 -//#define BMP280_SPI_INSTANCE SPI2 +//#define BMP280_CS_PIN PB12 +//#define BMP280_SPI_INSTANCE SPI2 //#define USE_BARO_BMP280 //#define USE_BARO_SPI_BMP280 #define OSD #define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI2 -#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN //#define USE_SDCARD //#define USE_SDCARD_SPI2 // -//#define SDCARD_SPI_INSTANCE SPI2 -//#define SDCARD_SPI_CS_PIN PB12 +//#define SDCARD_SPI_INSTANCE SPI2 +//#define SDCARD_SPI_CS_PIN PB12 //// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: //#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 //// Divide to under 25MHz for normal operation: @@ -125,7 +125,7 @@ #define USE_VCP #define USE_UART1 #define USE_UART2 -#define SERIAL_PORT_COUNT 3 +#define SERIAL_PORT_COUNT 3 // uart2 gpio for shared serial rx/ppm //#define UART2_TX_PIN GPIO_Pin_5 // PD5 @@ -139,11 +139,11 @@ #define I2C_DEVICE (I2CDEV_1) #define USE_ADC -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PC0 -#define CURRENT_METER_ADC_PIN PC1 -#define RSSI_ADC_PIN PC2 -#define EXTERNAL1_ADC_PIN PC3 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP #define WS2811_PIN PB8 // TIM16_CH1 @@ -153,19 +153,18 @@ #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define LED_STRIP -#define LED_STRIP_TIMER TIM16 +#define LED_STRIP_TIMER TIM16 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - 303 in 100pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTE 0xffff -#define TARGET_IO_PORTF 0x00ff +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0x00ff -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) From 89b288a49818e314c90c953c11ff3477a056a680 Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 15 Jul 2016 22:49:24 +1000 Subject: [PATCH 030/456] Fix for F4 telemetry --- src/main/drivers/dma.h | 6 ++++-- src/main/drivers/dma_stm32f4xx.c | 4 ++-- src/main/drivers/serial_uart_stm32f4xx.c | 1 + src/main/target/BLUEJAYF4/target.h | 1 - 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index 4f0dabdfff..422b4c45a4 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -22,13 +22,15 @@ typedef void (*dmaCallbackHandlerFuncPtr)(struct dmaChannelDescriptor_s *channel #ifdef STM32F4 typedef enum { - DMA1_ST1_HANDLER = 0, + DMA1_ST0_HANDLER = 0, + DMA1_ST1_HANDLER, DMA1_ST2_HANDLER, DMA1_ST3_HANDLER, DMA1_ST4_HANDLER, DMA1_ST5_HANDLER, DMA1_ST6_HANDLER, DMA1_ST7_HANDLER, + DMA2_ST0_HANDLER, DMA2_ST1_HANDLER, DMA2_ST2_HANDLER, DMA2_ST3_HANDLER, @@ -63,7 +65,7 @@ typedef struct dmaChannelDescriptor_s { #define DMA_IT_TEIF ((uint32_t)0x00000008) #define DMA_IT_DMEIF ((uint32_t)0x00000004) #define DMA_IT_FEIF ((uint32_t)0x00000001) - + #else typedef enum { diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index ce960ab4c2..9ee7486371 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -45,7 +45,6 @@ static dmaChannelDescriptor_t dmaDescriptors[] = { DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream5, 38, DMA2_Stream5_IRQn, RCC_AHB1Periph_DMA2), DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream6, 48, DMA2_Stream6_IRQn, RCC_AHB1Periph_DMA2), DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream7, 54, DMA2_Stream7_IRQn, RCC_AHB1Periph_DMA2), - }; /* @@ -63,7 +62,8 @@ DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_ST2_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_ST3_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_ST4_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_ST5_HANDLER) - +DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_ST6_HANDLER) +DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_ST7_HANDLER) void dmaInit(void) { diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index 97f087a510..3adecfc892 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -376,6 +376,7 @@ void USART1_IRQHandler(void) uartPort_t *s = &(uartHardwareMap[UARTDEV_1]->port); uartIrqHandler(s); } + #endif #ifdef USE_UART2 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 6bad2fee16..ffae24c925 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -98,7 +98,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 #define UART3_RX_PIN PB11 From a0d7d92883d1e6cd336fa9d331102b186d70d1f9 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Fri, 15 Jul 2016 14:05:38 +0100 Subject: [PATCH 031/456] Fix for ccache detection problem on cygwin. --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 90a221d2a7..281b169c63 100644 --- a/Makefile +++ b/Makefile @@ -548,8 +548,8 @@ VPATH := $(VPATH):$(STDPERIPH_DIR)/src # Find out if ccache is installed on the system CCACHE := ccache -RESULT = $(shell which $(CCACHE)) -ifeq ($(RESULT),) +RESULT = $(shell (which $(CCACHE) > /dev/null 2>&1; echo $$?) ) +ifneq ($(RESULT),0) CCACHE := endif From c8d25ac7cfe4beab8f2a762497358eea1f2612dc Mon Sep 17 00:00:00 2001 From: JOhn Aughey Date: Fri, 15 Jul 2016 09:37:15 -0500 Subject: [PATCH 032/456] Removing the error condition if the processor isn't known. --- src/main/drivers/bus_spi.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index dbba94881c..5a6ea7dd37 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -31,8 +31,6 @@ #define SPI_IO_AF_MOSI_CFG IO_CONFIG(GPIO_Mode_AF_PP, GPIO_Speed_50MHz) #define SPI_IO_AF_MISO_CFG IO_CONFIG(GPIO_Mode_IN_FLOATING, GPIO_Speed_50MHz) #define SPI_IO_CS_CFG IO_CONFIG(GPIO_Mode_Out_PP, GPIO_Speed_50MHz) -#else -#error "Unknown processor" #endif /* From 81dd00298ae1f61ad9fe9e037cf32cad2e1bbf0a Mon Sep 17 00:00:00 2001 From: JOhn Aughey Date: Fri, 15 Jul 2016 09:37:30 -0500 Subject: [PATCH 033/456] Adding fake gyro/acc values for the new design --- src/main/sensors/initialisation.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 3ba1f439a2..908606c364 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -118,9 +118,15 @@ static bool fakeGyroReadTemp(int16_t *tempData) return true; } + +static bool fakeGyroInitStatus(void) { + return true; +} + bool fakeGyroDetect(gyro_t *gyro) { gyro->init = fakeGyroInit; + gyro->intStatus = fakeGyroInitStatus; gyro->read = fakeGyroRead; gyro->temperature = fakeGyroReadTemp; gyro->scale = 1.0f / 16.4f; @@ -143,6 +149,7 @@ bool fakeAccDetect(acc_t *acc) { acc->init = fakeAccInit; acc->read = fakeAccRead; + acc->acc_1G = 512*8; acc->revisionCode = 0; return true; } From a9fc299840662fa7429b50494b30a9814eb66858 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 15 Jul 2016 18:02:53 +0100 Subject: [PATCH 034/456] Whitespace tidy of target.h files --- src/main/target/DOGE/target.h | 48 +++++----- src/main/target/EUSTM32F103RC/target.h | 43 +++++---- src/main/target/F4BY/target.h | 112 ++++++++++++------------ src/main/target/FURYF3/target.h | 73 ++++++++------- src/main/target/FURYF4/target.h | 75 ++++++++-------- src/main/target/IRCFUSIONF3/target.h | 43 +++++---- src/main/target/KISSFC/target.h | 53 ++++++----- src/main/target/MOTOLAB/target.h | 62 ++++++------- src/main/target/PIKOBLX/target.h | 65 +++++++------- src/main/target/REVO/target.h | 8 +- src/main/target/RMDO/target.h | 44 +++++----- src/main/target/SPRACINGF3EVO/target.h | 43 +++++---- src/main/target/SPRACINGF3MINI/target.h | 70 +++++++-------- 13 files changed, 364 insertions(+), 375 deletions(-) diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index e4c2f76ba0..8f57af76d7 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -22,15 +22,13 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT // tqfp48 pin 34 -#define LED0 PA13 - +#define LED0 PA13 // tqfp48 pin 37 -#define LED1 PA14 - +#define LED1 PA14 // tqfp48 pin 38 -#define LED2 PA15 +#define LED2 PA15 -#define BEEPER PB2 +#define BEEPER PB2 #define BEEPER_INVERTED #define USE_SPI @@ -81,13 +79,13 @@ // #define USE_FAKE_GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG // ?? +#define GYRO_MPU6500_ALIGN CW270_DEG // ?? #define ACC // #define USE_FAKE_ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG // ?? +#define ACC_MPU6500_ALIGN CW270_DEG // ?? #define BARO #define USE_BARO_BMP280 @@ -100,20 +98,20 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PB6 -#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 -#define UART2_TX_PIN PA2 -#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 // mpu_int definition in sensors/initialisation.c #define USE_EXTI @@ -135,19 +133,19 @@ #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SPEKTRUM_BIND // Use UART3 for speksat -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // !!TODO - check the TARGET_IO_PORTs are correct -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 747b9b03bb..87c531b109 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -19,21 +19,21 @@ #define TARGET_BOARD_IDENTIFIER "EUF1" -#define LED0 PB3 // PB3 (LED) -#define LED1 PB4 // PB4 (LED) +#define LED0 PB3 +#define LED1 PB4 -#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO +#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO #define INVERTER_USART USART2 #define USE_EXTI -#define MPU6000_CS_GPIO GPIOB -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define MPU6000_CS_GPIO GPIOB +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_INSTANCE SPI2 #define MPU6500_CS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOB -#define MPU6500_CS_GPIO GPIOB -#define MPU6500_CS_PIN PB12 -#define MPU6500_SPI_INSTANCE SPI2 +#define MPU6500_CS_GPIO GPIOB +#define MPU6500_CS_PIN PB12 +#define MPU6500_SPI_INSTANCE SPI2 #define GYRO #define USE_FAKE_GYRO @@ -44,7 +44,7 @@ #define USE_GYRO_SPI_MPU6000 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6050_ALIGN CW0_DEG +#define GYRO_MPU6050_ALIGN CW0_DEG #define ACC #define USE_FAKE_ACC @@ -55,7 +55,7 @@ //#define USE_ACC_SPI_MPU6000 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6050_ALIGN CW0_DEG +#define ACC_MPU6050_ALIGN CW0_DEG #define BARO #define USE_BARO_MS5611 @@ -66,7 +66,7 @@ #define USE_MAG_HMC5883 #define USE_MAG_AK8975 -#define MAG_AK8975_ALIGN CW180_DEG_FLIP +#define MAG_AK8975_ALIGN CW180_DEG_FLIP #define SONAR #define SONAR_TRIGGER_PIN PB0 @@ -80,12 +80,12 @@ #define USE_UART2 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER TIM3 #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 #define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 @@ -104,14 +104,13 @@ #define SPEKTRUM_BIND // USART2, PA3 -#define BIND_PIN PA3 +#define BIND_PIN PA3 // IO - stm32f103RCT6 in 64pin package (TODO) -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 127a33d4bb..d78895ec84 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -20,46 +20,47 @@ #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define USBD_PRODUCT_STRING "Swift-Flyer F4BY" +#define USBD_PRODUCT_STRING "Swift-Flyer F4BY" -#define LED0 PE3 // Blue LED -#define LED1 PE2 // Red LED -#define LED2 PE1 // Blue LED -#define BEEPER PE5 +#define LED0 PE3 // Blue LED +#define LED1 PE2 // Red LED +#define LED2 PE1 // Blue LED -#define INVERTER PD3 -#define INVERTER_USART USART6 +#define BEEPER PE5 + +#define INVERTER PD3 +#define INVERTER_USART USART6 // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PB0 +#define MPU_INT_EXTI PB0 #define USE_EXTI -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG +#define ACC_MPU6000_ALIGN CW90_DEG #define GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG +#define GYRO_MPU6000_ALIGN CW90_DEG #define MAG #define USE_MAG_HMC5883 -#define MAG_HMC5883_ALIGN CW90_DEG +#define MAG_HMC5883_ALIGN CW90_DEG #define BARO #define USE_BARO_MS5611 #define USE_SDCARD -#define SDCARD_SPI_INSTANCE SPI2 -#define SDCARD_SPI_CS_PIN PE15 +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN PE15 // SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz @@ -76,43 +77,41 @@ #define USABLE_TIMER_CHANNEL_COUNT 17 #define USE_VCP -#define VBUS_SENSING_PIN PA9 +#define VBUS_SENSING_PIN PA9 #define USE_UART1 -#define UART1_RX_PIN PB7 -#define UART1_TX_PIN PB6 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART2 -#define UART2_RX_PIN PD6 -#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 +#define UART2_TX_PIN PD5 #define USE_UART3 -#define UART3_RX_PIN PD9 -#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 +#define UART3_TX_PIN PD8 #define USE_UART4 -#define UART4_RX_PIN PC11 -#define UART4_TX_PIN PC10 +#define UART4_RX_PIN PC11 +#define UART4_TX_PIN PC10 //#define USE_UART5 - error in DMA!!! -//#define UART5_RX_PIN PD2 -//#define UART5_TX_PIN PC12 +//#define UART5_RX_PIN PD2 +//#define UART5_TX_PIN PC12 #define USE_UART6 -#define UART6_RX_PIN PC7 -#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 - -#define SERIAL_PORT_COUNT 6 //VCP, UART1, UART2, UART3, UART4, UART5, UART6 +#define SERIAL_PORT_COUNT 6 //VCP, UART1, UART2, UART3, UART4, UART5, UART6 #define USE_SPI - #define USE_SPI_DEVICE_1 -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 -#define SPI1_NSS_PIN NONE +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define SPI1_NSS_PIN NONE #define USE_SPI_DEVICE_2 #define SPI2_NSS_PIN NONE @@ -121,42 +120,41 @@ #define SPI2_MOSI_PIN PB15 #define USE_SPI_DEVICE_3 -#define SPI3_NSS_PIN PA15 -#define SPI3_SCK_PIN PB3 -#define SPI3_MISO_PIN PB4 -#define SPI3_MOSI_PIN PB5 - +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) +#define I2C_DEVICE (I2CDEV_2) #define USE_I2C_PULLUP -#define I2C2_SCL PB10 -#define I2C2_SDA PB11 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define VBAT_ADC_PIN PC3 -#define CURRENT_METER_ADC_PIN PC2 -#define RSSI_ADC_PIN PC1 +#define VBAT_ADC_PIN PC3 +#define CURRENT_METER_ADC_PIN PC2 +#define RSSI_ADC_PIN PC1 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) -#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define SPEKTRUM_BIND // UART6, PC7 -#define BIND_PIN PC7 +#define BIND_PIN PC7 #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff -#define USED_TIMERS ( TIM_N(1) |TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(1) |TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index e67a920cc6..4ee1a3835a 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -21,13 +21,13 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define MPU_INT_EXTI PC4 +#define MPU_INT_EXTI PC4 #define USE_EXTI #define CONFIG_PREFER_ACC_ON -#define LED0 PC14 +#define LED0 PC14 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect @@ -36,26 +36,25 @@ #define ENSURE_MPU_DATA_READY_IS_LOW #define GYRO -#define ACC -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 - -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_INSTANCE SPI1 #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_MPU6000_ALIGN CW180_DEG #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW90_DEG +#define GYRO_MPU6500_ALIGN CW90_DEG +#define ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW90_DEG +#define ACC_MPU6500_ALIGN CW90_DEG #define BARO #define USE_BARO_MS5611 @@ -113,31 +112,31 @@ #define USE_SOFTSERIAL1 #define SERIAL_PORT_COUNT 5 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 1 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 2 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // SDA (PB9/AF4), SCL (PB8/AF4) +#define I2C_DEVICE (I2CDEV_1) // SDA (PB9/AF4), SCL (PB8/AF4) -#define I2C1_SCL_PIN PB8 -#define I2C1_SDA_PIN PB9 +#define I2C1_SCL_PIN PB8 +#define I2C1_SDA_PIN PB9 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PA0 -#define RSSI_ADC_PIN PA1 -#define CURRENT_METER_ADC_PIN PA2 +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PA0 +#define RSSI_ADC_PIN PA1 +#define CURRENT_METER_ADC_PIN PA2 #define LED_STRIP @@ -150,23 +149,23 @@ #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER #define SONAR -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN PB0 #define DEFAULT_FEATURES FEATURE_BLACKBOX #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTF (BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(16) |TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index b9631d6124..c6263bcd47 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -20,44 +20,45 @@ #define TARGET_BOARD_IDENTIFIER "FYF4" #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define USBD_PRODUCT_STRING "FuryF4" +#define USBD_PRODUCT_STRING "FuryF4" -#define LED0 PB5 -#define LED1 PB4 -#define BEEPER PA8 +#define LED0 PB5 +#define LED1 PB4 + +#define BEEPER PA8 #define BEEPER_INVERTED #define INVERTER PC0 // PC0 used as inverter select GPIO #define INVERTER_USART USART1 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_INSTANCE SPI1 #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_MPU6000_ALIGN CW180_DEG #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PC4 +#define MPU_INT_EXTI PC4 #define USE_EXTI #define BARO #define USE_BARO_MS5611 -#define MS5611_I2C_INSTANCE I2CDEV_1 +#define MS5611_I2C_INSTANCE I2CDEV_1 #define USE_SDCARD @@ -97,29 +98,29 @@ #define USE_FLASHFS #define USE_FLASH_M25P16 -#define M25P16_CS_PIN PB3 -#define M25P16_SPI_INSTANCE SPI3 +#define M25P16_CS_PIN PB3 +#define M25P16_SPI_INSTANCE SPI3 #define USABLE_TIMER_CHANNEL_COUNT 5 #define USE_VCP -#define VBUS_SENSING_PIN PC5 +#define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED #define USE_UART1 -#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 #define USE_UART6 -#define UART6_RX_PIN PC7 -#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 -#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 +#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 #define USE_SPI @@ -138,16 +139,16 @@ #define SPI3_MOSI_PIN PC12 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6-SCL, PB7-SDA +#define I2C_DEVICE (I2CDEV_1) // PB6-SCL, PB7-SDA #define USE_I2C_PULLUP -#define I2C1_SCL PB6 -#define I2C1_SDA PB7 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 #define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define VBAT_ADC_PIN PC1 -#define RSSI_ADC_GPIO_PIN PC2 -#define CURRENT_METER_ADC_PIN PC3 +#define VBAT_ADC_PIN PC1 +#define RSSI_ADC_GPIO_PIN PC2 +#define CURRENT_METER_ADC_PIN PC3 #define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL @@ -157,14 +158,14 @@ #define SPEKTRUM_BIND // USART3 Rx, PB11 -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 8e41030edf..8dbd6e55ba 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -21,7 +21,7 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0 PB3 #define USABLE_TIMER_CHANNEL_COUNT 17 @@ -32,11 +32,11 @@ #define GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_MPU6050_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW270_DEG #define BARO #define USE_BARO_BMP085 @@ -49,17 +49,17 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 3 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 @@ -67,16 +67,16 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE /* @@ -100,11 +100,10 @@ */ // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index de059f7b46..3c5f4cd5d7 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -23,51 +23,50 @@ #define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN | SERIAL_INVERTED | SERIAL_BIDIR) -#define LED0 PB1 -#define BEEPER PB13 +#define LED0 PB1 + +#define BEEPER PB13 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 12 #define USE_EXTI -#define MPU_INT_EXTI PB2 +#define MPU_INT_EXTI PB2 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW180_DEG +#define GYRO_MPU6050_ALIGN CW180_DEG #define ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW180_DEG - +#define ACC_MPU6050_ALIGN CW180_DEG #define USE_VCP #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART2_TX_PIN PB3 +#define UART2_RX_PIN PB4 -#define UART2_TX_PIN PB3 -#define UART2_RX_PIN PB4 - -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_ADC -#define VBAT_SCALE_DEFAULT 160 -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PA0 -//#define CURRENT_METER_ADC_PIN PA5 -//#define RSSI_ADC_PIN PB2 +#define VBAT_SCALE_DEFAULT 160 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PA0 +//#define CURRENT_METER_ADC_PIN PA5 +//#define RSSI_ADC_PIN PB2 #define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL @@ -75,12 +74,12 @@ #define SERIALRX_UART SERIAL_PORT_USART2 #define SPEKTRUM_BIND -#define BIND_PIN PB4 +#define BIND_PIN PB4 -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTF (BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 092059f5c3..1414ae6be3 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -22,17 +22,17 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB5 // Blue LEDs - PB5 -//#define LED1 PB9 // Green LEDs - PB9 +#define LED0 PB5 // Blue LEDs - PB5 +//#define LED1 PB9 // Green LEDs - PB9 -#define BEEPER PA0 +#define BEEPER PA0 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 9 // MPU6050 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define MPU_INT_EXTI PA15 #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL //#define ENSURE_MPU_DATA_READY_IS_LOW @@ -41,20 +41,20 @@ #define ACC #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW180_DEG +#define GYRO_MPU6050_ALIGN CW180_DEG #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW180_DEG +#define ACC_MPU6050_ALIGN CW180_DEG #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define ACC_MPU6000_ALIGN CW180_DEG -#define MPU6000_CS_GPIO GPIOB -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define MPU6000_CS_GPIO GPIOB +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_INSTANCE SPI2 //#define BARO //#define USE_BARO_MS5611 @@ -66,30 +66,30 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PB6 // PB6 -#define UART1_RX_PIN PB7 // PB7 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 -#define UART2_TX_PIN PB3 // PB3 -#define UART2_RX_PIN PB4 // PB4 +#define UART2_TX_PIN PB3 +#define UART2_RX_PIN PB4 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) -#define I2C2_SCL PA9 -#define I2C2_SDA PA10 +#define I2C_DEVICE (I2CDEV_2) +#define I2C2_SCL PA9 +#define I2C2_SDA PA10 #define USE_SPI #define USE_SPI_DEVICE_2 -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_INSTANCE SPI2 +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 -//#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) -#define SENSORS_SET (SENSOR_ACC) +//#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) +#define SENSORS_SET (SENSOR_ACC) #undef GPS #define DISPLAY @@ -135,17 +135,17 @@ #define SPEKTRUM_BIND // USART2, PB4 -#define BIND_PIN PB4 +#define BIND_PIN PB4 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) // #define TARGET_IO_PORTF (BIT(0)|BIT(1)) // !!TODO - check the following line is correct -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 938af8f98b..bb6e33717b 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -22,63 +22,63 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB9 -#define LED1 PB5 +#define LED0 PB9 +#define LED1 PB5 -#define BEEPER PA0 +#define BEEPER PA0 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 9 // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PA15 +#define MPU_INT_EXTI PA15 #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL #define GYRO -#define ACC - #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW180_DEG -#define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG -#define MPU6000_CS_GPIO GPIOB -#define MPU6000_CS_PIN PB12 -#define MPU6000_SPI_INSTANCE SPI2 +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG + +#define MPU6000_CS_GPIO GPIOB +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_INSTANCE SPI2 #define USE_VCP #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PB6 // PB6 -#define UART1_RX_PIN PB7 // PB7 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 -#define UART2_TX_PIN PB3 // PB3 -#define UART2_RX_PIN PB4 // PB4 +#define UART2_TX_PIN PB3 +#define UART2_RX_PIN PB4 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_SPI #define USE_SPI_DEVICE_2 -#define SENSORS_SET (SENSOR_ACC) +#define SENSORS_SET (SENSOR_ACC) #define TELEMETRY #define BLACKBOX #define SERIAL_RX #define USE_SERVOS -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define CURRENT_METER_ADC_PIN PA2 -#define VBAT_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define CURRENT_METER_ADC_PIN PA2 +#define VBAT_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP #if 1 @@ -124,16 +124,17 @@ #define SPEKTRUM_BIND // USART3, PB11 -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -// #define TARGET_IO_PORTF (BIT(0)|BIT(1)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +// #define TARGET_IO_PORTF (BIT(0)|BIT(1)) // !!TODO - check the following line is correct -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index fcf4ae395f..db1464f3d0 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -21,7 +21,7 @@ #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) -#define USBD_PRODUCT_STRING "Revolution" +#define USBD_PRODUCT_STRING "Revolution" #ifdef OPBL #define USBD_SERIALNUMBER_STRING "0x8020000" #endif @@ -51,10 +51,10 @@ #define MAG #define USE_MAG_HMC5883 -#define MAG_HMC5883_ALIGN CW90_DEG +#define MAG_HMC5883_ALIGN CW90_DEG //#define USE_MAG_NAZA -//#define MAG_NAZA_ALIGN CW180_DEG_FLIP +//#define MAG_NAZA_ALIGN CW180_DEG_FLIP #define BARO #define USE_BARO_MS5611 @@ -121,4 +121,4 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff -#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) ) diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 983c7d55b7..fadbdb874b 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -21,9 +21,9 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0 PB3 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 17 @@ -39,11 +39,11 @@ #define GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW270_DEG +#define GYRO_MPU6050_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW270_DEG #define BARO #define USE_BARO_BMP280 @@ -52,34 +52,34 @@ #define USE_FLASH_M25P16 #define SONAR -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN PB0 #define USE_UART1 #define USE_UART2 #define USE_UART3 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 5 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 // PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER TIM3 #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 #define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 @@ -109,15 +109,15 @@ #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 39ee765f8d..5075367764 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -21,9 +21,9 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB8 +#define LED0 PB8 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 12 // PPM, 8 PWM, UART3 RX/TX, LED Strip @@ -31,7 +31,7 @@ #define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -47,8 +47,8 @@ #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG -#define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG #define BARO #define USE_BARO_BMP280 @@ -68,19 +68,19 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) @@ -148,21 +148,20 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index d76558a9c2..799da79d47 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -24,9 +24,9 @@ // early prototype had slightly different pin mappings. //#define SPRACINGF3MINI_MKII_REVA -#define LED0 PB3 +#define LED0 PB3 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 12 // 8 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins. @@ -34,24 +34,22 @@ #define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH - #define GYRO //#define USE_FAKE_GYRO #define USE_GYRO_MPU6500 +#define GYRO_MPU6500_ALIGN CW180_DEG #define ACC //#define USE_FAKE_ACC #define USE_ACC_MPU6500 - -#define ACC_MPU6500_ALIGN CW180_DEG -#define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG #define BARO #define USE_BARO_BMP280 @@ -60,40 +58,39 @@ #define USE_MPU9250_MAG // Enables bypass configuration #define USE_MAG_AK8975 #define USE_MAG_HMC5883 // External - -#define MAG_AK8975_ALIGN CW90_DEG_FLIP +#define MAG_AK8975_ALIGN CW90_DEG_FLIP #define SONAR -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN PB0 #define USB_IO #define USB_CABLE_DETECTION -#define USB_DETECT_PIN PB5 +#define USB_DETECT_PIN PB5 #define USE_VCP #define USE_UART1 #define USE_UART2 #define USE_UART3 #define USE_SOFTSERIAL1 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 5 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define SOFTSERIAL_1_TIMER TIM2 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 9 // PA0 / PAD3 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 10 // PA1 / PAD4 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 @@ -124,10 +121,8 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - - +#define USE_ADC #define ADC_INSTANCE ADC2 #define VBAT_ADC_PIN PA4 #define CURRENT_METER_ADC_PIN PA5 @@ -141,7 +136,6 @@ #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER - #define TRANSPONDER #define TRANSPONDER_GPIO GPIOA #define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA @@ -158,27 +152,29 @@ #define REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES FEATURE_BLACKBOX + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define BUTTONS -#define BUTTON_A_PORT GPIOB -#define BUTTON_A_PIN Pin_1 -#define BUTTON_B_PORT GPIOB -#define BUTTON_B_PIN Pin_0 +#define BUTTON_A_PORT GPIOB +#define BUTTON_A_PIN Pin_1 +#define BUTTON_B_PORT GPIOB +#define BUTTON_B_PIN Pin_0 #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define HARDWARE_BIND_PLUG -#define BINDPLUG_PIN PB0 +#define BINDPLUG_PIN PB0 #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) From 6c63aa78cabd18754d2f773d17a7ee5dd5d2108b Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Fri, 15 Jul 2016 19:18:11 +0200 Subject: [PATCH 035/456] Additional fixes after feedback --- src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 4d1fdb9364..c5b2eea77c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2502,7 +2502,7 @@ static void cliName(char *cmdline) { uint32_t len = strlen(cmdline); - if (*cmdline == 0) { + if (len == 0) { cliPrintf("name %s\r\n", masterConfig.name); } else if ('-' == cmdline[0]) { memset(masterConfig.name, '\0', MAX_NAME_LENGTH); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 2db9951554..2feb08b052 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1878,7 +1878,7 @@ static bool processInCommand(void) break; case MSP_SET_NAME: - memset(masterConfig.name, '\0', MAX_NAME_LENGTH); + memset(masterConfig.name, 0, MAX_NAME_LENGTH+1); for (i = 0; i < MIN(MAX_NAME_LENGTH, currentPort->dataSize); i++) { masterConfig.name[i] = read8(); } From d65f7a03837da4e4e5827ed29e5e5e268a20f2f0 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 16 Jul 2016 12:38:06 +1000 Subject: [PATCH 036/456] Minor updates to naming the craft --- src/main/blackbox/blackbox.c | 1 + src/main/io/serial_cli.c | 12 +++--------- src/main/io/serial_msp.c | 5 +---- 3 files changed, 5 insertions(+), 13 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index c529f4132b..4081d75923 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1140,6 +1140,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s", "Cleanflight"); BLACKBOX_PRINT_HEADER_LINE("Firmware revision:Betaflight %s (%s) %s", FC_VERSION_STRING, shortGitRevision, targetName); BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime); + BLACKBOX_PRINT_HEADER_LINE("Craft name:%s", masterConfig.name); BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle); BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.escAndServoConfig.maxthrottle); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5a19d2f994..aa6b563a51 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2500,18 +2500,12 @@ static void cliMotor(char *cmdline) static void cliName(char *cmdline) { - uint32_t len = strlen(cmdline); - if (len == 0) { - cliPrintf("name %s\r\n", masterConfig.name); - } else if ('-' == cmdline[0]) { - memset(masterConfig.name, '\0', MAX_NAME_LENGTH); - cliPrintf("name removed\r\n"); - } else { - memset(masterConfig.name, '\0', MAX_NAME_LENGTH); + if (len > 0) { + memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); - cliPrintf("name %s\r\n", masterConfig.name); } + cliPrintf("name %s\r\n", strlen(masterConfig.name) > 0 ? masterConfig.name : "-"); return; } diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 2feb08b052..f28c68a605 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1878,13 +1878,10 @@ static bool processInCommand(void) break; case MSP_SET_NAME: - memset(masterConfig.name, 0, MAX_NAME_LENGTH+1); + memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); for (i = 0; i < MIN(MAX_NAME_LENGTH, currentPort->dataSize); i++) { masterConfig.name[i] = read8(); } - if (masterConfig.name[0] == '-') { - memset(masterConfig.name, '\0', MAX_NAME_LENGTH); - } break; default: // we do not know how to handle the (valid) message, indicate error MSP $M! From 141b369667d425f7739f8f754b57156ad684362c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 16 Jul 2016 07:54:58 +0100 Subject: [PATCH 037/456] Removed trailing whitespace --- src/main/blackbox/blackbox.c | 4 +- src/main/blackbox/blackbox_io.c | 2 +- src/main/common/maths.c | 4 +- src/main/common/printf.c | 2 +- src/main/config/config.c | 10 +-- src/main/config/config_master.h | 6 +- src/main/drivers/accgyro_mpu.c | 2 +- src/main/drivers/accgyro_mpu.h | 4 +- src/main/drivers/accgyro_mpu6050.c | 2 +- src/main/drivers/accgyro_spi_mpu6000.c | 4 +- src/main/drivers/accgyro_spi_mpu9250.c | 8 +- src/main/drivers/adc_impl.h | 6 +- src/main/drivers/adc_stm32f10x.c | 10 +-- src/main/drivers/adc_stm32f30x.c | 12 +-- src/main/drivers/adc_stm32f4xx.c | 12 +-- src/main/drivers/barometer_bmp085.c | 8 +- src/main/drivers/barometer_bmp085.h | 2 +- src/main/drivers/bus_i2c.h | 2 +- src/main/drivers/bus_i2c_stm32f10x.c | 44 +++++------ src/main/drivers/bus_i2c_stm32f30x.c | 24 +++--- src/main/drivers/bus_spi.c | 4 +- src/main/drivers/dma.h | 2 +- src/main/drivers/exti.c | 20 ++--- src/main/drivers/flash_m25p16.c | 2 +- src/main/drivers/inverter.c | 2 +- src/main/drivers/io.h | 2 +- src/main/drivers/io_impl.h | 4 +- src/main/drivers/light_led.c | 4 +- .../drivers/light_ws2811strip_stm32f4xx.c | 4 +- src/main/drivers/max7456.c | 2 +- src/main/drivers/pwm_mapping.c | 4 +- src/main/drivers/pwm_mapping.h | 2 +- src/main/drivers/pwm_output.c | 2 +- src/main/drivers/sdcard.c | 4 +- src/main/drivers/serial_uart_stm32f30x.c | 4 +- src/main/drivers/serial_uart_stm32f4xx.c | 74 +++++++++---------- src/main/drivers/system_stm32f10x.c | 2 +- src/main/drivers/system_stm32f30x.c | 2 +- src/main/drivers/system_stm32f4xx.c | 2 +- src/main/drivers/timer.c | 2 +- src/main/drivers/timer.h | 2 +- src/main/drivers/timer_stm32f10x.c | 4 +- src/main/drivers/timer_stm32f30x.c | 2 +- src/main/drivers/usb_io.c | 2 +- src/main/drivers/vtx_rtc6705.c | 2 +- src/main/flight/pid.c | 4 +- src/main/io/gps.c | 2 +- src/main/io/rc_controls.c | 4 +- src/main/io/serial.c | 2 +- src/main/io/serial_4way_stk500v2.c | 2 +- src/main/io/serial_cli.c | 18 ++--- src/main/io/serial_msp.c | 4 +- src/main/io/vtx.h | 2 +- src/main/main.c | 2 +- src/main/rx/xbus.c | 4 +- src/main/sensors/battery.c | 2 +- src/main/sensors/initialisation.c | 2 +- src/main/target/AIR32/target.h | 2 +- .../target/ALIENFLIGHTF3/hardware_revision.h | 2 +- src/main/target/BLUEJAYF4/target.c | 2 +- src/main/target/BLUEJAYF4/target.h | 2 +- src/main/target/CHEBUZZF3/target.h | 4 +- src/main/target/CJMCU/hardware_revision.h | 2 +- src/main/target/COLIBRI_RACE/target.h | 2 +- src/main/target/DOGE/target.c | 2 +- src/main/target/DOGE/target.h | 2 +- src/main/target/F4BY/target.c | 18 ++--- src/main/target/FURYF3/target.c | 2 +- src/main/target/FURYF3/target.h | 2 +- src/main/target/LUX_RACE/target.h | 2 +- src/main/target/MOTOLAB/target.h | 2 +- src/main/target/NAZE/target.h | 2 +- src/main/target/REVO/target.c | 2 +- src/main/target/REVO/target.h | 2 +- src/main/target/SPRACINGF3EVO/target.h | 2 +- src/main/target/ZCOREF3/target.h | 2 +- src/main/target/common.h | 2 +- src/main/target/system_stm32f30x.c | 72 +++++++++--------- src/main/target/system_stm32f30x.h | 18 ++--- src/main/target/system_stm32f4xx.h | 14 ++-- src/main/vcp/hw_config.c | 14 ++-- src/main/vcp/platform_config.h | 4 +- src/main/vcp/stm32_it.h | 4 +- src/main/vcp/usb_conf.h | 4 +- src/main/vcp/usb_desc.c | 4 +- src/main/vcp/usb_desc.h | 4 +- src/main/vcp/usb_istr.h | 4 +- src/main/vcp/usb_prop.c | 4 +- src/main/vcp/usb_prop.h | 4 +- src/main/vcpf4/stm32f4xx_it.h | 6 +- src/main/vcpf4/usb_conf.h | 68 ++++++++--------- src/main/vcpf4/usbd_cdc_vcp.c | 22 +++--- src/main/vcpf4/usbd_cdc_vcp.h | 2 +- src/main/vcpf4/usbd_conf.h | 18 ++--- src/main/vcpf4/usbd_desc.c | 74 +++++++++---------- src/main/vcpf4/usbd_desc.h | 40 +++++----- src/main/vcpf4/usbd_usr.c | 20 ++--- 97 files changed, 415 insertions(+), 415 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4081d75923..d97cb39a75 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -632,7 +632,7 @@ static void writeInterframe(void) arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT); blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); - /* + /* * The PID I field changes very slowly, most of the time +-2, so use an encoding * that can pack all three fields into one byte in that situation. */ @@ -1315,7 +1315,7 @@ static void blackboxCheckAndLogFlightMode() } } -/* +/* * Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control * the portion of logged loop iterations. */ diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 7f9b1ec5bd..cd4f05d980 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -507,7 +507,7 @@ void blackboxWriteFloat(float value) /** * If there is data waiting to be written to the blackbox device, attempt to write (a portion of) that now. - * + * * Intended to be called regularly for the blackbox device to perform housekeeping. */ void blackboxDeviceFlush(void) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 888847e075..52990c6ac8 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -231,7 +231,7 @@ int32_t quickMedianFilter5(int32_t * v) QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[0], p[3]); QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[2]); QMF_SORT(p[2], p[3]); - QMF_SORT(p[1], p[2]); + QMF_SORT(p[1], p[2]); return p[2]; } @@ -279,7 +279,7 @@ float quickMedianFilter5f(float * v) QMF_SORTF(p[0], p[1]); QMF_SORTF(p[3], p[4]); QMF_SORTF(p[0], p[3]); QMF_SORTF(p[1], p[4]); QMF_SORTF(p[1], p[2]); QMF_SORTF(p[2], p[3]); - QMF_SORTF(p[1], p[2]); + QMF_SORTF(p[1], p[2]); return p[2]; } diff --git a/src/main/common/printf.c b/src/main/common/printf.c index 7847fb3035..e11d49d151 100644 --- a/src/main/common/printf.c +++ b/src/main/common/printf.c @@ -56,7 +56,7 @@ typedef void (*putcf) (void *, char); static putcf stdout_putf; static void *stdout_putp; -// print bf, padded from left to at least n characters. +// print bf, padded from left to at least n characters. // padding is zero ('0') if z!=0, space (' ') otherwise static int putchw(void *putp, putcf putf, int n, char z, char *bf) { diff --git a/src/main/config/config.c b/src/main/config/config.c index 7841ef9328..3cdc4f82aa 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -159,7 +159,7 @@ size_t custom_flash_memory_address = 0; #define CONFIG_START_FLASH_ADDRESS (custom_flash_memory_address) #else // use the last flash pages for storage -#ifndef CONFIG_START_FLASH_ADDRESS +#ifndef CONFIG_START_FLASH_ADDRESS #define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) #endif #endif @@ -349,7 +349,7 @@ void resetSerialConfig(serialConfig_t *serialConfig) serialConfig->reboot_character = 'R'; } -static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) +static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcRate8 = 100; controlRateConfig->rcYawRate8 = 100; @@ -366,7 +366,7 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) } -void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) +void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { rcControlsConfig->deadband = 0; rcControlsConfig->yaw_deadband = 0; @@ -374,7 +374,7 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) rcControlsConfig->alt_hold_fast_change = 1; } -void resetMixerConfig(mixerConfig_t *mixerConfig) +void resetMixerConfig(mixerConfig_t *mixerConfig) { mixerConfig->yaw_motor_direction = 1; #ifdef USE_SERVOS @@ -658,7 +658,7 @@ static void resetConf(void) targetConfiguration(); #endif - + // copy first profile into remaining profile for (int i = 1; i < MAX_PROFILE_COUNT; i++) { memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 4f8f4903fb..6632ca3afd 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -137,7 +137,7 @@ typedef struct master_t { profile_t profile[MAX_PROFILE_COUNT]; uint8_t current_profile_index; - + modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT]; @@ -161,9 +161,9 @@ typedef struct master_t { uint8_t magic_ef; // magic number, should be 0xEF uint8_t chk; // XOR checksum - + char name[MAX_NAME_LENGTH+1]; - + } master_t; extern master_t masterConfig; diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 9a3cd57253..1c705ef137 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -252,7 +252,7 @@ void mpuIntExtiInit(void) EXTIEnable(mpuIntIO, true); #endif - mpuExtiInitDone = true; + mpuExtiInitDone = true; } static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data) diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 917c06f1a8..132c9e13a0 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -18,7 +18,7 @@ #pragma once #include "exti.h" - + // MPU6050 #define MPU_RA_WHO_AM_I 0x75 #define MPU_RA_WHO_AM_I_LEGACY 0x00 @@ -119,7 +119,7 @@ typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data); typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data); -typedef void(*mpuResetFuncPtr)(void); +typedef void(*mpuResetFuncPtr)(void); typedef struct mpuConfiguration_s { uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 3cc3ac8f69..78e8b8e801 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -104,7 +104,7 @@ static void mpu6050GyroInit(uint8_t lpf) delay(100); ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) - delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure + delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure ack = mpuConfiguration.write(MPU_RA_CONFIG, lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index b483380912..dc21901e7a 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -158,7 +158,7 @@ bool mpu6000SpiDetect(void) uint8_t in; uint8_t attemptsRemaining = 5; -#ifdef MPU6000_CS_PIN +#ifdef MPU6000_CS_PIN mpuSpi6000CsPin = IOGetByTag(IO_TAG(MPU6000_CS_PIN)); #endif IOInit(mpuSpi6000CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0); @@ -253,7 +253,7 @@ static void mpu6000AccAndGyroInit(void) { delayMicroseconds(15); #endif - spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); delayMicroseconds(1); mpuSpi6000InitDone = true; diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index b5beca2946..0406e6023c 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -54,7 +54,7 @@ static IO_t mpuSpi9250CsPin = IO_NONE; #define DISABLE_MPU9250 IOHi(mpuSpi9250CsPin) #define ENABLE_MPU9250 IOLo(mpuSpi9250CsPin) -void mpu9250ResetGyro(void) +void mpu9250ResetGyro(void) { // Device Reset mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); @@ -123,7 +123,7 @@ void mpu9250SpiAccInit(acc_t *acc) acc->acc_1G = 512 * 8; } -bool verifympu9250WriteRegister(uint8_t reg, uint8_t data) +bool verifympu9250WriteRegister(uint8_t reg, uint8_t data) { uint8_t in; uint8_t attemptsRemaining = 20; @@ -176,7 +176,7 @@ static void mpu9250AccAndGyroInit(uint8_t lpf) { verifympu9250WriteRegister(MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. #endif - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); mpuSpi9250InitDone = true; //init done } @@ -208,7 +208,7 @@ bool mpu9250SpiDetect(void) } } while (attemptsRemaining--); - spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); + spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); return true; } diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 258c8acc8d..146ada62be 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -24,14 +24,14 @@ #define ADC_TAG_MAP_COUNT 16 #elif defined(STM32F3) #define ADC_TAG_MAP_COUNT 39 -#else +#else #define ADC_TAG_MAP_COUNT 10 -#endif +#endif typedef enum ADCDevice { ADCINVALID = -1, ADCDEV_1 = 0, -#if defined(STM32F3) +#if defined(STM32F3) ADCDEV_2, ADCDEV_MAX = ADCDEV_2, #elif defined(STM32F4) diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index bc23c2b96c..3104bcd5e9 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -35,13 +35,13 @@ #define ADC_INSTANCE ADC1 #endif -const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 } +const adcDevice_t adcHardware[] = { + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 } }; ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) { - if (instance == ADC1) + if (instance == ADC1) return ADCDEV_1; /* TODO -- ADC2 available on large 10x devices. @@ -51,7 +51,7 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) return ADCINVALID; } -const adcTagMap_t adcTagMap[] = { +const adcTagMap_t adcTagMap[] = { { DEFIO_TAG_E__PA0, ADC_Channel_0 }, // ADC12 { DEFIO_TAG_E__PA1, ADC_Channel_1 }, // ADC12 { DEFIO_TAG_E__PA2, ADC_Channel_2 }, // ADC12 @@ -61,7 +61,7 @@ const adcTagMap_t adcTagMap[] = { { DEFIO_TAG_E__PA6, ADC_Channel_6 }, // ADC12 { DEFIO_TAG_E__PA7, ADC_Channel_7 }, // ADC12 { DEFIO_TAG_E__PB0, ADC_Channel_8 }, // ADC12 - { DEFIO_TAG_E__PB1, ADC_Channel_9 }, // ADC12 + { DEFIO_TAG_E__PB1, ADC_Channel_9 }, // ADC12 }; // Driver for STM32F103CB onboard ADC diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 5cbe50607c..f58a8086d6 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -37,12 +37,12 @@ #define ADC_INSTANCE ADC1 #endif -const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 }, - { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA2), .DMAy_Channelx = DMA2_Channel1 } +const adcDevice_t adcHardware[] = { + { .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA1), .DMAy_Channelx = DMA1_Channel1 }, + { .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .rccDMA = RCC_AHB(DMA2), .DMAy_Channelx = DMA2_Channel1 } }; -const adcTagMap_t adcTagMap[] = { +const adcTagMap_t adcTagMap[] = { { DEFIO_TAG_E__PA0, ADC_Channel_1 }, // ADC1 { DEFIO_TAG_E__PA1, ADC_Channel_2 }, // ADC1 { DEFIO_TAG_E__PA2, ADC_Channel_3 }, // ADC1 @@ -86,7 +86,7 @@ const adcTagMap_t adcTagMap[] = { ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) { - if (instance == ADC1) + if (instance == ADC1) return ADCDEV_1; if (instance == ADC2) @@ -133,7 +133,7 @@ void adcInit(drv_adc_config_t *init) if (device == ADCINVALID) return; - adcDevice_t adc = adcHardware[device]; + adcDevice_t adc = adcHardware[device]; for (int i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].tag) diff --git a/src/main/drivers/adc_stm32f4xx.c b/src/main/drivers/adc_stm32f4xx.c index 197ff12999..968f05a67e 100644 --- a/src/main/drivers/adc_stm32f4xx.c +++ b/src/main/drivers/adc_stm32f4xx.c @@ -36,13 +36,13 @@ #define ADC_INSTANCE ADC1 #endif -const adcDevice_t adcHardware[] = { - { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 }, - //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } +const adcDevice_t adcHardware[] = { + { .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 }, + //{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 } }; /* note these could be packed up for saving space */ -const adcTagMap_t adcTagMap[] = { +const adcTagMap_t adcTagMap[] = { /* { DEFIO_TAG_E__PF3, ADC_Channel_9 }, { DEFIO_TAG_E__PF4, ADC_Channel_14 }, @@ -73,7 +73,7 @@ const adcTagMap_t adcTagMap[] = { ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) { - if (instance == ADC1) + if (instance == ADC1) return ADCDEV_1; /* if (instance == ADC2) // TODO add ADC2 and 3 @@ -126,7 +126,7 @@ void adcInit(drv_adc_config_t *init) if (device == ADCINVALID) return; - adcDevice_t adc = adcHardware[device]; + adcDevice_t adc = adcHardware[device]; for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].tag) diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c index 4c3e779855..9b35646b6b 100644 --- a/src/main/drivers/barometer_bmp085.c +++ b/src/main/drivers/barometer_bmp085.c @@ -35,7 +35,7 @@ #ifdef BARO -#if defined(BARO_EOC_GPIO) +#if defined(BARO_EOC_GPIO) static IO_t eocIO; @@ -49,7 +49,7 @@ void bmp085_extiHandler(extiCallbackRec_t* cb) isConversionComplete = true; } -bool bmp085TestEOCConnected(const bmp085Config_t *config); +bool bmp085TestEOCConnected(const bmp085Config_t *config); # endif typedef struct { @@ -139,7 +139,7 @@ static IO_t xclrIO; #endif -void bmp085InitXclrIO(const bmp085Config_t *config) +void bmp085InitXclrIO(const bmp085Config_t *config) { if (!xclrIO && config && config->xclrIO) { xclrIO = IOGetByTag(config->xclrIO); @@ -184,7 +184,7 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro) delay(20); // datasheet says 10ms, we'll be careful and do 20. - ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */ + ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */ if (ack) { bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID); bmp085.oversampling_setting = 3; diff --git a/src/main/drivers/barometer_bmp085.h b/src/main/drivers/barometer_bmp085.h index 328bffb134..ffe863d1ba 100644 --- a/src/main/drivers/barometer_bmp085.h +++ b/src/main/drivers/barometer_bmp085.h @@ -19,7 +19,7 @@ typedef struct bmp085Config_s { ioTag_t xclrIO; - ioTag_t eocIO; + ioTag_t eocIO; } bmp085Config_t; bool bmp085Detect(const bmp085Config_t *config, baro_t *baro); diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index bdfc75dfa9..86f7a66181 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -26,7 +26,7 @@ #ifndef I2C_DEVICE #define I2C_DEVICE I2CINVALID -#endif +#endif typedef enum I2CDevice { I2CINVALID = -1, diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index 74a9f36559..7daea6fa95 100644 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -47,35 +47,35 @@ static void i2cUnstick(IO_t scl, IO_t sda); #define IOCFG_I2C IOCFG_AF_OD #endif -#ifndef I2C1_SCL -#define I2C1_SCL PB8 +#ifndef I2C1_SCL +#define I2C1_SCL PB8 #endif -#ifndef I2C1_SDA -#define I2C1_SDA PB9 +#ifndef I2C1_SDA +#define I2C1_SDA PB9 #endif #else -#ifndef I2C1_SCL -#define I2C1_SCL PB6 +#ifndef I2C1_SCL +#define I2C1_SCL PB6 #endif -#ifndef I2C1_SDA -#define I2C1_SDA PB7 +#ifndef I2C1_SDA +#define I2C1_SDA PB7 #endif #endif -#ifndef I2C2_SCL -#define I2C2_SCL PB10 +#ifndef I2C2_SCL +#define I2C2_SCL PB10 #endif -#ifndef I2C2_SDA +#ifndef I2C2_SDA #define I2C2_SDA PB11 #endif #ifdef STM32F4 -#ifndef I2C3_SCL +#ifndef I2C3_SCL #define I2C3_SCL PA8 #endif -#ifndef I2C3_SDA -#define I2C3_SDA PB4 +#ifndef I2C3_SDA +#define I2C3_SDA PB4 #endif #endif @@ -93,7 +93,7 @@ static i2cState_t i2cState[] = { { false, false, 0, 0, 0, 0, 0, 0, 0 }, { false, false, 0, 0, 0, 0, 0, 0, 0 }, { false, false, 0, 0, 0, 0, 0, 0, 0 } -}; +}; void I2C1_ER_IRQHandler(void) { i2c_er_handler(I2CDEV_1); @@ -121,7 +121,7 @@ void I2C3_EV_IRQHandler(void) { } #endif -static bool i2cHandleHardwareFailure(I2CDevice device) +static bool i2cHandleHardwareFailure(I2CDevice device) { i2cErrorCount++; // reinit peripheral + clock out garbage @@ -129,7 +129,7 @@ static bool i2cHandleHardwareFailure(I2CDevice device) return false; } -bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) +bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) { if (device == I2CINVALID) @@ -138,7 +138,7 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint32_t timeout = I2C_DEFAULT_TIMEOUT; I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; + I2Cx = i2cHardwareMap[device].dev; i2cState_t *state; state = &(i2cState[device]); @@ -171,12 +171,12 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, return !(state->error); } -bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) +bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data) { return i2cWriteBuffer(device, addr_, reg_, 1, &data); } -bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) { if (device == I2CINVALID) return false; @@ -369,7 +369,7 @@ void i2c_ev_handler(I2CDevice device) { } } -void i2cInit(I2CDevice device) +void i2cInit(I2CDevice device) { if (device == I2CINVALID) return; @@ -392,7 +392,7 @@ void i2cInit(I2CDevice device) I2C_ITConfig(i2c->dev, I2C_IT_EVT | I2C_IT_ERR, DISABLE); i2cUnstick(scl, sda); - + // Init pins #ifdef STM32F4 IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_I2C); diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index d645a70504..524df1bc86 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -39,18 +39,18 @@ #define I2C_SHORT_TIMEOUT ((uint32_t)0x1000) #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) -#define I2C_GPIO_AF GPIO_AF_4 +#define I2C_GPIO_AF GPIO_AF_4 -#ifndef I2C1_SCL -#define I2C1_SCL PB6 +#ifndef I2C1_SCL +#define I2C1_SCL PB6 #endif -#ifndef I2C1_SDA -#define I2C1_SDA PB7 +#ifndef I2C1_SDA +#define I2C1_SDA PB7 #endif -#ifndef I2C2_SCL -#define I2C2_SCL PF4 +#ifndef I2C2_SCL +#define I2C2_SCL PF4 #endif -#ifndef I2C2_SDA +#ifndef I2C2_SDA #define I2C2_SDA PA10 #endif @@ -82,7 +82,7 @@ void i2cInit(I2CDevice device) I2C_TypeDef *I2Cx; I2Cx = i2c->dev; - + IO_t scl = IOGetByTag(i2c->scl); IO_t sda = IOGetByTag(i2c->sda); @@ -108,7 +108,7 @@ void i2cInit(I2CDevice device) I2C_Init(I2Cx, &i2cInit); I2C_StretchClockCmd(I2Cx, ENABLE); - + I2C_Cmd(I2Cx, ENABLE); } @@ -122,7 +122,7 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) addr_ <<= 1; I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; + I2Cx = i2cHardwareMap[device].dev; /* Test on BUSY Flag */ i2cTimeout = I2C_LONG_TIMEOUT; @@ -188,7 +188,7 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* addr_ <<= 1; I2C_TypeDef *I2Cx; - I2Cx = i2cHardwareMap[device].dev; + I2Cx = i2cHardwareMap[device].dev; /* Test on BUSY Flag */ i2cTimeout = I2C_LONG_TIMEOUT; diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 005f7936b6..690f25b52e 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -36,7 +36,7 @@ #ifndef GPIO_AF_SPI3 #define GPIO_AF_SPI3 GPIO_AF_6 #endif -#endif +#endif #ifndef SPI1_SCK_PIN #define SPI1_NSS_PIN PA4 @@ -84,7 +84,7 @@ static spiDevice_t spiHardwareMap[] = { SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) { - if (instance == SPI1) + if (instance == SPI1) return SPIDEV_1; if (instance == SPI2) diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index 422b4c45a4..726a5a1807 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -65,7 +65,7 @@ typedef struct dmaChannelDescriptor_s { #define DMA_IT_TEIF ((uint32_t)0x00000008) #define DMA_IT_DMEIF ((uint32_t)0x00000004) #define DMA_IT_FEIF ((uint32_t)0x00000001) - + #else typedef enum { diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c index cd97ba9b74..ce665bb077 100644 --- a/src/main/drivers/exti.c +++ b/src/main/drivers/exti.c @@ -24,22 +24,22 @@ static uint8_t extiGroupPriority[EXTI_IRQ_GROUPS]; #if defined(STM32F1) || defined(STM32F4) static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = { - EXTI0_IRQn, - EXTI1_IRQn, - EXTI2_IRQn, - EXTI3_IRQn, + EXTI0_IRQn, + EXTI1_IRQn, + EXTI2_IRQn, + EXTI3_IRQn, EXTI4_IRQn, - EXTI9_5_IRQn, + EXTI9_5_IRQn, EXTI15_10_IRQn }; #elif defined(STM32F3) static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = { - EXTI0_IRQn, - EXTI1_IRQn, - EXTI2_TS_IRQn, - EXTI3_IRQn, + EXTI0_IRQn, + EXTI1_IRQn, + EXTI2_TS_IRQn, + EXTI3_IRQn, EXTI4_IRQn, - EXTI9_5_IRQn, + EXTI9_5_IRQn, EXTI15_10_IRQn }; #else diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 806c777dab..29620fbe11 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -199,7 +199,7 @@ static bool m25p16_readIdentification() bool m25p16_init() { -#ifdef M25P16_CS_PIN +#ifdef M25P16_CS_PIN m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN)); #endif IOInit(m25p16CsPin, OWNER_FLASH, RESOURCE_SPI_CS, 0); diff --git a/src/main/drivers/inverter.c b/src/main/drivers/inverter.c index fbbff6ffd4..967c87680a 100644 --- a/src/main/drivers/inverter.c +++ b/src/main/drivers/inverter.c @@ -20,7 +20,7 @@ #include "platform.h" -#ifdef INVERTER +#ifdef INVERTER #include "io.h" #include "io_impl.h" diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index 07c53a2032..af9deca810 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -52,7 +52,7 @@ typedef uint8_t ioConfig_t; // packed IO configuration #define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5)) #define IOCFG_OUT_PP IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) // TODO -#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_25MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) +#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_25MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) #define IOCFG_OUT_OD IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL) #define IOCFG_AF_PP IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) #define IOCFG_AF_PP_PD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_DOWN) diff --git a/src/main/drivers/io_impl.h b/src/main/drivers/io_impl.h index 1b3cca2bcf..0c880fd78d 100644 --- a/src/main/drivers/io_impl.h +++ b/src/main/drivers/io_impl.h @@ -12,7 +12,7 @@ typedef struct ioRec_s { GPIO_TypeDef *gpio; uint16_t pin; resourceOwner_t owner; - resourceType_t resource; + resourceType_t resource; uint8_t index; } ioRec_t; @@ -26,7 +26,7 @@ int IO_GPIO_PortSource(IO_t io); #if defined(STM32F3) || defined(STM32F4) int IO_EXTI_PortSourceGPIO(IO_t io); -int IO_EXTI_PinSource(IO_t io); +int IO_EXTI_PinSource(IO_t io); #endif GPIO_TypeDef* IO_GPIO(IO_t io); diff --git a/src/main/drivers/light_led.c b/src/main/drivers/light_led.c index 8b1bdc5ab4..a6b2a6892e 100644 --- a/src/main/drivers/light_led.c +++ b/src/main/drivers/light_led.c @@ -27,12 +27,12 @@ static const IO_t leds[] = { DEFIO_IO(LED0), #else DEFIO_IO(NONE), -#endif +#endif #ifdef LED1 DEFIO_IO(LED1), #else DEFIO_IO(NONE), -#endif +#endif #ifdef LED2 DEFIO_IO(LED2), #else diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index 3a888bd21b..377954ce99 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -31,8 +31,8 @@ #ifdef LED_STRIP -#if !defined(WS2811_PIN) -#define WS2811_PIN PA0 +#if !defined(WS2811_PIN) +#define WS2811_PIN PA0 #define WS2811_TIMER TIM5 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER #define WS2811_DMA_STREAM DMA1_Stream2 diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index b9f521273c..cfa2e95e2d 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -147,7 +147,7 @@ void max7456_dma_irq_handler(dmaChannelDescriptor_t* descriptor) { } #endif -void max7456_init(uint8_t video_system) +void max7456_init(uint8_t video_system) { uint8_t max_screen_rows; uint8_t srdata = 0; diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 59e1167a27..d6f41808cb 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -95,7 +95,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) int channelIndex = 0; memset(&pwmOutputConfiguration, 0, sizeof(pwmOutputConfiguration)); - + // this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ] if (init->airplane) i = 2; // switch to air hardware config @@ -260,7 +260,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) if (init->useChannelForwarding && !init->airplane) { #if defined(NAZE) && defined(WS2811_TIMER) // if LED strip is active, PWM5-8 are unavailable, so map AUX1+AUX2 to PWM13+PWM14 - if (init->useLEDStrip) { + if (init->useLEDStrip) { if (timerIndex >= PWM13 && timerIndex <= PWM14) { type = MAP_TO_SERVO_OUTPUT; } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index ffd16e74d6..5a3f402e97 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -117,7 +117,7 @@ enum { PWM13, PWM14, PWM15, - PWM16, + PWM16, PWM17, PWM18, PWM19, diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index c5ebfcd176..ecd0447a9a 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -131,7 +131,7 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 p->tim = timerHardware->tim; *p->ccr = 0; - + return p; } diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index d940ddc857..878d5eacf6 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -126,7 +126,7 @@ void sdcardInsertionDetectDeinit(void) #ifdef SDCARD_DETECT_PIN sdCardDetectPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN)); IOInit(sdCardDetectPin, OWNER_FREE, RESOURCE_NONE, 0); - IOConfigGPIO(sdCardDetectPin, IOCFG_IN_FLOATING); + IOConfigGPIO(sdCardDetectPin, IOCFG_IN_FLOATING); #endif } @@ -135,7 +135,7 @@ void sdcardInsertionDetectInit(void) #ifdef SDCARD_DETECT_PIN sdCardDetectPin = IOGetByTag(IO_TAG(SDCARD_DETECT_PIN)); IOInit(sdCardDetectPin, OWNER_SDCARD, RESOURCE_INPUT, 0); - IOConfigGPIO(sdCardDetectPin, IOCFG_IPU); + IOConfigGPIO(sdCardDetectPin, IOCFG_IPU); #endif } diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 09b6663e86..852a60831e 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -114,8 +114,8 @@ static void handleUsartTxDma(dmaChannelDescriptor_t* descriptor) void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, uint8_t af, uint8_t index) { if (options & SERIAL_BIDIR) { - ioConfig_t ioCfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, - (options & SERIAL_INVERTED) ? GPIO_OType_PP : GPIO_OType_OD, + ioConfig_t ioCfg = IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, + (options & SERIAL_INVERTED) ? GPIO_OType_PP : GPIO_OType_OD, (options & SERIAL_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP ); diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index 3adecfc892..f79a99ae6e 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -64,17 +64,17 @@ typedef struct uartDevice_s { //static uartPort_t uartPort[MAX_UARTS]; #ifdef USE_UART1 -static uartDevice_t uart1 = -{ +static uartDevice_t uart1 = +{ .DMAChannel = DMA_Channel_4, .txDMAStream = DMA2_Stream7, #ifdef USE_UART1_RX_DMA .rxDMAStream = DMA2_Stream5, #endif - .dev = USART1, - .rx = IO_TAG(UART1_RX_PIN), - .tx = IO_TAG(UART1_TX_PIN), - .af = GPIO_AF_USART1, + .dev = USART1, + .rx = IO_TAG(UART1_RX_PIN), + .tx = IO_TAG(UART1_TX_PIN), + .af = GPIO_AF_USART1, #ifdef UART1_AHB1_PERIPHERALS .rcc_ahb1 = UART1_AHB1_PERIPHERALS, #endif @@ -87,17 +87,17 @@ static uartDevice_t uart1 = #endif #ifdef USE_UART2 -static uartDevice_t uart2 = -{ +static uartDevice_t uart2 = +{ .DMAChannel = DMA_Channel_4, #ifdef USE_UART2_RX_DMA .rxDMAStream = DMA1_Stream5, #endif .txDMAStream = DMA1_Stream6, - .dev = USART2, - .rx = IO_TAG(UART2_RX_PIN), - .tx = IO_TAG(UART2_TX_PIN), - .af = GPIO_AF_USART2, + .dev = USART2, + .rx = IO_TAG(UART2_RX_PIN), + .tx = IO_TAG(UART2_TX_PIN), + .af = GPIO_AF_USART2, #ifdef UART2_AHB1_PERIPHERALS .rcc_ahb1 = UART2_AHB1_PERIPHERALS, #endif @@ -110,17 +110,17 @@ static uartDevice_t uart2 = #endif #ifdef USE_UART3 -static uartDevice_t uart3 = -{ +static uartDevice_t uart3 = +{ .DMAChannel = DMA_Channel_4, #ifdef USE_UART3_RX_DMA .rxDMAStream = DMA1_Stream1, #endif .txDMAStream = DMA1_Stream3, - .dev = USART3, - .rx = IO_TAG(UART3_RX_PIN), - .tx = IO_TAG(UART3_TX_PIN), - .af = GPIO_AF_USART3, + .dev = USART3, + .rx = IO_TAG(UART3_RX_PIN), + .tx = IO_TAG(UART3_TX_PIN), + .af = GPIO_AF_USART3, #ifdef UART3_AHB1_PERIPHERALS .rcc_ahb1 = UART3_AHB1_PERIPHERALS, #endif @@ -133,17 +133,17 @@ static uartDevice_t uart3 = #endif #ifdef USE_UART4 -static uartDevice_t uart4 = -{ +static uartDevice_t uart4 = +{ .DMAChannel = DMA_Channel_4, #ifdef USE_UART1_RX_DMA .rxDMAStream = DMA1_Stream2, #endif .txDMAStream = DMA1_Stream4, - .dev = UART4, - .rx = IO_TAG(UART4_RX_PIN), - .tx = IO_TAG(UART4_TX_PIN), - .af = GPIO_AF_UART4, + .dev = UART4, + .rx = IO_TAG(UART4_RX_PIN), + .tx = IO_TAG(UART4_TX_PIN), + .af = GPIO_AF_UART4, #ifdef UART4_AHB1_PERIPHERALS .rcc_ahb1 = UART4_AHB1_PERIPHERALS, #endif @@ -156,17 +156,17 @@ static uartDevice_t uart4 = #endif #ifdef USE_UART5 -static uartDevice_t uart5 = -{ +static uartDevice_t uart5 = +{ .DMAChannel = DMA_Channel_4, #ifdef USE_UART1_RX_DMA .rxDMAStream = DMA1_Stream0, #endif .txDMAStream = DMA2_Stream7, - .dev = UART5, - .rx = IO_TAG(UART5_RX_PIN), - .tx = IO_TAG(UART5_TX_PIN), - .af = GPIO_AF_UART5, + .dev = UART5, + .rx = IO_TAG(UART5_RX_PIN), + .tx = IO_TAG(UART5_TX_PIN), + .af = GPIO_AF_UART5, #ifdef UART5_AHB1_PERIPHERALS .rcc_ahb1 = UART5_AHB1_PERIPHERALS, #endif @@ -179,17 +179,17 @@ static uartDevice_t uart5 = #endif #ifdef USE_UART6 -static uartDevice_t uart6 = -{ +static uartDevice_t uart6 = +{ .DMAChannel = DMA_Channel_5, #ifdef USE_UART6_RX_DMA .rxDMAStream = DMA2_Stream1, #endif .txDMAStream = DMA2_Stream6, - .dev = USART6, - .rx = IO_TAG(UART6_RX_PIN), - .tx = IO_TAG(UART6_TX_PIN), - .af = GPIO_AF_USART6, + .dev = USART6, + .rx = IO_TAG(UART6_RX_PIN), + .tx = IO_TAG(UART6_TX_PIN), + .af = GPIO_AF_USART6, #ifdef UART6_AHB1_PERIPHERALS .rcc_ahb1 = UART6_AHB1_PERIPHERALS, #endif @@ -344,7 +344,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af); } - if (mode & MODE_RX) { + if (mode & MODE_RX) { IOInit(rx, OWNER_SERIAL, RESOURCE_UART_RX, RESOURCE_INDEX(device)); IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af); } diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index f5eab63fac..3e33c545b8 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -36,7 +36,7 @@ void systemReset(void) SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } -void systemResetToBootloader(void) +void systemResetToBootloader(void) { // 1FFFF000 -> 20000200 -> SP // 1FFFF004 -> 1FFFF021 -> PC diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index e0aaad1bb0..fb30936968 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -34,7 +34,7 @@ void systemReset(void) SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } -void systemResetToBootloader(void) +void systemResetToBootloader(void) { // 1FFFF000 -> 20000200 -> SP // 1FFFF004 -> 1FFFF021 -> PC diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 8101974377..5c09ebdf6a 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -41,7 +41,7 @@ void systemReset(void) NVIC_SystemReset(); } -void systemResetToBootloader(void) +void systemResetToBootloader(void) { if (mpuConfiguration.reset) mpuConfiguration.reset(); diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index a3b3624371..15e36cce3f 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -253,7 +253,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) } #else TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1; -#endif +#endif TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 26078d4517..e11f5a8974 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -87,7 +87,7 @@ typedef struct timerHardware_s { } timerHardware_t; enum { - TIMER_OUTPUT_ENABLED = 0x01, + TIMER_OUTPUT_ENABLED = 0x01, TIMER_OUTPUT_INVERTED = 0x02 }; diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index 09ef0efc8c..1a5085e63a 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -23,10 +23,10 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { { .TIMx = TIM8, .rcc = RCC_APB1(TIM8) }, { .TIMx = TIM9, .rcc = RCC_APB2(TIM9) }, { .TIMx = TIM10, .rcc = RCC_APB2(TIM10) }, - { .TIMx = TIM11, .rcc = RCC_APB2(TIM11) }, + { .TIMx = TIM11, .rcc = RCC_APB2(TIM11) }, { .TIMx = TIM12, .rcc = RCC_APB1(TIM12) }, { .TIMx = TIM13, .rcc = RCC_APB1(TIM13) }, - { .TIMx = TIM14, .rcc = RCC_APB1(TIM14) }, + { .TIMx = TIM14, .rcc = RCC_APB1(TIM14) }, #endif }; diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 033aa316c4..04069b20c3 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -54,7 +54,7 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { */ #define CCMR_OFFSET ((uint16_t)0x0018) #define CCMR_OC13M_MASK ((uint32_t)0xFFFEFF8F) -#define CCMR_OC24M_MASK ((uint32_t)0xFEFF8FFF) +#define CCMR_OC24M_MASK ((uint32_t)0xFEFF8FFF) void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t TIM_OCMode) { diff --git a/src/main/drivers/usb_io.c b/src/main/drivers/usb_io.c index d8a7206109..8520ef8e54 100644 --- a/src/main/drivers/usb_io.c +++ b/src/main/drivers/usb_io.c @@ -66,7 +66,7 @@ bool usbCableIsInserted(void) void usbGenerateDisconnectPulse(void) { /* Pull down PA12 to create USB disconnect pulse */ - IO_t usbPin = IOGetByTag(IO_TAG(PA12)); + IO_t usbPin = IOGetByTag(IO_TAG(PA12)); IOConfigGPIO(usbPin, IOCFG_OUT_OD); IOHi(usbPin); diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index ee540da60e..93ffa83ecb 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -179,7 +179,7 @@ void rtc6705SetFreq(uint16_t freq) uint32_t val_a = ((((uint64_t)freq*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) % RTC6705_SET_FDIV) / RTC6705_SET_NDIV; //Casts required to make sure correct math (large numbers) uint32_t val_n = (((uint64_t)freq*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) / RTC6705_SET_FDIV; //Casts required to make sure correct math (large numbers) - + val_hex |= RTC6705_SET_WRITE; val_hex |= (val_a << 5); val_hex |= (val_n << 12); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b0dee87496..25ca7c40aa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -71,12 +71,12 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we using -void setTargetPidLooptime(uint8_t pidProcessDenom) +void setTargetPidLooptime(uint8_t pidProcessDenom) { targetPidLooptime = gyro.targetLooptime * pidProcessDenom; } -uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) +uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) { uint16_t dynamicKi; uint16_t resetRate; diff --git a/src/main/io/gps.c b/src/main/io/gps.c index cf8e141aec..af963adcdc 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -1069,7 +1069,7 @@ static void gpsHandlePassthrough(uint8_t data) #endif } - + void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort) { diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 68896a9a89..f354577f87 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -672,8 +672,8 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rx applyStepAdjustment(controlRateConfig, adjustmentFunction, delta); } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { - uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); - uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; + uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); + uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth; applySelectAdjustment(adjustmentFunction, position); } diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 890e218280..15aae3e469 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -465,7 +465,7 @@ static void nopConsumer(uint8_t data) arbitrary serial passthrough "proxy". Optional callbacks can be given to allow for specialized data processing. */ -void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer +void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer *leftC, serialConsumer *rightC) { waitForSerialPortToFinishTransmitting(left); diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c index 9c81c89317..1c32e2c6bb 100644 --- a/src/main/io/serial_4way_stk500v2.c +++ b/src/main/io/serial_4way_stk500v2.c @@ -16,7 +16,7 @@ * Author: 4712 * have a look at https://github.com/sim-/tgy/blob/master/boot.inc * for info about the stk500v2 implementation - */ + */ #include #include #include diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index aa6b563a51..9954843894 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -112,7 +112,7 @@ static void cliRxFail(char *cmdline); static void cliAdjustmentRange(char *cmdline); static void cliMotorMix(char *cmdline); static void cliDefaults(char *cmdline); -void cliDfu(char *cmdLine); +void cliDfu(char *cmdLine); static void cliDump(char *cmdLine); void cliDumpProfile(uint8_t profileIndex); void cliDumpRateProfile(uint8_t rateProfileIndex) ; @@ -488,7 +488,7 @@ typedef enum { TABLE_GPS_SBAS_MODE, #endif #ifdef BLACKBOX - TABLE_BLACKBOX_DEVICE, + TABLE_BLACKBOX_DEVICE, #endif TABLE_CURRENT_SENSOR, TABLE_GIMBAL_MODE, @@ -1943,7 +1943,7 @@ static void cliDump(char *cmdline) dumpMask = DUMP_PROFILE; // only } if (strcasecmp(cmdline, "rates") == 0) { - dumpMask = DUMP_RATES; + dumpMask = DUMP_RATES; } if (strcasecmp(cmdline, "all") == 0) { @@ -2502,11 +2502,11 @@ static void cliName(char *cmdline) { uint32_t len = strlen(cmdline); if (len > 0) { - memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); - strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); + memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); + strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); } cliPrintf("name %s\r\n", strlen(masterConfig.name) > 0 ? masterConfig.name : "-"); - + return; } @@ -2697,7 +2697,7 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full) break; } } -static void cliPrintVarRange(const clivalue_t *var) +static void cliPrintVarRange(const clivalue_t *var) { switch (var->type & VALUE_MODE_MASK) { case (MODE_DIRECT): { @@ -2709,7 +2709,7 @@ static void cliPrintVarRange(const clivalue_t *var) cliPrint("Allowed values:"); uint8_t i; for (i = 0; i < tableEntry->valueCount ; i++) { - if (i > 0) + if (i > 0) cliPrint(","); cliPrintf(" %s", tableEntry->values[i]); } @@ -2923,7 +2923,7 @@ static void cliStatus(char *cmdline) #ifdef USE_SDCARD cliSdInfo(NULL); -#endif +#endif } #ifndef SKIP_TASK_STATISTICS diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index f28c68a605..c21680f778 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1876,9 +1876,9 @@ static bool processInCommand(void) masterConfig.baro_hardware = read8(); masterConfig.mag_hardware = read8(); break; - + case MSP_SET_NAME: - memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); + memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); for (i = 0; i < MIN(MAX_NAME_LENGTH, currentPort->dataSize); i++) { masterConfig.name[i] = read8(); } diff --git a/src/main/io/vtx.h b/src/main/io/vtx.h index 942a770547..fdd5e506f9 100644 --- a/src/main/io/vtx.h +++ b/src/main/io/vtx.h @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ - + #pragma once #define VTX_BAND_MIN 1 diff --git a/src/main/main.c b/src/main/main.c index 8cf45ac7bb..0daf402768 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -313,7 +313,7 @@ void init(void) bool use_unsyncedPwm = masterConfig.use_unsyncedPwm; // Configurator feature abused for enabling Fast PWM - pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED); + pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED); pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol; pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index 0e68854897..bbf4848bb7 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -226,7 +226,7 @@ static void xBusUnpackRJ01Frame(void) uint8_t outerCrc = 0; uint8_t i = 0; - // When using the Align RJ01 receiver with + // When using the Align RJ01 receiver with // a MODE B setting in the radio (XG14 tested) // the MODE_B -frame is packed within some // at the moment unknown bytes before and after: @@ -234,7 +234,7 @@ static void xBusUnpackRJ01Frame(void) // Compared to a standard MODE B frame that only // contains the "middle" package. // Hence, at the moment, the unknown header and footer - // of the RJ01 MODEB packages are discarded. + // of the RJ01 MODEB packages are discarded. // However, the LAST byte (CRC_OUTER) is infact an 8-bit // CRC for the whole package, using the Dallas-One-Wire CRC // method. diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 59fa7ef9ae..fed53524e7 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -91,7 +91,7 @@ void updateBattery(void) /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ batteryState = BATTERY_OK; /* wait for VBatt to stabilise then we can calc number of cells - (using the filtered value takes a long time to ramp up) + (using the filtered value takes a long time to ramp up) We only do this on the ground so don't care if we do block, not worse than original code anyway*/ delay(VBATTERY_STABLE_DELAY); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 908606c364..38d81a1f4f 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -89,7 +89,7 @@ const extiConfig_t *selectMPUIntExtiConfig(void) #if defined(MPU_INT_EXTI) static const extiConfig_t mpuIntExtiConfig = { .tag = IO_TAG(MPU_INT_EXTI) }; return &mpuIntExtiConfig; -#elif defined(USE_HARDWARE_REVISION_DETECTION) +#elif defined(USE_HARDWARE_REVISION_DETECTION) return selectMPUIntExtiConfigByHardwareRevision(); #else return NULL; diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index 253af13be0..10bc3a49be 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -21,7 +21,7 @@ #define USE_CLI #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - + #define LED0 PB5 // Blue LED - PB5 #define BEEPER PA0 diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.h b/src/main/target/ALIENFLIGHTF3/hardware_revision.h index 7e60ddc55f..9b23c614d1 100644 --- a/src/main/target/ALIENFLIGHTF3/hardware_revision.h +++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.h @@ -15,7 +15,7 @@ * along with Cleanflight. If not, see . */ #pragma once - + #include "drivers/exti.h" typedef enum awf3HardwareRevision_t { diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index f005e5d1a0..b85e50f539 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -33,7 +33,7 @@ const uint16_t multiPPM[] = { }; const uint16_t multiPWM[] = { - PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM1 | (MAP_TO_PWM_INPUT << 8), PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index ffae24c925..34bdcf8aa7 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -141,7 +141,7 @@ #define LED_STRIP // LED Strip can run off Pin 6 (PB1) of the ESC outputs. -#define WS2811_PIN PB1 +#define WS2811_PIN PB1 #define WS2811_TIMER TIM3 #define WS2811_TIMER_CHANNEL TIM_Channel_4 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index f60a7b7b2a..a1d9453e2f 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -22,8 +22,8 @@ #ifndef STM32F3DISCOVERY #define STM32F3DISCOVERY -#endif - +#endif + #define LED0 PE8 // Blue LEDs - PE8/PE12 #define LED0_INVERTED #define LED1 PE10 // Orange LEDs - PE10/PE14 diff --git a/src/main/target/CJMCU/hardware_revision.h b/src/main/target/CJMCU/hardware_revision.h index 4b3c13d674..89e3478c13 100755 --- a/src/main/target/CJMCU/hardware_revision.h +++ b/src/main/target/CJMCU/hardware_revision.h @@ -17,7 +17,7 @@ #pragma once #include "drivers/exti.h" - + typedef enum cjmcuHardwareRevision_t { UNKNOWN = 0, REV_1, // Blue LED3 diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index a40ba0083b..f98d39eb36 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -23,7 +23,7 @@ #define TARGET_CONFIG #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - + #define LED0 PC15 #define LED1 PC14 #define LED2 PC13 diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index c813ceecba..05b642f088 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -43,7 +43,7 @@ const uint16_t multiPWM[] = { PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 8f57af76d7..2399ede8d1 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -20,7 +20,7 @@ #define TARGET_BOARD_IDENTIFIER "DOGE" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - + // tqfp48 pin 34 #define LED0 PA13 // tqfp48 pin 37 diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index e023d7e960..a8d80e823d 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -30,11 +30,11 @@ const uint16_t multiPWM[] = { PWM3 | (MAP_TO_PWM_INPUT << 8), PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), - PWM6 | (MAP_TO_PWM_INPUT << 8), - PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // input #8 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), @@ -69,11 +69,11 @@ const uint16_t airPWM[] = { PWM3 | (MAP_TO_PWM_INPUT << 8), PWM4 | (MAP_TO_PWM_INPUT << 8), PWM5 | (MAP_TO_PWM_INPUT << 8), - PWM6 | (MAP_TO_PWM_INPUT << 8), - PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 PWM13 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 @@ -101,7 +101,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT - + { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // sonar echo if needed }; diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 21b05ab5d4..bf4ff7b20a 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -76,7 +76,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - S2 { TIM17, IO_TAG(PB5), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10}, // PWM6 - S3 { TIM16, IO_TAG(PB4), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - S4 - + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO TIMER - LED_STRIP }; diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 4ee1a3835a..82323ad058 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -24,7 +24,7 @@ #define MPU_INT_EXTI PC4 #define USE_EXTI #define CONFIG_PREFER_ACC_ON - + #define LED0 PC14 #define BEEPER PC15 diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 81fb3b3cee..ef5fc8547c 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -21,7 +21,7 @@ #define BOARD_HAS_VOLTAGE_DIVIDER #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - + #define LED0 PC15 #define LED1 PC14 #define LED2 PC13 diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 1414ae6be3..1db54b9886 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -21,7 +21,7 @@ #define USE_CLI #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - + #define LED0 PB5 // Blue LEDs - PB5 //#define LED1 PB9 // Green LEDs - PB9 diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index a5a91199b2..390dc39f2a 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -73,7 +73,7 @@ //#define DEBUG_MAG_DATA_READY_INTERRUPT #define USE_MAG_DATA_READY_SIGNAL #define MAG_INT_EXTI PC14 - + #define GYRO #define USE_GYRO_MPU3050 #define USE_GYRO_MPU6050 diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 03f60516d4..4e5aa61a12 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -93,7 +93,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S4_IN { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S5_IN { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S6_IN - + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S1_OUT { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S2_OUT { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM9 }, // S3_OUT diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index db1464f3d0..f566b07da7 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -107,7 +107,7 @@ #define VBAT_ADC_PIN PC2 #define RSSI_ADC_GPIO_PIN PA0 - + #define SENSORS_SET (SENSOR_ACC) diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 5075367764..704845c1ee 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -20,7 +20,7 @@ #define TARGET_BOARD_IDENTIFIER "SPEV" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - + #define LED0 PB8 #define BEEPER PC15 diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h index dc9f80cb23..aaa3635da6 100644 --- a/src/main/target/ZCOREF3/target.h +++ b/src/main/target/ZCOREF3/target.h @@ -74,7 +74,7 @@ #define SPI1_SCK_PIN PB3 #define SPI1_MISO_PIN PB4 #define SPI1_MOSI_PIN PB5 - + #define MPU6500_CS_PIN PB9 #define MPU6500_SPI_INSTANCE SPI1 diff --git a/src/main/target/common.h b/src/main/target/common.h index c9a9a0f1b3..65ab02c871 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -14,7 +14,7 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ - + #pragma once #define I2C1_OVERCLOCK true diff --git a/src/main/target/system_stm32f30x.c b/src/main/target/system_stm32f30x.c index 09ba3fa47c..5d62e47aa3 100644 --- a/src/main/target/system_stm32f30x.c +++ b/src/main/target/system_stm32f30x.c @@ -9,17 +9,17 @@ * and is generated by the clock configuration tool * stm32f30x_Clock_Configuration_V1.0.0.xls * - * 1. This file provides two functions and one global variable to be called from + * 1. This file provides two functions and one global variable to be called from * user application: * - SystemInit(): Setups the system clock (System clock source, PLL Multiplier * and Divider factors, AHB/APBx prescalers and Flash settings), - * depending on the configuration made in the clock xls tool. - * This function is called at startup just after reset and + * depending on the configuration made in the clock xls tool. + * This function is called at startup just after reset and * before branch to main program. This call is made inside * the "startup_stm32f30x.s" file. * * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used - * by the user application to setup the SysTick + * by the user application to setup the SysTick * timer or configure other parameters. * * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must @@ -31,7 +31,7 @@ * configure the system clock before to branch to main program. * * 3. If the system clock source selected by user fails to startup, the SystemInit() - * function will do nothing and HSI still used as system clock source. User can + * function will do nothing and HSI still used as system clock source. User can * add some code to deal with this issue inside the SetSysClock() function. * * 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define @@ -79,8 +79,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. @@ -93,8 +93,8 @@ /** @addtogroup stm32f30x_system * @{ - */ - + */ + /** @addtogroup STM32F30x_System_Private_Includes * @{ */ @@ -113,13 +113,13 @@ uint32_t hse_value = HSE_VALUE; * @{ */ /*!< Uncomment the following line if you need to relocate your vector Table in - Internal SRAM. */ + Internal SRAM. */ /* #define VECT_TAB_SRAM */ -#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field. - This value must be a multiple of 0x200. */ +#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ /** * @} - */ + */ /* Private macro -------------------------------------------------------------*/ @@ -151,7 +151,7 @@ void SetSysClock(void); /** * @brief Setup the microcontroller system - * Initialize the Embedded Flash Interface, the PLL and update the + * Initialize the Embedded Flash Interface, the PLL and update the * SystemFrequency variable. * @param None * @retval None @@ -184,19 +184,19 @@ void SystemInit(void) /* Reset USARTSW[1:0], I2CSW and TIMs bits */ RCC->CFGR3 &= (uint32_t)0xFF00FCCC; - + /* Disable all interrupts */ RCC->CIR = 0x00000000; - /* Configure the System clock source, PLL Multiplier and Divider factors, + /* Configure the System clock source, PLL Multiplier and Divider factors, AHB/APBx prescalers and Flash settings ----------------------------------*/ //SetSysClock(); // called from main() - + #ifdef VECT_TAB_SRAM SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */ #else SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */ -#endif +#endif } /** @@ -204,25 +204,25 @@ void SystemInit(void) * The SystemCoreClock variable contains the core clock (HCLK), it can * be used by the user application to setup the SysTick timer or configure * other parameters. - * + * * @note Each time the core clock (HCLK) changes, this function must be called * to update SystemCoreClock variable value. Otherwise, any configuration - * based on this variable will be incorrect. - * - * @note - The system frequency computed by this function is not the real - * frequency in the chip. It is calculated based on the predefined + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined * constant and the selected clock source: * * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) * * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) * - * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) * or HSI_VALUE(*) multiplied/divided by the PLL factors. - * + * * (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value * 8 MHz) but the real value may vary depending on the variations - * in voltage and temperature. + * in voltage and temperature. * * (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value * 8 MHz), user has to ensure that HSE_VALUE is same as the real @@ -231,7 +231,7 @@ void SystemInit(void) * * - The result of this function could be not correct when using fractional * value for HSE crystal. - * + * * @param None * @retval None */ @@ -241,7 +241,7 @@ void SystemCoreClockUpdate (void) /* Get SYSCLK source -------------------------------------------------------*/ tmp = RCC->CFGR & RCC_CFGR_SWS; - + switch (tmp) { case 0x00: /* HSI used as system clock */ @@ -255,7 +255,7 @@ void SystemCoreClockUpdate (void) pllmull = RCC->CFGR & RCC_CFGR_PLLMULL; pllsource = RCC->CFGR & RCC_CFGR_PLLSRC; pllmull = ( pllmull >> 18) + 2; - + if (pllsource == 0x00) { /* HSI oscillator clock divided by 2 selected as PLL clock entry */ @@ -265,8 +265,8 @@ void SystemCoreClockUpdate (void) { prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1; /* HSE oscillator clock selected as PREDIV1 clock entry */ - SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; - } + SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; + } break; default: /* HSI used as system clock */ SystemCoreClock = HSI_VALUE; @@ -276,7 +276,7 @@ void SystemCoreClockUpdate (void) /* Get HCLK prescaler */ tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; /* HCLK clock frequency */ - SystemCoreClock >>= tmp; + SystemCoreClock >>= tmp; } /** @@ -298,7 +298,7 @@ void SetSysClock(void) /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/ /* Enable HSE */ RCC->CR |= ((uint32_t)RCC_CR_HSEON); - + /* Wait till HSE is ready and if Time out is reached exit */ do { @@ -319,13 +319,13 @@ void SetSysClock(void) { /* Enable Prefetch Buffer and set Flash Latency */ FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1; - + /* HCLK = SYSCLK / 1 */ RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1; - + /* PCLK2 = HCLK / 1 */ RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1; - + /* PCLK1 = HCLK / 2 */ RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2; diff --git a/src/main/target/system_stm32f30x.h b/src/main/target/system_stm32f30x.h index 4f999d3058..b213ad0b1e 100644 --- a/src/main/target/system_stm32f30x.h +++ b/src/main/target/system_stm32f30x.h @@ -4,7 +4,7 @@ * @author MCD Application Team * @version V1.1.1 * @date 28-March-2014 - * @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices. + * @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices. ****************************************************************************** * @attention * @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. @@ -31,8 +31,8 @@ /** @addtogroup stm32f30x_system * @{ - */ - + */ + /** * @brief Define to prevent recursive inclusion */ @@ -41,7 +41,7 @@ #ifdef __cplusplus extern "C" { -#endif +#endif /* Exported types ------------------------------------------------------------*/ extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ @@ -52,7 +52,7 @@ extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Cloc /** @addtogroup STM32F30x_System_Exported_Functions * @{ */ - + extern void SystemInit(void); extern void SystemCoreClockUpdate(void); @@ -69,8 +69,8 @@ extern void SystemCoreClockUpdate(void); /** * @} */ - + /** * @} - */ + */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/target/system_stm32f4xx.h b/src/main/target/system_stm32f4xx.h index e266366801..45c811bb06 100644 --- a/src/main/target/system_stm32f4xx.h +++ b/src/main/target/system_stm32f4xx.h @@ -4,8 +4,8 @@ * @author MCD Application Team * @version V1.6.1 * @date 21-October-2015 - * @brief CMSIS Cortex-M4 Device System Source File for STM32F4xx devices. - ****************************************************************************** + * @brief CMSIS Cortex-M4 Device System Source File for STM32F4xx devices. + ****************************************************************************** * @attention * *

© COPYRIGHT 2015 STMicroelectronics

@@ -16,21 +16,21 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * - ****************************************************************************** - */ + ****************************************************************************** + */ #ifndef __SYSTEM_STM32F4XX_H #define __SYSTEM_STM32F4XX_H #ifdef __cplusplus extern "C" { -#endif +#endif extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ extern void SystemInit(void); diff --git a/src/main/vcp/hw_config.c b/src/main/vcp/hw_config.c index 7384b1e0b4..d7552ea0fb 100644 --- a/src/main/vcp/hw_config.c +++ b/src/main/vcp/hw_config.c @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. @@ -68,13 +68,13 @@ void Set_System(void) { #if !defined(STM32L1XX_MD) && !defined(STM32L1XX_HD) && !defined(STM32L1XX_MD_PLUS) GPIO_InitTypeDef GPIO_InitStructure; -#endif /* STM32L1XX_MD && STM32L1XX_XD */ +#endif /* STM32L1XX_MD && STM32L1XX_XD */ #if defined(USB_USE_EXTERNAL_PULLUP) GPIO_InitTypeDef GPIO_InitStructure; -#endif /* USB_USE_EXTERNAL_PULLUP */ +#endif /* USB_USE_EXTERNAL_PULLUP */ - /*!< At this stage the microcontroller clock setting is already configured, + /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup file (startup_stm32f10x_xx.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to @@ -83,7 +83,7 @@ void Set_System(void) #if defined(STM32L1XX_MD) || defined(STM32L1XX_HD)|| defined(STM32L1XX_MD_PLUS) || defined(STM32F37X) || defined(STM32F303xC) /* Enable the SYSCFG module clock */ RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); -#endif /* STM32L1XX_XD */ +#endif /* STM32L1XX_XD */ /*Pull down PA12 to create USB Disconnect Pulse*/ // HJI #if defined(STM32F303xC) // HJI @@ -277,7 +277,7 @@ static void IntToUnicode(uint32_t value, uint8_t *pbuf, uint8_t len) /******************************************************************************* * Function Name : Send DATA . - * Description : send the data received from the STM32 to the PC through USB + * Description : send the data received from the STM32 to the PC through USB * Input : None. * Output : None. * Return : None. diff --git a/src/main/vcp/platform_config.h b/src/main/vcp/platform_config.h index 35715588e9..3f09726229 100644 --- a/src/main/vcp/platform_config.h +++ b/src/main/vcp/platform_config.h @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/stm32_it.h b/src/main/vcp/stm32_it.h index 959a1e6db0..f3758882bf 100644 --- a/src/main/vcp/stm32_it.h +++ b/src/main/vcp/stm32_it.h @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/usb_conf.h b/src/main/vcp/usb_conf.h index 6b322297ea..c0c17673ff 100644 --- a/src/main/vcp/usb_conf.h +++ b/src/main/vcp/usb_conf.h @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/usb_desc.c b/src/main/vcp/usb_desc.c index 64ead56f62..3db4216b68 100644 --- a/src/main/vcp/usb_desc.c +++ b/src/main/vcp/usb_desc.c @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/usb_desc.h b/src/main/vcp/usb_desc.h index 92f38341a3..9a21291c0c 100644 --- a/src/main/vcp/usb_desc.h +++ b/src/main/vcp/usb_desc.h @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/usb_istr.h b/src/main/vcp/usb_istr.h index 1d72d91c67..7285da6852 100644 --- a/src/main/vcp/usb_istr.h +++ b/src/main/vcp/usb_istr.h @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/usb_prop.c b/src/main/vcp/usb_prop.c index b070ce9cca..c8558de342 100644 --- a/src/main/vcp/usb_prop.c +++ b/src/main/vcp/usb_prop.c @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcp/usb_prop.h b/src/main/vcp/usb_prop.h index 12d050c38d..f8e1d20249 100644 --- a/src/main/vcp/usb_prop.h +++ b/src/main/vcp/usb_prop.h @@ -16,8 +16,8 @@ * * http://www.st.com/software_license_agreement_liberty_v2 * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. diff --git a/src/main/vcpf4/stm32f4xx_it.h b/src/main/vcpf4/stm32f4xx_it.h index 74f17c9cce..dd000126fd 100644 --- a/src/main/vcpf4/stm32f4xx_it.h +++ b/src/main/vcpf4/stm32f4xx_it.h @@ -1,6 +1,6 @@ /** ****************************************************************************** - * @file GPIO/IOToggle/stm32f4xx_it.h + * @file GPIO/IOToggle/stm32f4xx_it.h * @author MCD Application Team * @version V1.0.0 * @date 19-September-2011 @@ -17,7 +17,7 @@ * *

© COPYRIGHT 2011 STMicroelectronics

****************************************************************************** - */ + */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef __STM32F4xx_IT_H @@ -25,7 +25,7 @@ #ifdef __cplusplus extern "C" { -#endif +#endif /* Includes ------------------------------------------------------------------*/ #include "stm32f4xx.h" diff --git a/src/main/vcpf4/usb_conf.h b/src/main/vcpf4/usb_conf.h index ea8d49bcf1..bd5bbe14f1 100644 --- a/src/main/vcpf4/usb_conf.h +++ b/src/main/vcpf4/usb_conf.h @@ -30,15 +30,15 @@ /** @addtogroup USB_OTG_DRIVER * @{ */ - + /** @defgroup USB_CONF * @brief USB low level driver configuration file * @{ - */ + */ /** @defgroup USB_CONF_Exported_Defines * @{ - */ + */ /* USB Core and PHY interface configuration. Tip: To avoid modifying these defines each time you need to change the USB @@ -66,7 +66,7 @@ #endif /* USE_I2C_PHY */ -#ifdef USE_USB_OTG_FS +#ifdef USE_USB_OTG_FS #define USB_OTG_FS_CORE #endif @@ -76,35 +76,35 @@ /******************************************************************************* * FIFO Size Configuration in Device mode -* -* (i) Receive data FIFO size = RAM for setup packets + +* +* (i) Receive data FIFO size = RAM for setup packets + * OUT endpoint control information + * data OUT packets + miscellaneous * Space = ONE 32-bits words * --> RAM for setup packets = 10 spaces -* (n is the nbr of CTRL EPs the device core supports) +* (n is the nbr of CTRL EPs the device core supports) * --> OUT EP CTRL info = 1 space -* (one space for status information written to the FIFO along with each +* (one space for status information written to the FIFO along with each * received packet) -* --> data OUT packets = (Largest Packet Size / 4) + 1 spaces +* --> data OUT packets = (Largest Packet Size / 4) + 1 spaces * (MINIMUM to receive packets) -* --> OR data OUT packets = at least 2*(Largest Packet Size / 4) + 1 spaces +* --> OR data OUT packets = at least 2*(Largest Packet Size / 4) + 1 spaces * (if high-bandwidth EP is enabled or multiple isochronous EPs) * --> miscellaneous = 1 space per OUT EP -* (one space for transfer complete status information also pushed to the +* (one space for transfer complete status information also pushed to the * FIFO with each endpoint's last packet) * -* (ii)MINIMUM RAM space required for each IN EP Tx FIFO = MAX packet size for +* (ii)MINIMUM RAM space required for each IN EP Tx FIFO = MAX packet size for * that particular IN EP. More space allocated in the IN EP Tx FIFO results * in a better performance on the USB and can hide latencies on the AHB. * * (iii) TXn min size = 16 words. (n : Transmit FIFO index) -* (iv) When a TxFIFO is not used, the Configuration should be as follows: +* (iv) When a TxFIFO is not used, the Configuration should be as follows: * case 1 : n > m and Txn is not used (n,m : Transmit FIFO indexes) * --> Txm can use the space allocated for Txn. * case2 : n < m and Txn is not used (n,m : Transmit FIFO indexes) * --> Txn should be configured with the minimum space of 16 words -* (v) The FIFO is used optimally when used TxFIFOs are allocated in the top +* (v) The FIFO is used optimally when used TxFIFOs are allocated in the top * of the FIFO.Ex: use EP1 and EP2 as IN instead of EP1 and EP3 as IN ones. *******************************************************************************/ @@ -126,7 +126,7 @@ * then the space must be at least two times the maximum packet size for * that channel. *******************************************************************************/ - + /****************** USB OTG HS CONFIGURATION **********************************/ #ifdef USB_OTG_HS_CORE #define RX_FIFO_HS_SIZE 512 @@ -211,20 +211,20 @@ #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED #if defined (__GNUC__) /* GNU Compiler */ #define __ALIGN_END __attribute__ ((aligned (4))) - #define __ALIGN_BEGIN + #define __ALIGN_BEGIN #else #define __ALIGN_END #if defined (__CC_ARM) /* ARM Compiler */ - #define __ALIGN_BEGIN __align(4) - #elif defined (__ICCARM__) /* IAR Compiler */ - #define __ALIGN_BEGIN - #elif defined (__TASKING__) /* TASKING Compiler */ #define __ALIGN_BEGIN __align(4) - #endif /* __CC_ARM */ - #endif /* __GNUC__ */ + #elif defined (__ICCARM__) /* IAR Compiler */ + #define __ALIGN_BEGIN + #elif defined (__TASKING__) /* TASKING Compiler */ + #define __ALIGN_BEGIN __align(4) + #endif /* __CC_ARM */ + #endif /* __GNUC__ */ #else #define __ALIGN_BEGIN - #define __ALIGN_END + #define __ALIGN_END #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ /* __packed keyword used to decrease the data type alignment to 1-byte */ @@ -242,37 +242,37 @@ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_Types * @{ - */ + */ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_Macros * @{ - */ + */ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_Variables * @{ - */ + */ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_FunctionsPrototype * @{ - */ + */ /** * @} - */ + */ #endif //__USB_CONF__H__ @@ -280,10 +280,10 @@ /** * @} - */ + */ /** * @} - */ + */ /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 61e26a1131..f312e4a34c 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -19,8 +19,8 @@ ****************************************************************************** */ -#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED -#pragma data_alignment = 4 +#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED +#pragma data_alignment = 4 #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ /* Includes ------------------------------------------------------------------*/ @@ -34,7 +34,7 @@ LINE_CODING g_lc; extern __IO uint8_t USB_Tx_State; __IO uint32_t bDeviceState = UNCONNECTED; /* USB device status */ -/* These are external variables imported from CDC core to be used for IN +/* These are external variables imported from CDC core to be used for IN transfer management. */ extern uint8_t APP_Rx_Buffer[]; /* Write CDC received data in this buffer. These data will be sent over USB IN endpoint @@ -105,7 +105,7 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) assert_param(Len>=sizeof(LINE_CODING)); switch (Cmd) { - /* Not needed for this driver, AT modem commands */ + /* Not needed for this driver, AT modem commands */ case SEND_ENCAPSULATED_COMMAND: case GET_ENCAPSULATED_RESPONSE: break; @@ -116,18 +116,18 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) case CLEAR_COMM_FEATURE: break; - + //Note - hw flow control on UART 1-3 and 6 only - case SET_LINE_CODING: + case SET_LINE_CODING: ust_cpy(&g_lc, plc); //Copy into structure to save for later break; - - + + case GET_LINE_CODING: ust_cpy(plc, &g_lc); break; - + case SET_CONTROL_LINE_STATE: /* Not needed for this driver */ //RSW - This tells how to set RTS and DTR @@ -153,7 +153,7 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) *******************************************************************************/ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) { - if (USB_Tx_State) + if (USB_Tx_State) return 0; VCP_DataTx(ptrBuffer, sendLength); @@ -177,7 +177,7 @@ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) return USBD_OK; } -uint8_t usbAvailable(void) +uint8_t usbAvailable(void) { return (usbData.bufferInPosition != usbData.bufferOutPosition); } diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index df1e29001c..553017ff28 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -59,7 +59,7 @@ typedef enum _DEVICE_STATE { } DEVICE_STATE; /* Exported typef ------------------------------------------------------------*/ -/* The following structures groups all needed parameters to be configured for the +/* The following structures groups all needed parameters to be configured for the ComPort. These parameters can modified on the fly by the host through CDC class command class requests. */ typedef struct diff --git a/src/main/vcpf4/usbd_conf.h b/src/main/vcpf4/usbd_conf.h index a32176efe8..02ceeae545 100644 --- a/src/main/vcpf4/usbd_conf.h +++ b/src/main/vcpf4/usbd_conf.h @@ -60,36 +60,36 @@ #define APP_FOPS VCP_fops /** * @} - */ + */ /** @defgroup USB_CONF_Exported_Types * @{ - */ + */ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_Macros * @{ - */ + */ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_Variables * @{ - */ + */ /** * @} - */ + */ /** @defgroup USB_CONF_Exported_FunctionsPrototype * @{ - */ + */ /** * @} - */ + */ #endif //__USBD_CONF__H__ diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index ab1a3e1d35..a0cf881916 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/src/main/vcpf4/usbd_desc.c @@ -17,7 +17,7 @@ * *

© COPYRIGHT 2011 STMicroelectronics

****************************************************************************** - */ + */ /* Includes ------------------------------------------------------------------*/ #include "usbd_core.h" @@ -32,22 +32,22 @@ */ -/** @defgroup USBD_DESC +/** @defgroup USBD_DESC * @brief USBD descriptors module * @{ - */ + */ /** @defgroup USBD_DESC_Private_TypesDefinitions * @{ - */ + */ /** * @} - */ + */ /** @defgroup USBD_DESC_Private_Defines * @{ - */ + */ #define USBD_VID 0x0483 @@ -55,7 +55,7 @@ /** @defgroup USB_String_Descriptors * @{ - */ + */ #define USBD_LANGID_STRING 0x409 #define USBD_MANUFACTURER_STRING "BetaFlight" @@ -83,36 +83,36 @@ #define USBD_INTERFACE_FS_STRING "VCP Interface" /** * @} - */ + */ /** @defgroup USBD_DESC_Private_Macros * @{ - */ + */ /** * @} - */ + */ /** @defgroup USBD_DESC_Private_Variables * @{ - */ + */ USBD_DEVICE USR_desc = { USBD_USR_DeviceDescriptor, - USBD_USR_LangIDStrDescriptor, + USBD_USR_LangIDStrDescriptor, USBD_USR_ManufacturerStrDescriptor, USBD_USR_ProductStrDescriptor, USBD_USR_SerialStrDescriptor, USBD_USR_ConfigStrDescriptor, USBD_USR_InterfaceStrDescriptor, - + }; #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED #if defined ( __ICCARM__ ) /*!< IAR Compiler */ - #pragma data_alignment=4 + #pragma data_alignment=4 #endif #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ /* USB Standard Device Descriptor */ @@ -140,7 +140,7 @@ __ALIGN_BEGIN uint8_t USBD_DeviceDesc[USB_SIZ_DEVICE_DESC] __ALIGN_END = #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED #if defined ( __ICCARM__ ) /*!< IAR Compiler */ - #pragma data_alignment=4 + #pragma data_alignment=4 #endif #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ /* USB Standard Device Descriptor */ @@ -160,36 +160,36 @@ __ALIGN_BEGIN uint8_t USBD_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC] __ALI #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED #if defined ( __ICCARM__ ) /*!< IAR Compiler */ - #pragma data_alignment=4 + #pragma data_alignment=4 #endif #endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */ /* USB Standard Device Descriptor */ __ALIGN_BEGIN uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID] __ALIGN_END = { - USB_SIZ_STRING_LANGID, - USB_DESC_TYPE_STRING, + USB_SIZ_STRING_LANGID, + USB_DESC_TYPE_STRING, LOBYTE(USBD_LANGID_STRING), - HIBYTE(USBD_LANGID_STRING), + HIBYTE(USBD_LANGID_STRING), }; /** * @} - */ + */ /** @defgroup USBD_DESC_Private_FunctionPrototypes * @{ - */ + */ /** * @} - */ + */ /** @defgroup USBD_DESC_Private_Functions * @{ - */ + */ /** -* @brief USBD_USR_DeviceDescriptor +* @brief USBD_USR_DeviceDescriptor * return the device descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -203,7 +203,7 @@ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) } /** -* @brief USBD_USR_LangIDStrDescriptor +* @brief USBD_USR_LangIDStrDescriptor * return the LangID string descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -212,13 +212,13 @@ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length) uint8_t * USBD_USR_LangIDStrDescriptor( uint8_t speed , uint16_t *length) { (void)speed; - *length = sizeof(USBD_LangIDDesc); + *length = sizeof(USBD_LangIDDesc); return USBD_LangIDDesc; } /** -* @brief USBD_USR_ProductStrDescriptor +* @brief USBD_USR_ProductStrDescriptor * return the product string descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -226,8 +226,8 @@ uint8_t * USBD_USR_LangIDStrDescriptor( uint8_t speed , uint16_t *length) */ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length) { + - if(speed == 0) USBD_GetString ((uint8_t*)USBD_PRODUCT_HS_STRING, USBD_StrDesc, length); else @@ -237,7 +237,7 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length) } /** -* @brief USBD_USR_ManufacturerStrDescriptor +* @brief USBD_USR_ManufacturerStrDescriptor * return the manufacturer string descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -251,7 +251,7 @@ uint8_t * USBD_USR_ManufacturerStrDescriptor( uint8_t speed , uint16_t *length) } /** -* @brief USBD_USR_SerialStrDescriptor +* @brief USBD_USR_SerialStrDescriptor * return the serial number string descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -268,7 +268,7 @@ uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length) } /** -* @brief USBD_USR_ConfigStrDescriptor +* @brief USBD_USR_ConfigStrDescriptor * return the configuration string descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -281,12 +281,12 @@ uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length) else USBD_GetString ((uint8_t*)USBD_CONFIGURATION_FS_STRING, USBD_StrDesc, length); - return USBD_StrDesc; + return USBD_StrDesc; } /** -* @brief USBD_USR_InterfaceStrDescriptor +* @brief USBD_USR_InterfaceStrDescriptor * return the interface string descriptor * @param speed : current device speed * @param length : pointer to data length variable @@ -299,22 +299,22 @@ uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length) else USBD_GetString ((uint8_t*)USBD_INTERFACE_FS_STRING, USBD_StrDesc, length); - return USBD_StrDesc; + return USBD_StrDesc; } /** * @} - */ + */ /** * @} - */ + */ /** * @} - */ + */ /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ diff --git a/src/main/vcpf4/usbd_desc.h b/src/main/vcpf4/usbd_desc.h index ed999dc62a..f67799cf42 100644 --- a/src/main/vcpf4/usbd_desc.h +++ b/src/main/vcpf4/usbd_desc.h @@ -6,18 +6,18 @@ * @date 19-September-2011 * @brief header file for the usbd_desc.c file ****************************************************************************** - * @attention + * @attention * * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE - * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. * *

© COPYRIGHT 2011 STMicroelectronics

****************************************************************************** - */ + */ /* Define to prevent recursive inclusion -------------------------------------*/ @@ -30,11 +30,11 @@ /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY * @{ */ - + /** @defgroup USB_DESC * @brief general defines for the usb device library file * @{ - */ + */ /** @defgroup USB_DESC_Exported_Defines * @{ @@ -49,7 +49,7 @@ /** * @} - */ + */ /** @defgroup USBD_DESC_Exported_TypesDefinitions @@ -57,33 +57,33 @@ */ /** * @} - */ + */ /** @defgroup USBD_DESC_Exported_Macros * @{ - */ + */ /** * @} - */ + */ /** @defgroup USBD_DESC_Exported_Variables * @{ - */ + */ extern uint8_t USBD_DeviceDesc [USB_SIZ_DEVICE_DESC]; extern uint8_t USBD_StrDesc[USB_MAX_STR_DESC_SIZ]; -extern uint8_t USBD_OtherSpeedCfgDesc[USB_LEN_CFG_DESC]; +extern uint8_t USBD_OtherSpeedCfgDesc[USB_LEN_CFG_DESC]; extern uint8_t USBD_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC]; extern uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID]; -extern USBD_DEVICE USR_desc; +extern USBD_DEVICE USR_desc; /** * @} - */ + */ /** @defgroup USBD_DESC_Exported_FunctionsPrototype * @{ - */ + */ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length); @@ -95,20 +95,20 @@ uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length); uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length); #ifdef USB_SUPPORT_USER_STRING_DESC -uint8_t * USBD_USR_USRStringDesc (uint8_t speed, uint8_t idx , uint16_t *length); -#endif /* USB_SUPPORT_USER_STRING_DESC */ - +uint8_t * USBD_USR_USRStringDesc (uint8_t speed, uint8_t idx , uint16_t *length); +#endif /* USB_SUPPORT_USER_STRING_DESC */ + /** * @} - */ + */ #endif /* __USBD_DESC_H */ /** * @} - */ + */ /** * @} -*/ +*/ /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ diff --git a/src/main/vcpf4/usbd_usr.c b/src/main/vcpf4/usbd_usr.c index 958e772322..b3de6d44a6 100644 --- a/src/main/vcpf4/usbd_usr.c +++ b/src/main/vcpf4/usbd_usr.c @@ -17,7 +17,7 @@ * *

© COPYRIGHT 2011 STMicroelectronics

****************************************************************************** - */ + */ #include "usbd_usr.h" #include "usbd_ioreq.h" @@ -29,25 +29,25 @@ USBD_Usr_cb_TypeDef USR_cb = USBD_USR_DeviceConfigured, USBD_USR_DeviceSuspended, USBD_USR_DeviceResumed, - + USBD_USR_DeviceConnected, USBD_USR_DeviceDisconnected, }; /** -* @brief USBD_USR_Init +* @brief USBD_USR_Init * Displays the message on LCD for host lib initialization * @param None * @retval None */ void USBD_USR_Init(void) -{ +{ } /** -* @brief USBD_USR_DeviceReset +* @brief USBD_USR_DeviceReset * Displays the message on LCD on device Reset Event * @param speed : device speed * @retval None @@ -56,14 +56,14 @@ void USBD_USR_DeviceReset(uint8_t speed ) { switch (speed) { - case USB_OTG_SPEED_HIGH: + case USB_OTG_SPEED_HIGH: break; - case USB_OTG_SPEED_FULL: + case USB_OTG_SPEED_FULL: break; default: break; - + } } @@ -101,7 +101,7 @@ void USBD_USR_DeviceDisconnected (void) } /** -* @brief USBD_USR_DeviceSuspended +* @brief USBD_USR_DeviceSuspended * Displays the message on LCD on device suspend Event * @param None * @retval None @@ -113,7 +113,7 @@ void USBD_USR_DeviceSuspended(void) /** -* @brief USBD_USR_DeviceResumed +* @brief USBD_USR_DeviceResumed * Displays the message on LCD on device resume Event * @param None * @retval None From cf6c045b42100f9e8f2171bcc8f34c85e6f2acf8 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 16 Jul 2016 08:41:37 +0100 Subject: [PATCH 038/456] Tidied whitespace in target.h files --- src/main/target/CHEBUZZF3/target.h | 37 ++++++------- src/main/target/F4BY/target.h | 14 ++--- src/main/target/OLIMEXINO/target.h | 21 ++++---- src/main/target/OMNIBUS/target.h | 66 +++++++++++------------ src/main/target/PIKOBLX/target.h | 2 +- src/main/target/PORT103R/target.h | 56 ++++++++++---------- src/main/target/SINGULARITY/target.h | 44 ++++++++-------- src/main/target/SIRINFPV/target.h | 73 +++++++++++++------------- src/main/target/SPRACINGF3EVO/target.h | 2 +- src/main/target/X_RACERSPI/target.h | 63 +++++++++++----------- src/main/target/ZCOREF3/target.h | 48 ++++++++--------- 11 files changed, 208 insertions(+), 218 deletions(-) diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index a1d9453e2f..84f7071c86 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -24,12 +24,12 @@ #define STM32F3DISCOVERY #endif -#define LED0 PE8 // Blue LEDs - PE8/PE12 +#define LED0 PE8 // Blue LEDs - PE8/PE12 #define LED0_INVERTED -#define LED1 PE10 // Orange LEDs - PE10/PE14 +#define LED1 PE10 // Orange LEDs - PE10/PE14 #define LED1_INVERTED -#define BEEPER PE9 // Red LEDs - PE9/PE13 +#define BEEPER PE9 // Red LEDs - PE9/PE13 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 18 @@ -81,16 +81,14 @@ #define ACC #define USE_ACC_MPU6050 #define USE_ACC_LSM303DLHC - -#define ACC_MPU6050_ALIGN CW0_DEG +#define ACC_MPU6050_ALIGN CW0_DEG #define BARO #define USE_BARO_MS5611 #define MAG #define USE_MAG_AK8975 - -#define MAG_AK8975_ALIGN CW90_DEG_FLIP +#define MAG_AK8975_ALIGN CW90_DEG_FLIP #define USE_VCP #define USE_UART1 @@ -101,20 +99,19 @@ #define I2C_DEVICE (I2CDEV_1) #define USE_ADC -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PC0 -#define CURRENT_METER_ADC_PIN PC1 -#define RSSI_ADC_PIN PC2 -#define EXTERNAL1_ADC_PIN PC3 +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PC0 +#define CURRENT_METER_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define EXTERNAL1_ADC_PIN PC3 // IO - assuming 303 in 64pin package, TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)|BIT(5)|BIT(6)|BIT(10)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTE 0xffff -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)|BIT(9)|BIT(10)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)|BIT(5)|BIT(6)|BIT(10)|BIT(12)|BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)|BIT(9)|BIT(10)) - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index d78895ec84..8b4b846c40 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -132,15 +132,15 @@ #define I2C2_SCL PB10 #define I2C2_SDA PB11 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define VBAT_ADC_PIN PC3 -#define CURRENT_METER_ADC_PIN PC2 -#define RSSI_ADC_PIN PC1 +#define USE_ADC +#define VBAT_ADC_PIN PC3 +#define CURRENT_METER_ADC_PIN PC2 +#define RSSI_ADC_PIN PC1 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) -#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index d795a22415..bb270cb4fc 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -23,12 +23,12 @@ //#define OLIMEXINO_UNCUT_LED2_E_JUMPER #ifdef OLIMEXINO_UNCUT_LED1_E_JUMPER -#define LED0 PA5 // D13, PA5/SPI1_SCK/ADC5 - "LED1" on silkscreen, Green +#define LED0 PA5 // D13, PA5/SPI1_SCK/ADC5 - "LED1" on silkscreen, Green #endif #ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER // "LED2" is using one of the PWM pins (CH2/PWM2), so we must not use PWM2 unless the jumper is cut. @See pwmInit() -#define LED1 PA1 // D3, PA1/UART2_RTS/ADC1/TIM2_CH3 - "LED2" on silkscreen, Yellow +#define LED1 PA1 // D3, PA1/UART2_RTS/ADC1/TIM2_CH3 - "LED2" on silkscreen, Yellow #endif #define GYRO @@ -59,14 +59,14 @@ #define USE_MAG_HMC5883 #define SONAR -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN PB0 #define USE_UART1 #define USE_UART2 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 #define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 @@ -86,10 +86,9 @@ // IO - assuming all IOs on smt32f103rb LQFP64 package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index b9119c5fd3..87c5c286a9 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -21,9 +21,9 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0 PB3 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 10 // 6 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins. @@ -35,19 +35,19 @@ #define MPU_INT_EXTI PC13 #define USE_EXTI -#define MPU6000_SPI_INSTANCE SPI1 -#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 #define GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW90_DEG +#define GYRO_MPU6000_ALIGN CW90_DEG #define ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW90_DEG +#define ACC_MPU6000_ALIGN CW90_DEG -#define BMP280_SPI_INSTANCE SPI1 -#define BMP280_CS_PIN PA13 +#define BMP280_SPI_INSTANCE SPI1 +#define BMP280_CS_PIN PA13 #define BARO #define USE_BARO_BMP280 @@ -60,13 +60,12 @@ //#define MAG_AK8975_ALIGN CW90_DEG_FLIP //#define SONAR -//#define SONAR_ECHO_PIN PB1 -//#define SONAR_TRIGGER_PIN PB0 +//#define SONAR_ECHO_PIN PB1 +//#define SONAR_TRIGGER_PIN PB0 #define USB_IO #define USB_CABLE_DETECTION - -#define USB_DETECT_PIN PB5 +#define USB_DETECT_PIN PB5 #define USE_VCP #define USE_UART1 @@ -74,14 +73,14 @@ #define USE_UART3 #define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) //#define USE_I2C //#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA @@ -100,8 +99,8 @@ #define OSD // include the max7456 driver #define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI1 -#define MAX7456_SPI_CS_PIN PB1 +#define MAX7456_SPI_INSTANCE SPI1 +#define MAX7456_SPI_CS_PIN PB1 //#define MAX7456_DMA_CHANNEL_TX DMA1_Channel3 //#define MAX7456_DMA_CHANNEL_RX DMA1_Channel2 //#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_CH3_HANDLER @@ -169,27 +168,28 @@ #define REDUCE_TRANSPONDER_CURRENT_DRAW_WHEN_USB_CABLE_PRESENT #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_ONESHOT125) + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_ONESHOT125) #define BUTTONS -#define BUTTON_A_PORT GPIOB -#define BUTTON_A_PIN Pin_1 -#define BUTTON_B_PORT GPIOB -#define BUTTON_B_PIN Pin_0 +#define BUTTON_A_PORT GPIOB +#define BUTTON_A_PIN Pin_1 +#define BUTTON_B_PORT GPIOB +#define BUTTON_B_PIN Pin_0 #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define HARDWARE_BIND_PLUG -#define BINDPLUG_PIN PB0 +#define BINDPLUG_PIN PB0 #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index bb6e33717b..111f6fc476 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -134,7 +134,7 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) // #define TARGET_IO_PORTF (BIT(0)|BIT(1)) // !!TODO - check the following line is correct -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 22e5a396da..66e83fa3d5 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -19,36 +19,36 @@ #define TARGET_BOARD_IDENTIFIER "103R" -#define LED0 PB3 // PB3 (LED) -#define LED1 PB4 // PB4 (LED) -#define LED2 PD2 // PD2 (LED) - Labelled LED4 +#define LED0 PB3 +#define LED1 PB4 +#define LED2 PD2 // PD2 (LED) - Labelled LED4 -#define BEEPER PA12 // PA12 (Beeper) +#define BEEPER PA12 // PA12 (Beeper) -#define BARO_XCLR_GPIO GPIOC -#define BARO_XCLR_PIN PC13 -#define BARO_EOC_GPIO GPIOC -#define BARO_EOC_PIN PC14 -#define BARO_APB2_PERIPHERALS RCC_APB2Periph_GPIOC +#define BARO_XCLR_GPIO GPIOC +#define BARO_XCLR_PIN PC13 +#define BARO_EOC_GPIO GPIOC +#define BARO_EOC_PIN PC14 +#define BARO_APB2_PERIPHERALS RCC_APB2Periph_GPIOC -#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO -#define INVERTER_USART USART2 +#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO +#define INVERTER_USART USART2 #define USE_SPI #define USE_SPI_DEVICE_2 -#define PORT103R_SPI_INSTANCE SPI2 -#define PORT103R_SPI_CS_PIN PB12 +#define PORT103R_SPI_INSTANCE SPI2 +#define PORT103R_SPI_CS_PIN PB12 // We either have this 16mbit flash chip on SPI or the MPU6500 acc/gyro depending on board revision: -#define M25P16_CS_PIN PORT103R_SPI_CS_PIN -#define M25P16_SPI_INSTANCE PORT103R_SPI_INSTANCE +#define M25P16_CS_PIN PORT103R_SPI_CS_PIN +#define M25P16_SPI_INSTANCE PORT103R_SPI_INSTANCE -#define MPU6000_CS_PIN PORT103R_SPI_CS_PIN -#define MPU6000_SPI_INSTANCE PORT103R_SPI_INSTANCE +#define MPU6000_CS_PIN PORT103R_SPI_CS_PIN +#define MPU6000_SPI_INSTANCE PORT103R_SPI_INSTANCE -#define MPU6500_CS_PIN PORT103R_SPI_CS_PIN -#define MPU6500_SPI_INSTANCE PORT103R_SPI_INSTANCE +#define MPU6500_CS_PIN PORT103R_SPI_CS_PIN +#define MPU6500_SPI_INSTANCE PORT103R_SPI_INSTANCE #define GYRO #define USE_FAKE_GYRO @@ -93,17 +93,17 @@ #define USE_UART2 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER TIM3 #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 #define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 #define USE_I2C -#define I2C_DEVICE (I2CDEV_2) +#define I2C_DEVICE (I2CDEV_2) // #define SOFT_I2C // enable to test software i2c // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) @@ -118,12 +118,10 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f103RCT6 in 64pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) - +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) - diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 06e9f3cfee..09a5678174 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -21,23 +21,23 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PB3 +#define LED0 PB3 -#define BEEPER PC15 +#define BEEPER PC15 #define USABLE_TIMER_CHANNEL_COUNT 10 #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL #define GYRO #define USE_GYRO_MPU6050 -#define GYRO_MPU6050_ALIGN CW0_DEG_FLIP +#define GYRO_MPU6050_ALIGN CW0_DEG_FLIP #define ACC #define USE_ACC_MPU6050 -#define ACC_MPU6050_ALIGN CW0_DEG_FLIP +#define ACC_MPU6050_ALIGN CW0_DEG_FLIP #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -49,22 +49,22 @@ #define USE_SOFTSERIAL1 // Telemetry #define SERIAL_PORT_COUNT 5 -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 //Not connected -#define UART2_RX_PIN PA15 +#define UART2_TX_PIN PA14 //Not connected +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 -#define SOFTSERIAL_1_TIMER TIM15 +#define SOFTSERIAL_1_TIMER TIM15 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 7 //Not connected #define SOFTSERIAL_1_TIMER_TX_HARDWARE 8 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_1 // PA4, 5, 6, 7 @@ -81,9 +81,9 @@ #define USE_ADC -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PB2 -#define VBAT_SCALE_DEFAULT 77 +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PB2 +#define VBAT_SCALE_DEFAULT 77 #define LED_STRIP @@ -103,17 +103,17 @@ #define SPEKTRUM_BIND // USART2, PA15 -#define BIND_PIN PA15 +#define BIND_PIN PA15 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) //#define TARGET_IO_PORTF (BIT(0)|BIT(1)) // !!TODO - check the following line is correct -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 42f2378c66..0b2ca5f969 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -19,14 +19,14 @@ #define TARGET_BOARD_IDENTIFIER "SIRF" -#define LED0 PB2 -#define BEEPER PA1 +#define LED0 PB2 +#define BEEPER PA1 #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU_INT #define USE_EXTI #define USE_MPU_DATA_READY_SIGNAL -#define MPU_INT_EXTI PA8 +#define MPU_INT_EXTI PA8 #define ENSURE_MPU_DATA_READY_IS_LOW #define GYRO @@ -40,18 +40,18 @@ #define USE_ACC_SPI_MPU6500 // MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG -#define GYRO_MPU6000_ALIGN CW180_DEG +#define ACC_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 // MPU6500 -#define ACC_MPU6500_ALIGN CW90_DEG -#define GYRO_MPU6500_ALIGN CW90_DEG +#define ACC_MPU6500_ALIGN CW90_DEG +#define GYRO_MPU6500_ALIGN CW90_DEG -#define MPU6500_CS_PIN PA4 -#define MPU6500_SPI_INSTANCE SPI1 +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_INSTANCE SPI1 #define USB_IO @@ -59,16 +59,16 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA2 // PA14 / SWCLK -#define UART2_RX_PIN PA3 // PA15 +#define UART2_TX_PIN PA2 // PA14 / SWCLK +#define UART2_RX_PIN PA3 // PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #undef USE_I2C @@ -93,17 +93,17 @@ #define SPI3_MOSI_PIN PB5 #define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI3 -#define MAX7456_SPI_CS_PIN PA15 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PA15 #define MAX7456_DMA_CHANNEL_TX DMA2_Channel2 #define MAX7456_DMA_CHANNEL_RX DMA2_Channel1 #define MAX7456_DMA_IRQ_HANDLER_ID DMA2_CH1_HANDLER #define USE_RTC6705 -#define RTC6705_SPIDATA_PIN PC15 -#define RTC6705_SPILE_PIN PC14 -#define RTC6705_SPICLK_PIN PC13 +#define RTC6705_SPIDATA_PIN PC15 +#define RTC6705_SPILE_PIN PC14 +#define RTC6705_SPICLK_PIN PC13 #define USE_SDCARD #define USE_SDCARD_SPI2 @@ -124,32 +124,33 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - -#define ADC_INSTANCE ADC1 -#define VBAT_ADC_PIN PA0 +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define VBAT_ADC_PIN PA0 //#define USE_QUAD_MIXER_ONLY #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + #define OSD #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define USE_SERVOS -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL | FEATURE_OSD | FEATURE_VTX) + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL | FEATURE_OSD | FEATURE_VTX) #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 6 -#define USED_TIMERS (TIM_N(3) | TIM_N(4)) -#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) -#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOB) +#define USED_TIMERS (TIM_N(3) | TIM_N(4)) +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOB) diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 704845c1ee..9494f612c5 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -163,5 +163,5 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 4a38e233fb..024a3a6c28 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -22,33 +22,31 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PC14 -#define BEEPER PC15 +#define LED0 PC14 +#define BEEPER PC15 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 17 - - #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH -#define MPU6000_CS_PIN PA15 -#define MPU6000_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN PA15 +#define MPU6000_SPI_INSTANCE SPI1 #define GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_MPU6000_ALIGN CW270_DEG #define ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_MPU6000_ALIGN CW270_DEG // MPU6000 interrupts #define USE_MPU_DATA_READY_SIGNAL #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PC13 +#define MPU_INT_EXTI PC13 #define USE_EXTI @@ -60,27 +58,27 @@ #define USE_UART3 #define USE_SOFTSERIAL1 #define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 5 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA2 // PA14 / SWCLK -#define UART2_RX_PIN PA3 // PA15 +#define UART2_TX_PIN PA2 // PA14 / SWCLK +#define UART2_RX_PIN PA3 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) -#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER TIM3 #define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 #define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER TIM3 #define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 #define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_1 @@ -100,12 +98,12 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP @@ -119,21 +117,20 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES FEATURE_BLACKBOX +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) - -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h index aaa3635da6..c65abb7d92 100644 --- a/src/main/target/ZCOREF3/target.h +++ b/src/main/target/ZCOREF3/target.h @@ -21,32 +21,30 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT -#define LED0 PB8 +#define LED0 PB8 -#define BEEPER PC15 +#define BEEPER PC15 #define BEEPER_INVERTED #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH - #define GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 +#define GYRO_MPU6500_ALIGN CW180_DEG #define ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 - -#define ACC_MPU6500_ALIGN CW180_DEG -#define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG #define BARO #define USE_BARO_BMP280 @@ -54,17 +52,17 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 3 +#define SERIAL_PORT_COUNT 3 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) @@ -84,25 +82,25 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 17 // PPM, 8 PWM, UART3 RX/TX, LED Strip -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) From a5a0024fbd26d35b2cd2e3ade6b68d580f8fabc3 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 16 Jul 2016 11:05:46 +0200 Subject: [PATCH 039/456] Fix Q parameter not being used from input --- src/main/common/filter.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 98d9dc3c58..90c3decdae 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -67,7 +67,7 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh const float omega = 2 * M_PI_FLOAT * filterFreq / sampleRate; const float sn = sinf(omega); const float cs = cosf(omega); - const float alpha = sn / (2 * BIQUAD_Q); + const float alpha = sn / (2 * Q); float b0, b1, b2, a0, a1, a2; From 63c7ae18ff8dc5d3a95f861956cbb00501b95f1d Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 16 Jul 2016 11:24:05 +0200 Subject: [PATCH 040/456] Revert "Betaflight led strip resubmit" --- src/main/common/utils.h | 11 - src/main/config/config.c | 4 +- src/main/config/config_master.h | 6 +- src/main/drivers/light_ws2811strip.h | 3 + .../drivers/light_ws2811strip_stm32f10x.c | 1 - .../drivers/light_ws2811strip_stm32f30x.c | 1 - .../drivers/light_ws2811strip_stm32f4xx.c | 1 - src/main/io/ledstrip.c | 1559 ++++++++--------- src/main/io/ledstrip.h | 206 +-- src/main/io/serial_cli.c | 58 +- src/main/io/serial_msp.c | 64 +- src/main/io/serial_msp.h | 4 +- src/main/main.c | 4 +- src/main/sensors/battery.c | 2 +- 14 files changed, 809 insertions(+), 1115 deletions(-) diff --git a/src/main/common/utils.h b/src/main/common/utils.h index 27a51f3984..a5c0591918 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -71,15 +71,4 @@ http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html static inline int16_t cmp16(uint16_t a, uint16_t b) { return a-b; } static inline int32_t cmp32(uint32_t a, uint32_t b) { return a-b; } -// using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions -// than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions) -#ifdef UNIT_TEST -// Call memcpy when building unittest - this is easier that asm symbol name mangling (symbols start with _underscore on win32) -#include -static inline void memcpy_fn ( void * destination, const void * source, size_t num ) { memcpy(destination, source, num); }; -#else -void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("memcpy"); -#endif - - #endif diff --git a/src/main/config/config.c b/src/main/config/config.c index 6407623c1f..0caa5c62e7 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -615,10 +615,8 @@ static void resetConf(void) } #ifdef LED_STRIP - applyDefaultColors(masterConfig.colors); + applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT); applyDefaultLedStripConfig(masterConfig.ledConfigs); - applyDefaultModeColors(masterConfig.modeColors); - applyDefaultSpecialColors(&(masterConfig.specialColors)); masterConfig.ledstrip_visual_beeper = 0; #endif diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 29a236e565..5ce1d36833 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -119,10 +119,8 @@ typedef struct master_t { telemetryConfig_t telemetryConfig; #ifdef LED_STRIP - ledConfig_t ledConfigs[LED_MAX_STRIP_LENGTH]; - hsvColor_t colors[LED_CONFIGURABLE_COLOR_COUNT]; - modeColorIndexes_t modeColors[LED_MODE_COUNT]; - specialColorIndexes_t specialColors; + ledConfig_t ledConfigs[MAX_LED_STRIP_LENGTH]; + hsvColor_t colors[CONFIGURABLE_COLOR_COUNT]; uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on #endif diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 853ac7acbc..974c19c655 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -53,3 +53,6 @@ bool isWS2811LedStripReady(void); extern uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; extern volatile uint8_t ws2811LedDataTransferInProgress; + +extern const hsvColor_t hsv_white; +extern const hsvColor_t hsv_black; diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index b4a9b54268..96bb85fd26 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -105,7 +105,6 @@ void ws2811LedStripHardwareInit(void) DMA_ITConfig(DMA1_Channel6, DMA_IT_TC, ENABLE); - const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 722dd9a2e4..ff52a64836 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -111,7 +111,6 @@ void ws2811LedStripHardwareInit(void) DMA_ITConfig(WS2811_DMA_CHANNEL, DMA_IT_TC, ENABLE); - const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index e8676e2cdf..377954ce99 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -154,7 +154,6 @@ void ws2811LedStripHardwareInit(void) dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); - const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 4ab463bd1c..e74ca001a0 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -42,7 +42,6 @@ #include #include "common/axis.h" -#include "common/utils.h" #include "io/rc_controls.h" @@ -62,8 +61,6 @@ #include "io/osd.h" #include "io/vtx.h" -#include "rx/rx.h" - #include "flight/failsafe.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -77,13 +74,6 @@ #include "config/config_profile.h" #include "config/config_master.h" -/* -PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0); -PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0); -PG_REGISTER_ARR_WITH_RESET_FN(modeColorIndexes_t, LED_MODE_COUNT, modeColors, PG_MODE_COLOR_CONFIG, 0); -PG_REGISTER_WITH_RESET_FN(specialColorIndexes_t, specialColors, PG_SPECIAL_COLOR_CONFIG, 0); -*/ - static bool ledStripInitialised = false; static bool ledStripEnabled = true; @@ -92,13 +82,73 @@ static void ledStripDisable(void); //#define USE_LED_ANIMATION //#define USE_LED_RING_DEFAULT_CONFIG -#define LED_STRIP_HZ(hz) ((int32_t)((1000 * 1000) / (hz))) -#define LED_STRIP_MS(ms) ((int32_t)(1000 * (ms))) - -#if LED_MAX_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH -# error "Led strip length must match driver" +// timers +#ifdef USE_LED_ANIMATION +static uint32_t nextAnimationUpdateAt = 0; #endif +static uint32_t nextIndicatorFlashAt = 0; +static uint32_t nextWarningFlashAt = 0; +static uint32_t nextRotationUpdateAt = 0; + +#define LED_STRIP_20HZ ((1000 * 1000) / 20) +#define LED_STRIP_10HZ ((1000 * 1000) / 10) +#define LED_STRIP_5HZ ((1000 * 1000) / 5) + +#if MAX_LED_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH +#error "Led strip length must match driver" +#endif + +// H S V +#define LED_BLACK { 0, 0, 0} +#define LED_WHITE { 0, 255, 255} +#define LED_RED { 0, 0, 255} +#define LED_ORANGE { 30, 0, 255} +#define LED_YELLOW { 60, 0, 255} +#define LED_LIME_GREEN { 90, 0, 255} +#define LED_GREEN {120, 0, 255} +#define LED_MINT_GREEN {150, 0, 255} +#define LED_CYAN {180, 0, 255} +#define LED_LIGHT_BLUE {210, 0, 255} +#define LED_BLUE {240, 0, 255} +#define LED_DARK_VIOLET {270, 0, 255} +#define LED_MAGENTA {300, 0, 255} +#define LED_DEEP_PINK {330, 0, 255} + +const hsvColor_t hsv_black = LED_BLACK; +const hsvColor_t hsv_white = LED_WHITE; +const hsvColor_t hsv_red = LED_RED; +const hsvColor_t hsv_orange = LED_ORANGE; +const hsvColor_t hsv_yellow = LED_YELLOW; +const hsvColor_t hsv_limeGreen = LED_LIME_GREEN; +const hsvColor_t hsv_green = LED_GREEN; +const hsvColor_t hsv_mintGreen = LED_MINT_GREEN; +const hsvColor_t hsv_cyan = LED_CYAN; +const hsvColor_t hsv_lightBlue = LED_LIGHT_BLUE; +const hsvColor_t hsv_blue = LED_BLUE; +const hsvColor_t hsv_darkViolet = LED_DARK_VIOLET; +const hsvColor_t hsv_magenta = LED_MAGENTA; +const hsvColor_t hsv_deepPink = LED_DEEP_PINK; + +#define LED_DIRECTION_COUNT 6 + +const hsvColor_t * const defaultColors[] = { + &hsv_black, + &hsv_white, + &hsv_red, + &hsv_orange, + &hsv_yellow, + &hsv_limeGreen, + &hsv_green, + &hsv_mintGreen, + &hsv_cyan, + &hsv_lightBlue, + &hsv_blue, + &hsv_darkViolet, + &hsv_magenta, + &hsv_deepPink +}; + typedef enum { COLOR_BLACK = 0, COLOR_WHITE, @@ -114,134 +164,98 @@ typedef enum { COLOR_DARK_VIOLET, COLOR_MAGENTA, COLOR_DEEP_PINK, -} colorId_e; +} colorIds; -const hsvColor_t hsv[] = { - // H S V - [COLOR_BLACK] = { 0, 0, 0}, - [COLOR_WHITE] = { 0, 255, 255}, - [COLOR_RED] = { 0, 0, 255}, - [COLOR_ORANGE] = { 30, 0, 255}, - [COLOR_YELLOW] = { 60, 0, 255}, - [COLOR_LIME_GREEN] = { 90, 0, 255}, - [COLOR_GREEN] = {120, 0, 255}, - [COLOR_MINT_GREEN] = {150, 0, 255}, - [COLOR_CYAN] = {180, 0, 255}, - [COLOR_LIGHT_BLUE] = {210, 0, 255}, - [COLOR_BLUE] = {240, 0, 255}, - [COLOR_DARK_VIOLET] = {270, 0, 255}, - [COLOR_MAGENTA] = {300, 0, 255}, - [COLOR_DEEP_PINK] = {330, 0, 255}, +typedef enum { + DIRECTION_NORTH = 0, + DIRECTION_EAST, + DIRECTION_SOUTH, + DIRECTION_WEST, + DIRECTION_UP, + DIRECTION_DOWN +} directionId_e; + +typedef struct modeColorIndexes_s { + uint8_t north; + uint8_t east; + uint8_t south; + uint8_t west; + uint8_t up; + uint8_t down; +} modeColorIndexes_t; + + +// Note, the color index used for the mode colors below refer to the default colors. +// if the colors are reconfigured the index is still valid but the displayed color might +// be different. +// See colors[] and defaultColors[] and applyDefaultColors[] + +static const modeColorIndexes_t orientationModeColors = { + COLOR_WHITE, + COLOR_DARK_VIOLET, + COLOR_RED, + COLOR_DEEP_PINK, + COLOR_BLUE, + COLOR_ORANGE }; -// macro to save typing on default colors -#define HSV(color) (hsv[COLOR_ ## color]) -STATIC_UNIT_TESTED uint8_t ledGridWidth; -STATIC_UNIT_TESTED uint8_t ledGridHeight; -// grid offsets -STATIC_UNIT_TESTED uint8_t highestYValueForNorth; -STATIC_UNIT_TESTED uint8_t lowestYValueForSouth; -STATIC_UNIT_TESTED uint8_t highestXValueForWest; -STATIC_UNIT_TESTED uint8_t lowestXValueForEast; - -STATIC_UNIT_TESTED ledCounts_t ledCounts; - -// macro for initializer -#define LF(name) LED_FUNCTION_ ## name -#define LO(name) LED_FLAG_OVERLAY(LED_OVERLAY_ ## name) -#define LD(name) LED_FLAG_DIRECTION(LED_DIRECTION_ ## name) - -#ifdef USE_LED_RING_DEFAULT_CONFIG -static const ledConfig_t defaultLedStripConfig[] = { - DEFINE_LED( 2, 2, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 2, 1, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 2, 0, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 1, 0, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 0, 0, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 0, 1, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 0, 2, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 1, 2, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), +static const modeColorIndexes_t headfreeModeColors = { + COLOR_LIME_GREEN, + COLOR_DARK_VIOLET, + COLOR_ORANGE, + COLOR_DEEP_PINK, + COLOR_BLUE, + COLOR_ORANGE }; -#else -static const ledConfig_t defaultLedStripConfig[] = { - DEFINE_LED(15, 15, 0, LD(SOUTH) | LD(EAST), LF(ARM_STATE), LO(INDICATOR), 0), - DEFINE_LED(15, 8, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0), - DEFINE_LED(15, 7, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0), +static const modeColorIndexes_t horizonModeColors = { + COLOR_BLUE, + COLOR_DARK_VIOLET, + COLOR_YELLOW, + COLOR_DEEP_PINK, + COLOR_BLUE, + COLOR_ORANGE +}; - DEFINE_LED(15, 0, 0, LD(NORTH) | LD(EAST), LF(ARM_STATE) , LO(INDICATOR), 0), - - DEFINE_LED( 8, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0), - DEFINE_LED( 7, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0), - - DEFINE_LED( 0, 0, 0, LD(NORTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0), - - DEFINE_LED( 0, 7, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0), - DEFINE_LED( 0, 8, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0), - - DEFINE_LED( 0, 15, 0, LD(SOUTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0), - - DEFINE_LED( 7, 15, 0, LD(SOUTH) , LF(FLIGHT_MODE), LO(WARNING), 0), - DEFINE_LED( 8, 15, 0, LD(SOUTH) , LF(FLIGHT_MODE), LO(WARNING), 0), - - DEFINE_LED( 7, 7, 0, LD(UP) , LF(FLIGHT_MODE), LO(WARNING), 0), - DEFINE_LED( 8, 7, 0, LD(UP) , LF(FLIGHT_MODE), LO(WARNING), 0), - DEFINE_LED( 7, 8, 0, LD(DOWN) , LF(FLIGHT_MODE), LO(WARNING), 0), - DEFINE_LED( 8, 8, 0, LD(DOWN) , LF(FLIGHT_MODE), LO(WARNING), 0), - - DEFINE_LED( 8, 9, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 9, 10, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED(10, 11, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED(10, 12, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 9, 13, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 8, 14, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 7, 14, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 6, 13, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 5, 12, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 5, 11, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 6, 10, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 7, 9, 3, 0, LF(THRUST_RING), 0, 0), +static const modeColorIndexes_t angleModeColors = { + COLOR_CYAN, + COLOR_DARK_VIOLET, + COLOR_YELLOW, + COLOR_DEEP_PINK, + COLOR_BLUE, + COLOR_ORANGE +}; +#ifdef MAG +static const modeColorIndexes_t magModeColors = { + COLOR_MINT_GREEN, + COLOR_DARK_VIOLET, + COLOR_ORANGE, + COLOR_DEEP_PINK, + COLOR_BLUE, + COLOR_ORANGE }; #endif -#undef LD -#undef LF -#undef LO - -static const modeColorIndexes_t defaultModeColors[] = { - // NORTH EAST SOUTH WEST UP DOWN - [LED_MODE_ORIENTATION] = {{ COLOR_WHITE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, - [LED_MODE_HEADFREE] = {{ COLOR_LIME_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, - [LED_MODE_HORIZON] = {{ COLOR_BLUE, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, - [LED_MODE_ANGLE] = {{ COLOR_CYAN, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, - [LED_MODE_MAG] = {{ COLOR_MINT_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, - [LED_MODE_BARO] = {{ COLOR_LIGHT_BLUE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, -}; - -static const specialColorIndexes_t defaultSpecialColors[] = { - {{ [LED_SCOLOR_DISARMED] = COLOR_GREEN, - [LED_SCOLOR_ARMED] = COLOR_BLUE, - [LED_SCOLOR_ANIMATION] = COLOR_WHITE, - [LED_SCOLOR_BACKGROUND] = COLOR_BLACK, - [LED_SCOLOR_BLINKBACKGROUND] = COLOR_BLACK, - [LED_SCOLOR_GPSNOSATS] = COLOR_RED, - [LED_SCOLOR_GPSNOLOCK] = COLOR_ORANGE, - [LED_SCOLOR_GPSLOCKED] = COLOR_GREEN, - }} +static const modeColorIndexes_t baroModeColors = { + COLOR_LIGHT_BLUE, + COLOR_DARK_VIOLET, + COLOR_RED, + COLOR_DEEP_PINK, + COLOR_BLUE, + COLOR_ORANGE }; -static int scaledThrottle; +uint8_t ledGridWidth; +uint8_t ledGridHeight; +uint8_t ledCount; +uint8_t ledsInRingCount; + +ledConfig_t *ledConfigs; +hsvColor_t *colors; - - -/* #ifdef USE_LED_RING_DEFAULT_CONFIG const ledConfig_t defaultLedStripConfig[] = { { CALCULATE_LED_XY( 2, 2), 3, LED_FUNCTION_THRUST_RING}, @@ -259,16 +273,16 @@ const ledConfig_t defaultLedStripConfig[] = { }; #elif defined(USE_COLIBTI_RACE_LED_DEFAULT_CONFIG) const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, }; #else const ledConfig_t defaultLedStripConfig[] = { @@ -312,58 +326,118 @@ const ledConfig_t defaultLedStripConfig[] = { }; #endif -*/ +/* + * 6 coords @nn,nn + * 4 direction @## + * 6 modes @#### + * = 16 bytes per led + * 16 * 32 leds = 512 bytes storage needed worst case. + * = not efficient to store led configs as strings in flash. + * = becomes a problem to send all the data via cli due to serial/cli buffers + */ -static void updateLedRingCounts(void); +typedef enum { + X_COORDINATE, + Y_COORDINATE, + DIRECTIONS, + FUNCTIONS, + RING_COLORS +} parseState_e; -STATIC_UNIT_TESTED void determineLedStripDimensions(void) +#define PARSE_STATE_COUNT 5 + +static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0' }; + +static const char directionCodes[] = { 'N', 'E', 'S', 'W', 'U', 'D' }; +#define DIRECTION_COUNT (sizeof(directionCodes) / sizeof(directionCodes[0])) +static const uint8_t directionMappings[DIRECTION_COUNT] = { + LED_DIRECTION_NORTH, + LED_DIRECTION_EAST, + LED_DIRECTION_SOUTH, + LED_DIRECTION_WEST, + LED_DIRECTION_UP, + LED_DIRECTION_DOWN +}; + +static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T', 'R', 'C' }; +#define FUNCTION_COUNT (sizeof(functionCodes) / sizeof(functionCodes[0])) +static const uint16_t functionMappings[FUNCTION_COUNT] = { + LED_FUNCTION_INDICATOR, + LED_FUNCTION_WARNING, + LED_FUNCTION_FLIGHT_MODE, + LED_FUNCTION_ARM_STATE, + LED_FUNCTION_THROTTLE, + LED_FUNCTION_THRUST_RING, + LED_FUNCTION_COLOR +}; + +// grid offsets +uint8_t highestYValueForNorth; +uint8_t lowestYValueForSouth; +uint8_t highestXValueForWest; +uint8_t lowestXValueForEast; + +void determineLedStripDimensions(void) { - int maxX = 0; - int maxY = 0; + ledGridWidth = 0; + ledGridHeight = 0; - for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + uint8_t ledIndex; + const ledConfig_t *ledConfig; - maxX = MAX(ledGetX(ledConfig), maxX); - maxY = MAX(ledGetY(ledConfig), maxY); + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { + ledConfig = &ledConfigs[ledIndex]; + + if (GET_LED_X(ledConfig) >= ledGridWidth) { + ledGridWidth = GET_LED_X(ledConfig) + 1; + } + if (GET_LED_Y(ledConfig) >= ledGridHeight) { + ledGridHeight = GET_LED_Y(ledConfig) + 1; + } } - ledGridWidth = maxX + 1; - ledGridHeight = maxY + 1; } -STATIC_UNIT_TESTED void determineOrientationLimits(void) +void determineOrientationLimits(void) { + bool isOddHeight = (ledGridHeight & 1); + bool isOddWidth = (ledGridWidth & 1); + uint8_t heightModifier = isOddHeight ? 1 : 0; + uint8_t widthModifier = isOddWidth ? 1 : 0; + highestYValueForNorth = (ledGridHeight / 2) - 1; - lowestYValueForSouth = ((ledGridHeight + 1) / 2); + lowestYValueForSouth = (ledGridHeight / 2) + heightModifier; highestXValueForWest = (ledGridWidth / 2) - 1; - lowestXValueForEast = ((ledGridWidth + 1) / 2); + lowestXValueForEast = (ledGridWidth / 2) + widthModifier; } -STATIC_UNIT_TESTED void updateLedCount(void) +void updateLedCount(void) { - int count = 0, countRing = 0, countScanner= 0; + const ledConfig_t *ledConfig; + uint8_t ledIndex; + ledCount = 0; + ledsInRingCount = 0; - for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - - if (!(*ledConfig)) - break; - - count++; - - if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) - countRing++; - - if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) - countScanner++; + if( ledConfigs == 0 ){ + return; } - ledCounts.count = count; - ledCounts.ring = countRing; - ledCounts.larson = countScanner; + for (ledIndex = 0; ledIndex < MAX_LED_STRIP_LENGTH; ledIndex++) { + + ledConfig = &ledConfigs[ledIndex]; + + if (ledConfig->flags == 0 && ledConfig->xy == 0) { + break; + } + + ledCount++; + + if ((ledConfig->flags & LED_FUNCTION_THRUST_RING)) { + ledsInRingCount++; + } + } } void reevaluateLedConfig(void) @@ -371,714 +445,499 @@ void reevaluateLedConfig(void) updateLedCount(); determineLedStripDimensions(); determineOrientationLimits(); - updateLedRingCounts(); } -// get specialColor by index -static hsvColor_t* getSC(ledSpecialColorIds_e index) +#define CHUNK_BUFFER_SIZE 10 + +#define NEXT_PARSE_STATE(parseState) ((parseState + 1) % PARSE_STATE_COUNT) + + +bool parseLedStripConfig(uint8_t ledIndex, const char *config) { - return &masterConfig.colors[masterConfig.specialColors.color[index]]; -} + char chunk[CHUNK_BUFFER_SIZE]; + uint8_t chunkIndex; + uint8_t val; -static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' }; -static const char baseFunctionCodes[LED_BASEFUNCTION_COUNT] = { 'C', 'F', 'A', 'L', 'S', 'G', 'R' }; -static const char overlayCodes[LED_OVERLAY_COUNT] = { 'T', 'O', 'B', 'N', 'I', 'W' }; + uint8_t parseState = X_COORDINATE; + bool ok = true; -#define CHUNK_BUFFER_SIZE 11 + if (ledIndex >= MAX_LED_STRIP_LENGTH) { + return !ok; + } -bool parseLedStripConfig(int ledIndex, const char *config) -{ - if (ledIndex >= LED_MAX_STRIP_LENGTH) - return false; - - enum parseState_e { - X_COORDINATE, - Y_COORDINATE, - DIRECTIONS, - FUNCTIONS, - RING_COLORS, - PARSE_STATE_COUNT - }; - static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0'}; - - ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + ledConfig_t *ledConfig = &ledConfigs[ledIndex]; memset(ledConfig, 0, sizeof(ledConfig_t)); - int x = 0, y = 0, color = 0; // initialize to prevent warnings - int baseFunction = 0; - int overlay_flags = 0; - int direction_flags = 0; + while (ok) { - for (enum parseState_e parseState = 0; parseState < PARSE_STATE_COUNT; parseState++) { - char chunk[CHUNK_BUFFER_SIZE]; - { - char chunkSeparator = chunkSeparators[parseState]; - int chunkIndex = 0; - while (*config && *config != chunkSeparator && chunkIndex < (CHUNK_BUFFER_SIZE - 1)) { - chunk[chunkIndex++] = *config++; - } - chunk[chunkIndex++] = 0; // zero-terminate chunk - if (*config != chunkSeparator) { - return false; - } - config++; // skip separator + char chunkSeparator = chunkSeparators[parseState]; + + memset(&chunk, 0, sizeof(chunk)); + chunkIndex = 0; + + while (*config && chunkIndex < CHUNK_BUFFER_SIZE && *config != chunkSeparator) { + chunk[chunkIndex++] = *config++; } - switch (parseState) { + + if (*config++ != chunkSeparator) { + ok = false; + break; + } + + switch((parseState_e)parseState) { case X_COORDINATE: - x = atoi(chunk); + val = atoi(chunk); + ledConfig->xy |= CALCULATE_LED_X(val); break; case Y_COORDINATE: - y = atoi(chunk); + val = atoi(chunk); + ledConfig->xy |= CALCULATE_LED_Y(val); break; case DIRECTIONS: - for (char *ch = chunk; *ch; ch++) { - for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) { - if (directionCodes[dir] == *ch) { - direction_flags |= LED_FLAG_DIRECTION(dir); + for (chunkIndex = 0; chunk[chunkIndex] && chunkIndex < CHUNK_BUFFER_SIZE; chunkIndex++) { + for (uint8_t mappingIndex = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) { + if (directionCodes[mappingIndex] == chunk[chunkIndex]) { + ledConfig->flags |= directionMappings[mappingIndex]; break; } } } break; case FUNCTIONS: - for (char *ch = chunk; *ch; ch++) { - for (ledBaseFunctionId_e fn = 0; fn < LED_BASEFUNCTION_COUNT; fn++) { - if (baseFunctionCodes[fn] == *ch) { - baseFunction = fn; - break; - } - } - - for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) { - if (overlayCodes[ol] == *ch) { - overlay_flags |= LED_FLAG_OVERLAY(ol); + for (chunkIndex = 0; chunk[chunkIndex] && chunkIndex < CHUNK_BUFFER_SIZE; chunkIndex++) { + for (uint8_t mappingIndex = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) { + if (functionCodes[mappingIndex] == chunk[chunkIndex]) { + ledConfig->flags |= functionMappings[mappingIndex]; break; } } } break; case RING_COLORS: - color = atoi(chunk); - if (color >= LED_CONFIGURABLE_COLOR_COUNT) - color = 0; + if (atoi(chunk) < CONFIGURABLE_COLOR_COUNT) { + ledConfig->color = atoi(chunk); + } else { + ledConfig->color = 0; + } break; - case PARSE_STATE_COUNT:; // prevent warning + default : + break; + } + + parseState++; + if (parseState >= PARSE_STATE_COUNT) { + break; } } - *ledConfig = DEFINE_LED(x, y, color, direction_flags, baseFunction, overlay_flags, 0); + if (!ok) { + memset(ledConfig, 0, sizeof(ledConfig_t)); + } reevaluateLedConfig(); - return true; + return ok; } -void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize) +void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize) { - char directions[LED_DIRECTION_COUNT + 1]; - char baseFunctionOverlays[LED_OVERLAY_COUNT + 2]; + char functions[FUNCTION_COUNT]; + char directions[DIRECTION_COUNT]; + uint8_t index; + uint8_t mappingIndex; - ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + ledConfig_t *ledConfig = &ledConfigs[ledIndex]; memset(ledConfigBuffer, 0, bufferSize); + memset(&functions, 0, sizeof(functions)); + memset(&directions, 0, sizeof(directions)); - char *dptr = directions; - for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) { - if (ledGetDirectionBit(ledConfig, dir)) { - *dptr++ = directionCodes[dir]; + for (mappingIndex = 0, index = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) { + if (ledConfig->flags & functionMappings[mappingIndex]) { + functions[index++] = functionCodes[mappingIndex]; } } - *dptr = 0; - char *fptr = baseFunctionOverlays; - *fptr++ = baseFunctionCodes[ledGetFunction(ledConfig)]; - - for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) { - if (ledGetOverlayBit(ledConfig, ol)) { - *fptr++ = overlayCodes[ol]; + for (mappingIndex = 0, index = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) { + if (ledConfig->flags & directionMappings[mappingIndex]) { + directions[index++] = directionCodes[mappingIndex]; } } - *fptr = 0; - // TODO - check buffer length - sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig)); + sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions, ledConfig->color); } +void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const modeColorIndexes_t *modeColors) +{ + // apply up/down colors regardless of quadrant. + if ((ledConfig->flags & LED_DIRECTION_UP)) { + setLedHsv(ledIndex, &colors[modeColors->up]); + } + + if ((ledConfig->flags & LED_DIRECTION_DOWN)) { + setLedHsv(ledIndex, &colors[modeColors->down]); + } + + // override with n/e/s/w colors to each n/s e/w half - bail at first match. + if ((ledConfig->flags & LED_DIRECTION_WEST) && GET_LED_X(ledConfig) <= highestXValueForWest) { + setLedHsv(ledIndex, &colors[modeColors->west]); + } + + if ((ledConfig->flags & LED_DIRECTION_EAST) && GET_LED_X(ledConfig) >= lowestXValueForEast) { + setLedHsv(ledIndex, &colors[modeColors->east]); + } + + if ((ledConfig->flags & LED_DIRECTION_NORTH) && GET_LED_Y(ledConfig) <= highestYValueForNorth) { + setLedHsv(ledIndex, &colors[modeColors->north]); + } + + if ((ledConfig->flags & LED_DIRECTION_SOUTH) && GET_LED_Y(ledConfig) >= lowestYValueForSouth) { + setLedHsv(ledIndex, &colors[modeColors->south]); + } + +} typedef enum { - // the ordering is important, see below how NSEW is mapped to NE/SE/NW/SW - QUADRANT_NORTH = 1 << 0, - QUADRANT_SOUTH = 1 << 1, - QUADRANT_EAST = 1 << 2, - QUADRANT_WEST = 1 << 3, - QUADRANT_NORTH_EAST = 1 << 4, - QUADRANT_SOUTH_EAST = 1 << 5, - QUADRANT_NORTH_WEST = 1 << 6, - QUADRANT_SOUTH_WEST = 1 << 7, - QUADRANT_NONE = 1 << 8, - QUADRANT_NOTDIAG = 1 << 9, // not in NE/SE/NW/SW - // values for test - QUADRANT_ANY = QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST | QUADRANT_NONE, + QUADRANT_NORTH_EAST = 1, + QUADRANT_SOUTH_EAST, + QUADRANT_SOUTH_WEST, + QUADRANT_NORTH_WEST } quadrant_e; -static quadrant_e getLedQuadrant(const int ledIndex) +void applyQuadrantColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const quadrant_e quadrant, const hsvColor_t *color) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + switch (quadrant) { + case QUADRANT_NORTH_EAST: + if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) >= lowestXValueForEast) { + setLedHsv(ledIndex, color); + } + return; - int x = ledGetX(ledConfig); - int y = ledGetY(ledConfig); + case QUADRANT_SOUTH_EAST: + if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) >= lowestXValueForEast) { + setLedHsv(ledIndex, color); + } + return; - int quad = 0; - if (y <= highestYValueForNorth) - quad |= QUADRANT_NORTH; - else if (y >= lowestYValueForSouth) - quad |= QUADRANT_SOUTH; - if (x >= lowestXValueForEast) - quad |= QUADRANT_EAST; - else if (x <= highestXValueForWest) - quad |= QUADRANT_WEST; + case QUADRANT_SOUTH_WEST: + if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) <= highestXValueForWest) { + setLedHsv(ledIndex, color); + } + return; - if ((quad & (QUADRANT_NORTH | QUADRANT_SOUTH)) - && (quad & (QUADRANT_EAST | QUADRANT_WEST)) ) { // is led in one of NE/SE/NW/SW? - quad |= 1 << (4 + ((quad & QUADRANT_SOUTH) ? 1 : 0) + ((quad & QUADRANT_WEST) ? 2 : 0)); - } else { - quad |= QUADRANT_NOTDIAG; + case QUADRANT_NORTH_WEST: + if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) <= highestXValueForWest) { + setLedHsv(ledIndex, color); + } + return; } - - if ((quad & (QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST)) == 0) - quad |= QUADRANT_NONE; - - return quad; } -static const struct { - uint8_t dir; // ledDirectionId_e - uint16_t quadrantMask; // quadrant_e -} directionQuadrantMap[] = { - {LED_DIRECTION_SOUTH, QUADRANT_SOUTH}, - {LED_DIRECTION_NORTH, QUADRANT_NORTH}, - {LED_DIRECTION_EAST, QUADRANT_EAST}, - {LED_DIRECTION_WEST, QUADRANT_WEST}, - {LED_DIRECTION_DOWN, QUADRANT_ANY}, - {LED_DIRECTION_UP, QUADRANT_ANY}, -}; - -static hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIndexes_t *modeColors) +void applyLedModeLayer(void) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + const ledConfig_t *ledConfig; - quadrant_e quad = getLedQuadrant(ledIndex); - for (unsigned i = 0; i < ARRAYLEN(directionQuadrantMap); i++) { - ledDirectionId_e dir = directionQuadrantMap[i].dir; - quadrant_e quadMask = directionQuadrantMap[i].quadrantMask; + uint8_t ledIndex; + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - if (ledGetDirectionBit(ledConfig, dir) && (quad & quadMask)) - return &masterConfig.colors[modeColors->color[dir]]; - } - return NULL; -} + ledConfig = &ledConfigs[ledIndex]; + if (!(ledConfig->flags & LED_FUNCTION_THRUST_RING)) { + if (ledConfig->flags & LED_FUNCTION_COLOR) { + setLedHsv(ledIndex, &colors[ledConfig->color]); + } else { + setLedHsv(ledIndex, &hsv_black); + } + } -// map flight mode to led mode, in order of priority -// flightMode == 0 is always active -static const struct { - uint16_t flightMode; - uint8_t ledMode; -} flightModeToLed[] = { - {HEADFREE_MODE, LED_MODE_HEADFREE}, + if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) { + if (ledConfig->flags & LED_FUNCTION_ARM_STATE) { + if (!ARMING_FLAG(ARMED)) { + setLedHsv(ledIndex, &hsv_green); + } else { + setLedHsv(ledIndex, &hsv_blue); + } + } + continue; + } + + applyDirectionalModeColor(ledIndex, ledConfig, &orientationModeColors); + + if (FLIGHT_MODE(HEADFREE_MODE)) { + applyDirectionalModeColor(ledIndex, ledConfig, &headfreeModeColors); #ifdef MAG - {MAG_MODE, LED_MODE_MAG}, + } else if (FLIGHT_MODE(MAG_MODE)) { + applyDirectionalModeColor(ledIndex, ledConfig, &magModeColors); #endif #ifdef BARO - {BARO_MODE, LED_MODE_BARO}, + } else if (FLIGHT_MODE(BARO_MODE)) { + applyDirectionalModeColor(ledIndex, ledConfig, &baroModeColors); #endif - {HORIZON_MODE, LED_MODE_HORIZON}, - {ANGLE_MODE, LED_MODE_ANGLE}, - {0, LED_MODE_ORIENTATION}, -}; - -static void applyLedFixedLayers() -{ - for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND); - - int fn = ledGetFunction(ledConfig); - int hOffset = HSV_HUE_MAX; - - switch (fn) { - case LED_FUNCTION_COLOR: - color = masterConfig.colors[ledGetColor(ledConfig)]; - break; - - case LED_FUNCTION_FLIGHT_MODE: - for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) - if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { - color = *getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]); - break; // stop on first match - } - break; - - case LED_FUNCTION_ARM_STATE: - color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED); - break; - - case LED_FUNCTION_BATTERY: - color = HSV(RED); - hOffset += scaleRange(calculateBatteryCapacityRemainingPercentage(), 0, 100, -30, 120); - break; - - case LED_FUNCTION_RSSI: - color = HSV(RED); - hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120); - break; - - default: - break; + } else if (FLIGHT_MODE(HORIZON_MODE)) { + applyDirectionalModeColor(ledIndex, ledConfig, &horizonModeColors); + } else if (FLIGHT_MODE(ANGLE_MODE)) { + applyDirectionalModeColor(ledIndex, ledConfig, &angleModeColors); } - - if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { - hOffset += ((scaledThrottle - 10) * 4) / 3; - } - - color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1); - - setLedHsv(ledIndex, &color); - - } -} - -static void applyLedHsv(uint32_t mask, const hsvColor_t *color) -{ - for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - if ((*ledConfig & mask) == mask) - setLedHsv(ledIndex, color); } } typedef enum { - WARNING_ARMING_DISABLED, - WARNING_LOW_BATTERY, - WARNING_FAILSAFE, + WARNING_FLAG_NONE = 0, + WARNING_FLAG_LOW_BATTERY = (1 << 0), + WARNING_FLAG_FAILSAFE = (1 << 1), + WARNING_FLAG_ARMING_DISABLED = (1 << 2) } warningFlags_e; +static uint8_t warningFlags = WARNING_FLAG_NONE; -static void applyLedWarningLayer(bool updateNow, uint32_t *timer) +void applyLedWarningLayer(uint8_t updateNow) { + const ledConfig_t *ledConfig; + uint8_t ledIndex; static uint8_t warningFlashCounter = 0; - static uint8_t warningFlags = 0; // non-zero during blinks - if (updateNow) { - // keep counter running, so it stays in sync with blink - warningFlashCounter++; - warningFlashCounter &= 0xF; - - if (warningFlashCounter == 0) { // update when old flags was processed - warningFlags = 0; - if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) - warningFlags |= 1 << WARNING_LOW_BATTERY; - if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) - warningFlags |= 1 << WARNING_FAILSAFE; - if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) - warningFlags |= 1 << WARNING_ARMING_DISABLED; + if (updateNow && warningFlashCounter == 0) { + warningFlags = WARNING_FLAG_NONE; + if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) { + warningFlags |= WARNING_FLAG_LOW_BATTERY; + } + if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) { + warningFlags |= WARNING_FLAG_FAILSAFE; + } + if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) { + warningFlags |= WARNING_FLAG_ARMING_DISABLED; } - *timer += LED_STRIP_HZ(10); } - if (warningFlags) { - const hsvColor_t *warningColor = NULL; + if (warningFlags || warningFlashCounter > 0) { + const hsvColor_t *warningColor = &hsv_black; - bool colorOn = (warningFlashCounter % 2) == 0; // w_w_ - warningFlags_e warningId = warningFlashCounter / 4; - if (warningFlags & (1 << warningId)) { - switch (warningId) { - case WARNING_ARMING_DISABLED: - warningColor = colorOn ? &HSV(GREEN) : NULL; - break; - case WARNING_LOW_BATTERY: - warningColor = colorOn ? &HSV(RED) : NULL; - break; - case WARNING_FAILSAFE: - warningColor = colorOn ? &HSV(YELLOW) : &HSV(BLUE); - break; - default:; + if ((warningFlashCounter & 1) == 0) { + if (warningFlashCounter < 4 && (warningFlags & WARNING_FLAG_ARMING_DISABLED)) { + warningColor = &hsv_green; + } + if (warningFlashCounter >= 4 && warningFlashCounter < 12 && (warningFlags & WARNING_FLAG_LOW_BATTERY)) { + warningColor = &hsv_red; + } + if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) { + warningColor = &hsv_yellow; + } + } else { + if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) { + warningColor = &hsv_blue; } } - if (warningColor) - applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor); - } -} -static void applyLedBatteryLayer(bool updateNow, uint32_t *timer) -{ - static bool flash = false; + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - int state; - int timeOffset = 1; + ledConfig = &ledConfigs[ledIndex]; - if (updateNow) { - state = getBatteryState(); - - switch (state) { - case BATTERY_OK: - flash = false; - timeOffset = 1; - break; - case BATTERY_WARNING: - timeOffset = 2; - break; - default: - timeOffset = 8; - break; - } - flash = !flash; - } - - *timer += LED_STRIP_HZ(timeOffset); - - if (!flash) { - hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); - applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_BATTERY), bgc); - } -} - -static void applyLedRssiLayer(bool updateNow, uint32_t *timer) -{ - static bool flash = false; - - int state; - int timeOffset = 0; - - if (updateNow) { - state = (rssi * 100) / 1023; - - if (state > 50) { - flash = false; - timeOffset = 1; - } else if (state > 20) { - timeOffset = 2; - } else { - timeOffset = 8; - } - flash = !flash; - } - - - *timer += LED_STRIP_HZ(timeOffset); - - if (!flash) { - hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); - applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_RSSI), bgc); - } -} - -#ifdef GPS -static void applyLedGpsLayer(bool updateNow, uint32_t *timer) -{ - static uint8_t gpsFlashCounter = 0; - static uint8_t gpsPauseCounter = 0; - const uint8_t blinkPauseLength = 4; - - if (updateNow) { - if (gpsPauseCounter > 0) { - gpsPauseCounter--; - } else if (gpsFlashCounter >= GPS_numSat) { - gpsFlashCounter = 0; - gpsPauseCounter = blinkPauseLength; - } else { - gpsFlashCounter++; - gpsPauseCounter = 1; - } - *timer += LED_STRIP_HZ(2.5); - } - - const hsvColor_t *gpsColor; - - if (GPS_numSat == 0 || !sensors(SENSOR_GPS)) { - gpsColor = getSC(LED_SCOLOR_GPSNOSATS); - } else { - bool colorOn = gpsPauseCounter == 0; // each interval starts with pause - if (STATE(GPS_FIX)) { - gpsColor = colorOn ? getSC(LED_SCOLOR_GPSLOCKED) : getSC(LED_SCOLOR_BACKGROUND); - } else { - gpsColor = colorOn ? getSC(LED_SCOLOR_GPSNOLOCK) : getSC(LED_SCOLOR_GPSNOSATS); + if (!(ledConfig->flags & LED_FUNCTION_WARNING)) { + continue; + } + setLedHsv(ledIndex, warningColor); } } - applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_GPS), gpsColor); + if (updateNow && (warningFlags || warningFlashCounter)) { + warningFlashCounter++; + if (warningFlashCounter == 20) { + warningFlashCounter = 0; + } + } } -#endif - - #define INDICATOR_DEADBAND 25 -static void applyLedIndicatorLayer(bool updateNow, uint32_t *timer) +void applyLedIndicatorLayer(uint8_t indicatorFlashState) { - static bool flash = 0; + const ledConfig_t *ledConfig; + static const hsvColor_t *flashColor; - if (updateNow) { - if (rxIsReceivingSignal()) { - // calculate update frequency - int scale = MAX(ABS(rcCommand[ROLL]), ABS(rcCommand[PITCH])); // 0 - 500 - scale += (50 - INDICATOR_DEADBAND); // start increasing frequency right after deadband - *timer += LED_STRIP_HZ(5) * 50 / MAX(50, scale); // 5 - 50Hz update, 2.5 - 25Hz blink - - flash = !flash; - } else { - *timer += LED_STRIP_HZ(5); // try again soon - } - } - - if (!flash) + if (!rxIsReceivingSignal()) { return; - - const hsvColor_t *flashColor = &HSV(ORANGE); // TODO - use user color? - - quadrant_e quadrants = 0; - if (rcCommand[ROLL] > INDICATOR_DEADBAND) { - quadrants |= QUADRANT_NORTH_EAST | QUADRANT_SOUTH_EAST; - } else if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { - quadrants |= QUADRANT_NORTH_WEST | QUADRANT_SOUTH_WEST; - } - if (rcCommand[PITCH] > INDICATOR_DEADBAND) { - quadrants |= QUADRANT_NORTH_EAST | QUADRANT_NORTH_WEST; - } else if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { - quadrants |= QUADRANT_SOUTH_EAST | QUADRANT_SOUTH_WEST; } - for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - if (ledGetOverlayBit(ledConfig, LED_OVERLAY_INDICATOR)) { - if (getLedQuadrant(ledIndex) & quadrants) - setLedHsv(ledIndex, flashColor); + if (indicatorFlashState == 0) { + flashColor = &hsv_orange; + } else { + flashColor = &hsv_black; + } + + + uint8_t ledIndex; + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { + + ledConfig = &ledConfigs[ledIndex]; + + if (!(ledConfig->flags & LED_FUNCTION_INDICATOR)) { + continue; } + + if (rcCommand[ROLL] > INDICATOR_DEADBAND) { + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); + } + + if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); + } + + if (rcCommand[PITCH] > INDICATOR_DEADBAND) { + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); + } + + if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); + } + } +} + +void applyLedThrottleLayer() +{ + const ledConfig_t *ledConfig; + hsvColor_t color; + + uint8_t ledIndex; + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { + ledConfig = &ledConfigs[ledIndex]; + if (!(ledConfig->flags & LED_FUNCTION_THROTTLE)) { + continue; + } + + getLedHsv(ledIndex, &color); + + int scaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, -60, +60); + scaled += HSV_HUE_MAX; + color.h = (color.h + scaled) % HSV_HUE_MAX; + setLedHsv(ledIndex, &color); } } #define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off -#define ROTATION_SEQUENCE_LED_WIDTH 2 // 2 on +#define ROTATION_SEQUENCE_LED_WIDTH 2 -static void updateLedRingCounts(void) +#define RING_PATTERN_NOT_CALCULATED 255 + +void applyLedThrustRingLayer(void) { - int seqLen; - // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds - if ((ledCounts.ring % ROTATION_SEQUENCE_LED_COUNT) == 0) { - seqLen = ROTATION_SEQUENCE_LED_COUNT; - } else { - seqLen = ledCounts.ring; - // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds - // TODO - improve partitioning (15 leds -> 3x5) - while ((seqLen > ROTATION_SEQUENCE_LED_COUNT) && ((seqLen % 2) == 0)) { - seqLen /= 2; + const ledConfig_t *ledConfig; + hsvColor_t ringColor; + uint8_t ledIndex; + + // initialised to special value instead of using more memory for a flag. + static uint8_t rotationSeqLedCount = RING_PATTERN_NOT_CALCULATED; + static uint8_t rotationPhase = ROTATION_SEQUENCE_LED_COUNT; + bool nextLedOn = false; + + uint8_t ledRingIndex = 0; + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { + + ledConfig = &ledConfigs[ledIndex]; + + if ((ledConfig->flags & LED_FUNCTION_THRUST_RING) == 0) { + continue; } - } - ledCounts.ringSeqLen = seqLen; -} -static void applyLedThrustRingLayer(bool updateNow, uint32_t *timer) -{ - static uint8_t rotationPhase; - int ledRingIndex = 0; - - if (updateNow) { - rotationPhase = rotationPhase > 0 ? rotationPhase - 1 : ledCounts.ringSeqLen - 1; - - int scale = scaledThrottle; // ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; - *timer += LED_STRIP_HZ(5) * 10 / scale; // 5 - 50Hz update rate - } - - for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) { - - bool applyColor; - if (ARMING_FLAG(ARMED)) { - applyColor = (ledRingIndex + rotationPhase) % ledCounts.ringSeqLen < ROTATION_SEQUENCE_LED_WIDTH; - } else { - applyColor = !(ledRingIndex % 2); // alternating pattern + bool applyColor = false; + if (ARMING_FLAG(ARMED)) { + if ((ledRingIndex + rotationPhase) % rotationSeqLedCount < ROTATION_SEQUENCE_LED_WIDTH) { + applyColor = true; } - - if (applyColor) { - const hsvColor_t *ringColor = &masterConfig.colors[ledGetColor(ledConfig)]; - setLedHsv(ledIndex, ringColor); + } else { + if (nextLedOn == false) { + applyColor = true; } - - ledRingIndex++; + nextLedOn = !nextLedOn; } - } -} -typedef struct larsonParameters_s { - uint8_t currentBrightness; - int8_t currentIndex; - int8_t direction; -} larsonParameters_t; - -static int brightnessForLarsonIndex(larsonParameters_t *larsonParameters, uint8_t larsonIndex) -{ - int offset = larsonIndex - larsonParameters->currentIndex; - static const int larsonLowValue = 8; - - if (ABS(offset) > 1) - return (larsonLowValue); - - if (offset == 0) - return (larsonParameters->currentBrightness); - - if (larsonParameters->direction == offset) { - return (larsonParameters->currentBrightness - 127); - } - - return (255 - larsonParameters->currentBrightness); - -} - -static void larsonScannerNextStep(larsonParameters_t *larsonParameters, int delta) -{ - if (larsonParameters->currentBrightness > (255 - delta)) { - larsonParameters->currentBrightness = 127; - if (larsonParameters->currentIndex >= ledCounts.larson || larsonParameters->currentIndex < 0) { - larsonParameters->direction = -larsonParameters->direction; + if (applyColor) { + ringColor = colors[ledConfig->color]; + } else { + ringColor = hsv_black; } - larsonParameters->currentIndex += larsonParameters->direction; - } else { - larsonParameters->currentBrightness += delta; - } -} -static void applyLarsonScannerLayer(bool updateNow, uint32_t *timer) -{ - static larsonParameters_t larsonParameters = { 0, 0, 1 }; + setLedHsv(ledIndex, &ringColor); - if (updateNow) { - larsonScannerNextStep(&larsonParameters, 15); - *timer += LED_STRIP_HZ(60); + ledRingIndex++; } - int scannerLedIndex = 0; - for (unsigned i = 0; i < ledCounts.count; i++) { + uint8_t ledRingLedCount = ledRingIndex; + if (rotationSeqLedCount == RING_PATTERN_NOT_CALCULATED) { + // update ring pattern according to total number of ring leds found - const ledConfig_t *ledConfig = &ledConfigs[i]; + rotationSeqLedCount = ledRingLedCount; - if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) { - hsvColor_t ledColor; - getLedHsv(i, &ledColor); - ledColor.v = brightnessForLarsonIndex(&larsonParameters, scannerLedIndex); - setLedHsv(i, &ledColor); - scannerLedIndex++; - } - } -} - -// blink twice, then wait ; either always or just when landing -static void applyLedBlinkLayer(bool updateNow, uint32_t *timer) -{ - const uint16_t blinkPattern = 0x8005; // 0b1000000000000101; - static uint16_t blinkMask; - - if (updateNow) { - blinkMask = blinkMask >> 1; - if (blinkMask <= 1) - blinkMask = blinkPattern; - - *timer += LED_STRIP_HZ(10); - } - - bool ledOn = (blinkMask & 1); // b_b_____... - if (!ledOn) { - for (int i = 0; i < ledCounts.count; ++i) { - const ledConfig_t *ledConfig = &ledConfigs[i]; - - if (ledGetOverlayBit(ledConfig, LED_OVERLAY_BLINK) || - (ledGetOverlayBit(ledConfig, LED_OVERLAY_LANDING_FLASH) && scaledThrottle < 55 && scaledThrottle > 10)) { - setLedHsv(i, getSC(LED_SCOLOR_BLINKBACKGROUND)); + // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds + if ((ledRingLedCount % ROTATION_SEQUENCE_LED_COUNT) == 0) { + rotationSeqLedCount = ROTATION_SEQUENCE_LED_COUNT; + } else { + // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds + while ((rotationSeqLedCount > ROTATION_SEQUENCE_LED_COUNT) && ((rotationSeqLedCount % 2) == 0)) { + rotationSeqLedCount >>= 1; } } + + // trigger start over + rotationPhase = 1; + } + + rotationPhase--; + if (rotationPhase == 0) { + rotationPhase = rotationSeqLedCount; } } - #ifdef USE_LED_ANIMATION +static uint8_t previousRow; +static uint8_t currentRow; +static uint8_t nextRow; -static void applyLedAnimationLayer(bool updateNow, uint32_t *timer) +void updateLedAnimationState(void) { static uint8_t frameCounter = 0; - const int animationFrames = ledGridHeight; - if(updateNow) { - frameCounter = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; - *timer += LED_STRIP_HZ(20); + + uint8_t animationFrames = ledGridHeight; + + previousRow = (frameCounter + animationFrames - 1) % animationFrames; + currentRow = frameCounter; + nextRow = (frameCounter + 1) % animationFrames; + + frameCounter = (frameCounter + 1) % animationFrames; +} + +static void applyLedAnimationLayer(void) +{ + const ledConfig_t *ledConfig; + + if (ARMING_FLAG(ARMED)) { + return; } - if (ARMING_FLAG(ARMED)) - return; + uint8_t ledIndex; + for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - int previousRow = frameCounter > 0 ? frameCounter - 1 : animationFrames - 1; - int currentRow = frameCounter; - int nextRow = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; + ledConfig = &ledConfigs[ledIndex]; - for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { - const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - - if (ledGetY(ledConfig) == previousRow) { - setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION)); + if (GET_LED_Y(ledConfig) == previousRow) { + setLedHsv(ledIndex, &hsv_white); scaleLedValue(ledIndex, 50); - } else if (ledGetY(ledConfig) == currentRow) { - setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION)); - } else if (ledGetY(ledConfig) == nextRow) { + + } else if (GET_LED_Y(ledConfig) == currentRow) { + setLedHsv(ledIndex, &hsv_white); + } else if (GET_LED_Y(ledConfig) == nextRow) { scaleLedValue(ledIndex, 50); } } } #endif -typedef enum { - timBlink, - timLarson, - timBattery, - timRssi, -#ifdef GPS - timGps, -#endif - timWarning, - timIndicator, -#ifdef USE_LED_ANIMATION - timAnimation, -#endif - timRing, - timTimerCount -} timId_e; - -static uint32_t timerVal[timTimerCount]; - - -// function to apply layer. -// function must replan self using timer pointer -// when updateNow is true (timer triggered), state must be updated first, -// before calculating led state. Otherwise update started by different trigger -// may modify LED state. -typedef void applyLayerFn_timed(bool updateNow, uint32_t *timer); - - -static applyLayerFn_timed* layerTable[] = { - [timBlink] = &applyLedBlinkLayer, - [timLarson] = &applyLarsonScannerLayer, - [timBattery] = &applyLedBatteryLayer, - [timRssi] = &applyLedRssiLayer, -#ifdef GPS - [timGps] = &applyLedGpsLayer, -#endif - [timWarning] = &applyLedWarningLayer, - [timIndicator] = &applyLedIndicatorLayer, -#ifdef USE_LED_ANIMATION - [timAnimation] = &applyLedAnimationLayer, -#endif - [timRing] = &applyLedThrustRingLayer -}; - void updateLedStrip(void) { + if (!(ledStripInitialised && isWS2811LedStripReady())) { return; } @@ -1088,173 +947,161 @@ void updateLedStrip(void) ledStripDisable(); ledStripEnabled = false; } + } else { + ledStripEnabled = true; + } + + if (!ledStripEnabled){ return; } - ledStripEnabled = true; + uint32_t now = micros(); - // test all led timers, setting corresponding bits - uint32_t timActive = 0; - for (timId_e timId = 0; timId < timTimerCount; timId++) { - // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value. - // max delay is limited to 5s - int32_t delta = cmp32(now, timerVal[timId]); - if (delta < 0 && delta > -LED_STRIP_MS(5000)) - continue; // not ready yet - timActive |= 1 << timId; - if (delta >= LED_STRIP_MS(100) || delta < 0) { - timerVal[timId] = now; + bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; + bool warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L; + bool rotationUpdateNow = (int32_t)(now - nextRotationUpdateAt) >= 0L; +#ifdef USE_LED_ANIMATION + bool animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L; +#endif + if (!( + indicatorFlashNow || + rotationUpdateNow || + warningFlashNow +#ifdef USE_LED_ANIMATION + || animationUpdateNow +#endif + )) { + return; + } + + static uint8_t indicatorFlashState = 0; + + // LAYER 1 + applyLedModeLayer(); + applyLedThrottleLayer(); + + // LAYER 2 + + if (warningFlashNow) { + nextWarningFlashAt = now + LED_STRIP_10HZ; + } + applyLedWarningLayer(warningFlashNow); + + // LAYER 3 + + if (indicatorFlashNow) { + + uint8_t rollScale = ABS(rcCommand[ROLL]) / 50; + uint8_t pitchScale = ABS(rcCommand[PITCH]) / 50; + uint8_t scale = MAX(rollScale, pitchScale); + nextIndicatorFlashAt = now + (LED_STRIP_5HZ / MAX(1, scale)); + + if (indicatorFlashState == 0) { + indicatorFlashState = 1; + } else { + indicatorFlashState = 0; } } - if (!timActive) - return; // no change this update, keep old state + applyLedIndicatorLayer(indicatorFlashState); - // apply all layers; triggered timed functions has to update timers - - scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; - - applyLedFixedLayers(); - - for (timId_e timId = 0; timId < ARRAYLEN(layerTable); timId++) { - uint32_t *timer = &timerVal[timId]; - bool updateNow = timActive & (1 << timId); - (*layerTable[timId])(updateNow, timer); +#ifdef USE_LED_ANIMATION + if (animationUpdateNow) { + nextAnimationUpdateAt = now + LED_STRIP_20HZ; + updateLedAnimationState(); } + applyLedAnimationLayer(); +#endif + + if (rotationUpdateNow) { + + applyLedThrustRingLayer(); + + uint8_t animationSpeedScale = 1; + + if (ARMING_FLAG(ARMED)) { + animationSpeedScale = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 1, 10); + } + + nextRotationUpdateAt = now + LED_STRIP_5HZ/animationSpeedScale; + } + ws2811UpdateStrip(); } -bool parseColor(int index, const char *colorConfig) +bool parseColor(uint8_t index, const char *colorConfig) { const char *remainingCharacters = colorConfig; - hsvColor_t *color = &masterConfig.colors[index]; + hsvColor_t *color = &colors[index]; - bool result = true; - static const uint16_t hsv_limit[HSV_COLOR_COMPONENT_COUNT] = { - [HSV_HUE] = HSV_HUE_MAX, - [HSV_SATURATION] = HSV_SATURATION_MAX, - [HSV_VALUE] = HSV_VALUE_MAX, - }; - for (int componentIndex = 0; result && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) { - int val = atoi(remainingCharacters); - if(val > hsv_limit[componentIndex]) { - result = false; - break; - } + bool ok = true; + + uint8_t componentIndex; + for (componentIndex = 0; ok && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) { + uint16_t val = atoi(remainingCharacters); switch (componentIndex) { case HSV_HUE: - color->h = val; + if (val > HSV_HUE_MAX) { + ok = false; + continue; + + } + colors[index].h = val; break; case HSV_SATURATION: - color->s = val; + if (val > HSV_SATURATION_MAX) { + ok = false; + continue; + } + colors[index].s = (uint8_t)val; break; case HSV_VALUE: - color->v = val; + if (val > HSV_VALUE_MAX) { + ok = false; + continue; + } + colors[index].v = (uint8_t)val; break; } - remainingCharacters = strchr(remainingCharacters, ','); + remainingCharacters = strstr(remainingCharacters, ","); if (remainingCharacters) { - remainingCharacters++; // skip separator + remainingCharacters++; } else { - if (componentIndex < HSV_COLOR_COMPONENT_COUNT - 1) { - result = false; + if (componentIndex < 2) { + ok = false; } } } - if (!result) { - memset(color, 0, sizeof(*color)); + if (!ok) { + memset(color, 0, sizeof(hsvColor_t)); } - return result; + return ok; } -/* - * Redefine a color in a mode. - * */ -bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) +void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount) { - // check color - if (colorIndex < 0 || colorIndex >= LED_CONFIGURABLE_COLOR_COUNT) - return false; - if (modeIndex < LED_MODE_COUNT) { // modeIndex_e is unsigned, so one-sided test is enough - if(modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT) - return false; - masterConfig.modeColors[modeIndex].color[modeColorIndex] = colorIndex; - } else if (modeIndex == LED_SPECIAL) { - if (modeColorIndex < 0 || modeColorIndex >= LED_SPECIAL_COLOR_COUNT) - return false; - masterConfig.specialColors.color[modeColorIndex] = colorIndex; - } else { - return false; - } - return true; -} - -/* -void pgResetFn_ledConfigs(ledConfig_t *instance) -{ - memcpy_fn(instance, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); -} - -void pgResetFn_colors(hsvColor_t *instance) -{ - // copy hsv colors as default - BUILD_BUG_ON(ARRAYLEN(*colors_arr()) < ARRAYLEN(hsv)); - - for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { - *instance++ = hsv[colorIndex]; + memset(colors, 0, colorCount * sizeof(hsvColor_t)); + for (uint8_t colorIndex = 0; colorIndex < colorCount && colorIndex < (sizeof(defaultColors) / sizeof(defaultColors[0])); colorIndex++) { + *colors++ = *defaultColors[colorIndex]; } } -void pgResetFn_modeColors(modeColorIndexes_t *instance) -{ - memcpy_fn(instance, &defaultModeColors, sizeof(defaultModeColors)); -} - -void pgResetFn_specialColors(specialColorIndexes_t *instance) -{ - memcpy_fn(instance, &defaultSpecialColors, sizeof(defaultSpecialColors)); -} -*/ - void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) { - memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); + memset(ledConfigs, 0, MAX_LED_STRIP_LENGTH * sizeof(ledConfig_t)); memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); reevaluateLedConfig(); } -void applyDefaultColors(hsvColor_t *colors) -{ - // copy hsv colors as default - memset(colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t)); - for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { - *colors++ = hsv[colorIndex]; - } -} - -void applyDefaultModeColors(modeColorIndexes_t *modeColors) -{ - memcpy_fn(modeColors, &defaultModeColors, sizeof(defaultModeColors)); -} - -void applyDefaultSpecialColors(specialColorIndexes_t *specialColors) -{ - memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors)); -} - - - -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse) +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse) { ledConfigs = ledConfigsToUse; colors = colorsToUse; - modeColors = modeColorsToUse; - specialColors = *specialColorsToUse; ledStripInitialised = false; } @@ -1268,8 +1115,8 @@ void ledStripEnable(void) static void ledStripDisable(void) { - setStripColor(&HSV(BLACK)); - - ws2811UpdateStrip(); + setStripColor(&hsv_black); + + ws2811UpdateStrip(); } #endif diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 39b3cad6d2..18915229b7 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -17,163 +17,81 @@ #pragma once -#define LED_MAX_STRIP_LENGTH 32 -#define LED_CONFIGURABLE_COLOR_COUNT 16 -#define LED_MODE_COUNT 6 -#define LED_DIRECTION_COUNT 6 -#define LED_BASEFUNCTION_COUNT 7 -#define LED_OVERLAY_COUNT 6 -#define LED_SPECIAL_COLOR_COUNT 11 - -#define LED_POS_OFFSET 0 -#define LED_FUNCTION_OFFSET 8 -#define LED_OVERLAY_OFFSET 12 -#define LED_COLOR_OFFSET 18 -#define LED_DIRECTION_OFFSET 22 -#define LED_PARAMS_OFFSET 28 - -#define LED_POS_BITCNT 8 -#define LED_FUNCTION_BITCNT 4 -#define LED_OVERLAY_BITCNT 6 -#define LED_COLOR_BITCNT 4 -#define LED_DIRECTION_BITCNT 6 -#define LED_PARAMS_BITCNT 4 - -#define LED_FLAG_OVERLAY_MASK ((1 << LED_OVERLAY_BITCNT) - 1) -#define LED_FLAG_DIRECTION_MASK ((1 << LED_DIRECTION_BITCNT) - 1) - -#define LED_MOV_POS(pos) ((pos) << LED_POS_OFFSET) -#define LED_MOV_FUNCTION(func) ((func) << LED_FUNCTION_OFFSET) -#define LED_MOV_OVERLAY(overlay) ((overlay) << LED_OVERLAY_OFFSET) -#define LED_MOV_COLOR(colorId) ((colorId) << LED_COLOR_OFFSET) -#define LED_MOV_DIRECTION(direction) ((direction) << LED_DIRECTION_OFFSET) -#define LED_MOV_PARAMS(param) ((param) << LED_PARAMS_OFFSET) - -#define LED_BIT_MASK(len) ((1 << (len)) - 1) - -#define LED_POS_MASK LED_MOV_POS(((1 << LED_POS_BITCNT) - 1)) -#define LED_FUNCTION_MASK LED_MOV_FUNCTION(((1 << LED_FUNCTION_BITCNT) - 1)) -#define LED_OVERLAY_MASK LED_MOV_OVERLAY(LED_FLAG_OVERLAY_MASK) -#define LED_COLOR_MASK LED_MOV_COLOR(((1 << LED_COLOR_BITCNT) - 1)) -#define LED_DIRECTION_MASK LED_MOV_DIRECTION(LED_FLAG_DIRECTION_MASK) -#define LED_PARAMS_MASK LED_MOV_PARAMS(((1 << LED_PARAMS_BITCNT) - 1)) - -#define LED_FLAG_OVERLAY(id) (1 << (id)) -#define LED_FLAG_DIRECTION(id) (1 << (id)) +#define MAX_LED_STRIP_LENGTH 32 +#define CONFIGURABLE_COLOR_COUNT 16 #define LED_X_BIT_OFFSET 4 #define LED_Y_BIT_OFFSET 0 -#define LED_XY_MASK 0x0F -#define CALCULATE_LED_XY(x, y) ((((x) & LED_XY_MASK) << LED_X_BIT_OFFSET) | (((y) & LED_XY_MASK) << LED_Y_BIT_OFFSET)) + +#define LED_XY_MASK (0x0F) + +#define GET_LED_X(ledConfig) ((ledConfig->xy >> LED_X_BIT_OFFSET) & LED_XY_MASK) +#define GET_LED_Y(ledConfig) ((ledConfig->xy >> LED_Y_BIT_OFFSET) & LED_XY_MASK) + +#define CALCULATE_LED_X(x) ((x & LED_XY_MASK) << LED_X_BIT_OFFSET) +#define CALCULATE_LED_Y(y) ((y & LED_XY_MASK) << LED_Y_BIT_OFFSET) + + +#define CALCULATE_LED_XY(x,y) (CALCULATE_LED_X(x) | CALCULATE_LED_Y(y)) typedef enum { - LED_MODE_ORIENTATION = 0, - LED_MODE_HEADFREE, - LED_MODE_HORIZON, - LED_MODE_ANGLE, - LED_MODE_MAG, - LED_MODE_BARO, - LED_SPECIAL -} ledModeIndex_e; + LED_DISABLED = 0, + LED_DIRECTION_NORTH = (1 << 0), + LED_DIRECTION_EAST = (1 << 1), + LED_DIRECTION_SOUTH = (1 << 2), + LED_DIRECTION_WEST = (1 << 3), + LED_DIRECTION_UP = (1 << 4), + LED_DIRECTION_DOWN = (1 << 5), + LED_FUNCTION_INDICATOR = (1 << 6), + LED_FUNCTION_WARNING = (1 << 7), + LED_FUNCTION_FLIGHT_MODE = (1 << 8), + LED_FUNCTION_ARM_STATE = (1 << 9), + LED_FUNCTION_THROTTLE = (1 << 10), + LED_FUNCTION_THRUST_RING = (1 << 11), + LED_FUNCTION_COLOR = (1 << 12), +} ledFlag_e; -typedef enum { - LED_SCOLOR_DISARMED = 0, - LED_SCOLOR_ARMED, - LED_SCOLOR_ANIMATION, - LED_SCOLOR_BACKGROUND, - LED_SCOLOR_BLINKBACKGROUND, - LED_SCOLOR_GPSNOSATS, - LED_SCOLOR_GPSNOLOCK, - LED_SCOLOR_GPSLOCKED -} ledSpecialColorIds_e; - -typedef enum { - LED_DIRECTION_NORTH = 0, - LED_DIRECTION_EAST, - LED_DIRECTION_SOUTH, - LED_DIRECTION_WEST, - LED_DIRECTION_UP, - LED_DIRECTION_DOWN -} ledDirectionId_e; - -typedef enum { - LED_FUNCTION_COLOR, - LED_FUNCTION_FLIGHT_MODE, - LED_FUNCTION_ARM_STATE, - LED_FUNCTION_BATTERY, - LED_FUNCTION_RSSI, - LED_FUNCTION_GPS, - LED_FUNCTION_THRUST_RING, -} ledBaseFunctionId_e; - -typedef enum { - LED_OVERLAY_THROTTLE, - LED_OVERLAY_LARSON_SCANNER, - LED_OVERLAY_BLINK, - LED_OVERLAY_LANDING_FLASH, - LED_OVERLAY_INDICATOR, - LED_OVERLAY_WARNING, -} ledOverlayId_e; - -typedef struct modeColorIndexes_s { - uint8_t color[LED_DIRECTION_COUNT]; -} modeColorIndexes_t; - -typedef struct specialColorIndexes_s { - uint8_t color[LED_SPECIAL_COLOR_COUNT]; -} specialColorIndexes_t; - -typedef uint32_t ledConfig_t; - -typedef struct ledCounts_s { - uint8_t count; - uint8_t ring; - uint8_t larson; - uint8_t ringSeqLen; -} ledCounts_t; +#define LED_DIRECTION_BIT_OFFSET 0 +#define LED_DIRECTION_MASK ( \ + LED_DIRECTION_NORTH | \ + LED_DIRECTION_EAST | \ + LED_DIRECTION_SOUTH | \ + LED_DIRECTION_WEST | \ + LED_DIRECTION_UP | \ + LED_DIRECTION_DOWN \ +) +#define LED_FUNCTION_BIT_OFFSET 6 +#define LED_FUNCTION_MASK ( \ + LED_FUNCTION_INDICATOR | \ + LED_FUNCTION_WARNING | \ + LED_FUNCTION_FLIGHT_MODE | \ + LED_FUNCTION_ARM_STATE | \ + LED_FUNCTION_THROTTLE | \ + LED_FUNCTION_THRUST_RING | \ + LED_FUNCTION_COLOR \ +) -ledConfig_t *ledConfigs; -hsvColor_t *colors; -modeColorIndexes_t *modeColors; -specialColorIndexes_t specialColors; +typedef struct ledConfig_s { + uint8_t xy; // see LED_X/Y_MASK defines + uint8_t color; // see colors (config_master) + uint16_t flags; // see ledFlag_e +} ledConfig_t; -#define DEFINE_LED(x, y, col, dir, func, ol, params) (LED_MOV_POS(CALCULATE_LED_XY(x, y)) | LED_MOV_COLOR(col) | LED_MOV_DIRECTION(dir) | LED_MOV_FUNCTION(func) | LED_MOV_OVERLAY(ol) | LED_MOV_PARAMS(params)) +extern uint8_t ledCount; +extern uint8_t ledsInRingCount; -static inline uint8_t ledGetXY(const ledConfig_t *lcfg) { return ((*lcfg >> LED_POS_OFFSET) & LED_BIT_MASK(LED_POS_BITCNT)); } -static inline uint8_t ledGetX(const ledConfig_t *lcfg) { return ((*lcfg >> (LED_POS_OFFSET + LED_X_BIT_OFFSET)) & LED_XY_MASK); } -static inline uint8_t ledGetY(const ledConfig_t *lcfg) { return ((*lcfg >> (LED_POS_OFFSET + LED_Y_BIT_OFFSET)) & LED_XY_MASK); } -static inline uint8_t ledGetFunction(const ledConfig_t *lcfg) { return ((*lcfg >> LED_FUNCTION_OFFSET) & LED_BIT_MASK(LED_FUNCTION_BITCNT)); } -static inline uint8_t ledGetOverlay(const ledConfig_t *lcfg) { return ((*lcfg >> LED_OVERLAY_OFFSET) & LED_BIT_MASK(LED_OVERLAY_BITCNT)); } -static inline uint8_t ledGetColor(const ledConfig_t *lcfg) { return ((*lcfg >> LED_COLOR_OFFSET) & LED_BIT_MASK(LED_COLOR_BITCNT)); } -static inline uint8_t ledGetDirection(const ledConfig_t *lcfg) { return ((*lcfg >> LED_DIRECTION_OFFSET) & LED_BIT_MASK(LED_DIRECTION_BITCNT)); } -static inline uint8_t ledGetParams(const ledConfig_t *lcfg) { return ((*lcfg >> LED_PARAMS_OFFSET) & LED_BIT_MASK(LED_PARAMS_BITCNT)); } -static inline bool ledGetOverlayBit(const ledConfig_t *lcfg, int id) { return ((ledGetOverlay(lcfg) >> id) & 1); } -static inline bool ledGetDirectionBit(const ledConfig_t *lcfg, int id) { return ((ledGetDirection(lcfg) >> id) & 1); } -/* -PG_DECLARE_ARR(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs); -PG_DECLARE_ARR(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors); -PG_DECLARE_ARR(modeColorIndexes_t, LED_MODE_COUNT, modeColors); -PG_DECLARE(specialColorIndexes_t, specialColors); -*/ -bool parseColor(int index, const char *colorConfig); -bool parseLedStripConfig(int ledIndex, const char *config); -void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize); -void reevaluateLedConfig(void); - -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); -void ledStripEnable(void); +bool parseLedStripConfig(uint8_t ledIndex, const char *config); void updateLedStrip(void); - -bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex); - -extern uint16_t rssi; // FIXME dependency on mw.c - +void updateLedRing(void); void applyDefaultLedStripConfig(ledConfig_t *ledConfig); -void applyDefaultColors(hsvColor_t *colors); -void applyDefaultModeColors(modeColorIndexes_t *modeColors); -void applyDefaultSpecialColors(specialColorIndexes_t *specialColors); +void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize); +bool parseColor(uint8_t index, const char *colorConfig); +void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount); + +void ledStripEnable(void); +void reevaluateLedConfig(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 1e76d49197..4904764252 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -156,7 +156,6 @@ static void cliMap(char *cmdline); #ifdef LED_STRIP static void cliLed(char *cmdline); static void cliColor(char *cmdline); -static void cliModeColor(char *cmdline); #endif #ifndef USE_QUAD_MIXER_ONLY @@ -265,7 +264,6 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux), #ifdef LED_STRIP CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), - CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor), #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), @@ -1406,20 +1404,20 @@ static void cliLed(char *cmdline) char ledConfigBuffer[20]; if (isEmpty(cmdline)) { - for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { + for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); cliPrintf("led %u %s\r\n", i, ledConfigBuffer); } } else { ptr = cmdline; i = atoi(ptr); - if (i < LED_MAX_STRIP_LENGTH) { + if (i < MAX_LED_STRIP_LENGTH) { ptr = strchr(cmdline, ' '); if (!parseLedStripConfig(i, ++ptr)) { cliShowParseError(); } } else { - cliShowArgumentRangeError("index", 0, LED_MAX_STRIP_LENGTH - 1); + cliShowArgumentRangeError("index", 0, MAX_LED_STRIP_LENGTH - 1); } } } @@ -1430,7 +1428,7 @@ static void cliColor(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { + for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { cliPrintf("color %u %d,%u,%u\r\n", i, masterConfig.colors[i].h, @@ -1441,57 +1439,16 @@ static void cliColor(char *cmdline) } else { ptr = cmdline; i = atoi(ptr); - if (i < LED_CONFIGURABLE_COLOR_COUNT) { + if (i < CONFIGURABLE_COLOR_COUNT) { ptr = strchr(cmdline, ' '); if (!parseColor(i, ++ptr)) { cliShowParseError(); } } else { - cliShowArgumentRangeError("index", 0, LED_CONFIGURABLE_COLOR_COUNT - 1); + cliShowArgumentRangeError("index", 0, CONFIGURABLE_COLOR_COUNT - 1); } } } - -static void cliModeColor(char *cmdline) -{ - if (isEmpty(cmdline)) { - for (int i = 0; i < LED_MODE_COUNT; i++) { - for (int j = 0; j < LED_DIRECTION_COUNT; j++) { - int colorIndex = modeColors[i].color[j]; - cliPrintf("mode_color %u %u %u\r\n", i, j, colorIndex); - } - } - - for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { - int colorIndex = specialColors.color[j]; - cliPrintf("mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex); - } - } else { - enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; - int args[ARGS_COUNT]; - int argNo = 0; - char* ptr = strtok(cmdline, " "); - while (ptr && argNo < ARGS_COUNT) { - args[argNo++] = atoi(ptr); - ptr = strtok(NULL, " "); - } - - if (ptr != NULL || argNo != ARGS_COUNT) { - cliShowParseError(); - return; - } - - int modeIdx = args[MODE]; - int funIdx = args[FUNCTION]; - int color = args[COLOR]; - if(!setModeColor(modeIdx, funIdx, color)) { - cliShowParseError(); - return; - } - // values are validated - cliPrintf("mode_color %u %u %u\r\n", modeIdx, funIdx, color); - } -} #endif #ifdef USE_SERVOS @@ -2116,9 +2073,6 @@ static void cliDump(char *cmdline) cliPrint("\r\n\r\n# color\r\n"); cliColor(""); - - cliPrint("\r\n\r\n# mode_color\r\n"); - cliModeColor(""); #endif cliPrint("\r\n# aux\r\n"); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 666b6c7728..e7736441ec 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1150,8 +1150,8 @@ static bool processOutCommand(uint8_t cmdMSP) #ifdef LED_STRIP case MSP_LED_COLORS: - headSerialReply(LED_CONFIGURABLE_COLOR_COUNT * 4); - for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { + headSerialReply(CONFIGURABLE_COLOR_COUNT * 4); + for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; serialize16(color->h); serialize8(color->s); @@ -1160,27 +1160,14 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_LED_STRIP_CONFIG: - headSerialReply(LED_MAX_STRIP_LENGTH * 4); - for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { + headSerialReply(MAX_LED_STRIP_LENGTH * 7); + for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - serialize32(*ledConfig); - } - break; - - case MSP_LED_STRIP_MODECOLOR: - headSerialReply(((LED_MODE_COUNT * LED_DIRECTION_COUNT) + LED_SPECIAL_COLOR_COUNT) * 3); - for (int i = 0; i < LED_MODE_COUNT; i++) { - for (int j = 0; j < LED_DIRECTION_COUNT; j++) { - serialize8(i); - serialize8(j); - serialize8(masterConfig.modeColors[i].color[j]); - } - } - - for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { - serialize8(LED_MODE_COUNT); - serialize8(j); - serialize8(masterConfig.specialColors.color[j]); + serialize16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET); + serialize16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET); + serialize8(GET_LED_X(ledConfig)); + serialize8(GET_LED_Y(ledConfig)); + serialize8(ledConfig->color); } break; #endif @@ -1795,7 +1782,7 @@ static bool processInCommand(void) #ifdef LED_STRIP case MSP_SET_LED_COLORS: - for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { + for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; color->h = read16(); color->s = read8(); @@ -1806,26 +1793,31 @@ static bool processInCommand(void) case MSP_SET_LED_STRIP_CONFIG: { i = read8(); - if (i >= LED_MAX_STRIP_LENGTH || currentPort->dataSize != (1 + 4)) { + if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != (1 + 7)) { headSerialError(0); break; } ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - *ledConfig = read32(); + uint16_t mask; + // currently we're storing directions and functions in a uint16 (flags) + // the msp uses 2 x uint16_t to cater for future expansion + mask = read16(); + ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; + + mask = read16(); + ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; + + mask = read8(); + ledConfig->xy = CALCULATE_LED_X(mask); + + mask = read8(); + ledConfig->xy |= CALCULATE_LED_Y(mask); + + ledConfig->color = read8(); + reevaluateLedConfig(); } break; - - case MSP_SET_LED_STRIP_MODECOLOR: - { - ledModeIndex_e modeIdx = read8(); - int funIdx = read8(); - int color = read8(); - - if (!setModeColor(modeIdx, funIdx, color)) - return false; - } - break; #endif case MSP_REBOOT: isRebootScheduled = true; diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 67d06032a5..60d5a80d05 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 20 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 17 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 @@ -247,7 +247,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_3D 124 //out message Settings needed for reversible ESCs #define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll #define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag -#define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed @@ -268,7 +267,6 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll #define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults #define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag -#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings // #define MSP_BIND 240 //in message no param // #define MSP_ALARMS 242 diff --git a/src/main/main.c b/src/main/main.c index 8921d9d810..0daf402768 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -134,7 +134,7 @@ void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); void imuInit(void); void displayInit(rxConfig_t *intialRxConfig); -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); void spektrumBind(rxConfig_t *rxConfig); const sonarHardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor); void osdInit(void); @@ -544,7 +544,7 @@ void init(void) #endif #ifdef LED_STRIP - ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); + ledStripInit(masterConfig.ledConfigs, masterConfig.colors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 11211fa540..53e7096480 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -224,7 +224,7 @@ fix12_t calculateVbatPidCompensation(void) { uint8_t calculateBatteryPercentage(void) { - return constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100); + return (((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount); } uint8_t calculateBatteryCapacityRemainingPercentage(void) From 25f8c2fc5994154f376b562ad7477503004a0a21 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 16 Jul 2016 11:25:37 +0200 Subject: [PATCH 041/456] Revert "Move deadband3d_throttle from MSP_3D to MSP_RC_DEADBAND" --- src/main/io/serial_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 666b6c7728..7d3615bc59 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1262,6 +1262,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(masterConfig.flight3DConfig.deadband3d_low); serialize16(masterConfig.flight3DConfig.deadband3d_high); serialize16(masterConfig.flight3DConfig.neutral3d); + serialize16(masterConfig.flight3DConfig.deadband3d_throttle); break; case MSP_RC_DEADBAND: @@ -1269,7 +1270,6 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.rcControlsConfig.deadband); serialize8(masterConfig.rcControlsConfig.yaw_deadband); serialize8(masterConfig.rcControlsConfig.alt_hold_deadband); - serialize16(masterConfig.flight3DConfig.deadband3d_throttle); break; case MSP_SENSOR_ALIGNMENT: headSerialReply(3); @@ -1525,13 +1525,13 @@ static bool processInCommand(void) masterConfig.flight3DConfig.deadband3d_low = read16(); masterConfig.flight3DConfig.deadband3d_high = read16(); masterConfig.flight3DConfig.neutral3d = read16(); + masterConfig.flight3DConfig.deadband3d_throttle = read16(); break; case MSP_SET_RC_DEADBAND: masterConfig.rcControlsConfig.deadband = read8(); masterConfig.rcControlsConfig.yaw_deadband = read8(); masterConfig.rcControlsConfig.alt_hold_deadband = read8(); - masterConfig.flight3DConfig.deadband3d_throttle = read16(); break; case MSP_SET_RESET_CURR_PID: From b2b0298ebc68056629b96b715c262f99cb8bc8f2 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Sat, 16 Jul 2016 12:25:34 +0100 Subject: [PATCH 042/456] Turn off MAG support for CC3D to save some flash space. --- src/main/target/CC3D/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 0e7b290780..cee25b2723 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -117,6 +117,7 @@ #undef GPS #undef BARO +#undef MAG #ifdef CC3D_OPBL #define SKIP_CLI_COMMAND_HELP From fdd416318c072a501e5e1027b28221859111152d Mon Sep 17 00:00:00 2001 From: gaelj Date: Sat, 16 Jul 2016 18:14:57 +0200 Subject: [PATCH 043/456] MSP header size fix --- src/main/io/serial_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index c21680f778..04ab9f53af 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1245,7 +1245,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_3D: - headSerialReply(2 * 4); + headSerialReply(2 * 3); serialize16(masterConfig.flight3DConfig.deadband3d_low); serialize16(masterConfig.flight3DConfig.deadband3d_high); serialize16(masterConfig.flight3DConfig.neutral3d); @@ -1253,7 +1253,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_RC_DEADBAND: - headSerialReply(3); + headSerialReply(5); serialize8(masterConfig.rcControlsConfig.deadband); serialize8(masterConfig.rcControlsConfig.yaw_deadband); serialize8(masterConfig.rcControlsConfig.alt_hold_deadband); From ee8e1c16572f00234f953feb8edcea40d4f8e16f Mon Sep 17 00:00:00 2001 From: gaelj Date: Sat, 16 Jul 2016 18:41:43 +0200 Subject: [PATCH 044/456] re-implement move of parameter in MSP --- src/main/io/serial_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 04ab9f53af..24783e4131 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1249,7 +1249,6 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(masterConfig.flight3DConfig.deadband3d_low); serialize16(masterConfig.flight3DConfig.deadband3d_high); serialize16(masterConfig.flight3DConfig.neutral3d); - serialize16(masterConfig.flight3DConfig.deadband3d_throttle); break; case MSP_RC_DEADBAND: @@ -1257,6 +1256,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.rcControlsConfig.deadband); serialize8(masterConfig.rcControlsConfig.yaw_deadband); serialize8(masterConfig.rcControlsConfig.alt_hold_deadband); + serialize16(masterConfig.flight3DConfig.deadband3d_throttle); break; case MSP_SENSOR_ALIGNMENT: headSerialReply(3); From 312655183263eae0ed32a73b62ed038f0a43c6d9 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 17 Jul 2016 07:32:53 +1000 Subject: [PATCH 045/456] Fixed use_unsyncedpwm inversion issue --- src/main/flight/mixer.c | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 8db1443d0c..5b0ae97aa0 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -65,7 +65,7 @@ static flight3DConfig_t *flight3DConfig; static escAndServoConfig_t *escAndServoConfig; static airplaneConfig_t *airplaneConfig; static rxConfig_t *rxConfig; -static bool syncPwm = false; +static bool syncPwmWithPidLoop = false; static mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; @@ -427,7 +427,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura motorCount = 0; servoCount = pwmOutputConfiguration->servoCount; - syncPwm = use_unsyncedPwm; + syncPwmWithPidLoop = !use_unsyncedPwm; if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { // load custom mixer into currentMixer @@ -528,9 +528,12 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) customMixers = initialCustomMixers; } -void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration) +void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration, bool use_unsyncedPwm) { UNUSED(pwmOutputConfiguration); + + syncPwmWithPidLoop = !use_unsyncedPwm; + motorCount = 4; #ifdef USE_SERVOS servoCount = 0; @@ -644,7 +647,7 @@ void writeMotors(void) for (i = 0; i < motorCount; i++) pwmWriteMotor(i, motor[i]); - if (syncPwm) { + if (syncPwmWithPidLoop) { pwmCompleteOneshotMotorUpdate(motorCount); } } From c16bf1400f38a0a3395291dd70f447bb87211b14 Mon Sep 17 00:00:00 2001 From: gaelj Date: Sun, 17 Jul 2016 01:04:03 +0200 Subject: [PATCH 046/456] Fix wrong header size in MSP_STATUS_EX --- src/main/io/serial_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index c21680f778..01b9a9930b 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -737,7 +737,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_STATUS_EX: - headSerialReply(12); + headSerialReply(13); serialize16(cycleTime); #ifdef USE_I2C serialize16(i2cGetErrorCounter()); From 50da1cfa1defba9bc08520a3a03ec05c2efdec2f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 01:19:16 +0200 Subject: [PATCH 047/456] KISSFC Move PPM to AUX2 // Board identifier set to 4 chars --- src/main/target/KISSFC/target.c | 4 ++-- src/main/target/KISSFC/target.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index e270422ad6..5045599da5 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -22,7 +22,7 @@ #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { - PWM9 | (MAP_TO_PPM_INPUT << 8), + PWM12 | (MAP_TO_PPM_INPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), @@ -32,7 +32,6 @@ const uint16_t multiPPM[] = { PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; @@ -44,6 +43,7 @@ const uint16_t multiPWM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), PWM8 | (MAP_TO_PWM_INPUT << 8), PWM9 | (MAP_TO_PWM_INPUT << 8), PWM10 | (MAP_TO_PWM_INPUT << 8), diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 3c5f4cd5d7..7224db6b6a 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -17,7 +17,7 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "KISSFC" +#define TARGET_BOARD_IDENTIFIER "KISS" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE From f80fe9ba5e8d8b9440ca325848d5bac067b20ecd Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 01:22:20 +0200 Subject: [PATCH 048/456] Remove Auto Looptime Config --- src/main/io/serial_msp.c | 73 +--------------------------------------- 1 file changed, 1 insertion(+), 72 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index c21680f778..a2cf85cb66 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -104,74 +104,6 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c extern uint16_t rssi; // FIXME dependency on mw.c extern void resetPidProfile(pidProfile_t *pidProfile); -void setGyroSamplingSpeed(uint16_t looptime) { - uint16_t gyroSampleRate = 1000; - uint8_t maxDivider = 1; - - if (looptime != gyro.targetLooptime || looptime == 0) { - if (looptime == 0) looptime = gyro.targetLooptime; // needed for pid controller changes -#ifdef STM32F303xC - if (looptime < 1000) { - masterConfig.gyro_lpf = 0; - gyroSampleRate = 125; - maxDivider = 8; - masterConfig.pid_process_denom = 1; - masterConfig.acc_hardware = ACC_DEFAULT; - masterConfig.baro_hardware = BARO_DEFAULT; - masterConfig.mag_hardware = MAG_DEFAULT; - if (looptime < 250) { - masterConfig.acc_hardware = ACC_NONE; - masterConfig.baro_hardware = BARO_NONE; - masterConfig.mag_hardware = MAG_NONE; - masterConfig.pid_process_denom = 2; - } else if (looptime < 375) { - masterConfig.acc_hardware = CONFIG_FASTLOOP_PREFERRED_ACC; - masterConfig.baro_hardware = BARO_NONE; - masterConfig.mag_hardware = MAG_NONE; - masterConfig.pid_process_denom = 2; - } - masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider); - } else { - masterConfig.gyro_lpf = 0; - masterConfig.gyro_sync_denom = 8; - masterConfig.acc_hardware = ACC_DEFAULT; - masterConfig.baro_hardware = BARO_DEFAULT; - masterConfig.mag_hardware = MAG_DEFAULT; - } -#else - if (looptime < 1000) { - masterConfig.gyro_lpf = 0; - masterConfig.acc_hardware = ACC_NONE; - masterConfig.baro_hardware = BARO_NONE; - masterConfig.mag_hardware = MAG_NONE; - gyroSampleRate = 125; - maxDivider = 8; - masterConfig.pid_process_denom = 1; - if (currentProfile->pidProfile.pidController == PID_CONTROLLER_FLOAT) { - masterConfig.pid_process_denom = 2; - } - if (looptime < 250) { - masterConfig.pid_process_denom = 4; - } else if (looptime < 375) { - if (currentProfile->pidProfile.pidController == PID_CONTROLLER_FLOAT) { - masterConfig.pid_process_denom = 3; - } else { - masterConfig.pid_process_denom = 2; - } - } - masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider); - } else { - masterConfig.gyro_lpf = 0; - masterConfig.gyro_sync_denom = 8; - masterConfig.acc_hardware = ACC_DEFAULT; - masterConfig.baro_hardware = BARO_DEFAULT; - masterConfig.mag_hardware = MAG_DEFAULT; - masterConfig.pid_process_denom = 1; - } -#endif - } -} - void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. @@ -1311,7 +1243,6 @@ static bool processInCommand(void) uint32_t i; uint16_t tmp; uint8_t rate; - uint8_t oldPid; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0, alt = 0; @@ -1358,13 +1289,11 @@ static bool processInCommand(void) masterConfig.disarm_kill_switch = read8(); break; case MSP_SET_LOOP_TIME: - setGyroSamplingSpeed(read16()); + read16(); break; case MSP_SET_PID_CONTROLLER: - oldPid = currentProfile->pidProfile.pidController; currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); pidSetController(currentProfile->pidProfile.pidController); - if (oldPid != currentProfile->pidProfile.pidController) setGyroSamplingSpeed(0); // recalculate looptimes for new PID break; case MSP_SET_PID: for (i = 0; i < PID_ITEM_COUNT; i++) { From 34fd8f3c887efae56322a608229a2cd1452af2db Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 01:41:17 +0200 Subject: [PATCH 049/456] Improve Accuracy of roll_yaw_cam_mix feature --- src/main/mw.c | 46 +++++++++++++++++++++++----------------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index 5865a5118e..0f999d81c1 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -188,6 +188,24 @@ float calculateRate(int axis, int16_t rc) { return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection } +void scaleRcCommandToFpvCamAngle(void) { + //recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed + static uint8_t lastFpvCamAngleDegrees = 0; + static float cosFactor = 1.0; + static float sinFactor = 0.0; + + if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){ + lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees; + cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD); + sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD); + } + + int16_t roll = angleRate[ROLL]; + int16_t yaw = angleRate[YAW]; + angleRate[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500); + angleRate[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500); +} + void processRcCommand(void) { static int16_t lastCommand[4] = { 0, 0, 0, 0 }; @@ -208,6 +226,11 @@ void processRcCommand(void) if (isRXDataNew) { for (axis = 0; axis < 3; axis++) angleRate[axis] = calculateRate(axis, rcCommand[axis]); + // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) + if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) { + scaleRcCommandToFpvCamAngle(); + } + for (int channel=0; channel < 4; channel++) { deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); lastCommand[channel] = rcCommand[channel]; @@ -230,24 +253,6 @@ void processRcCommand(void) } } -void scaleRcCommandToFpvCamAngle(void) { - //recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed - static uint8_t lastFpvCamAngleDegrees = 0; - static float cosFactor = 1.0; - static float sinFactor = 0.0; - - if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){ - lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees; - cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD); - sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD); - } - - int16_t roll = rcCommand[ROLL]; - int16_t yaw = rcCommand[YAW]; - rcCommand[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500); - rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500); -} - static void updateRcCommands(void) { // PITCH & ROLL only dynamic PID adjustment, depending on throttle value @@ -310,11 +315,6 @@ static void updateRcCommands(void) rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } - - // experimental scaling of RC command to FPV cam angle - if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) { - scaleRcCommandToFpvCamAngle(); - } } static void updateLEDs(void) From 6582a958aa5c93a84f97e91abfb851df319860d3 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 01:46:04 +0200 Subject: [PATCH 050/456] PID float scaling in deg/sec fix --- src/main/flight/pid.c | 14 +++++++------- src/main/mw.c | 6 ++++-- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 25ca7c40aa..f90f1f48b0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -129,9 +129,9 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float // Scaling factors for Pids for better tunable range in configurator - static const float luxPTermScale = 1.0f / 128; - static const float luxITermScale = 1000000.0f / 0x1000000; - static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 508; + static const float PTermScale = 0.032029f; + static const float ITermScale = 0.244381f; + static const float DTermScale = 0.000529f; if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions @@ -170,7 +170,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat } } - gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled for easier int conversion + gyroRate = gyroADCf[axis] / 16.4f; // gyro output deg/sec // --------low-level gyro-based PID. ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes @@ -187,7 +187,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat } // -----calculate P component - PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor; + PTerm = PTermScale * RateError * pidProfile->P8[axis] * tpaFactor; // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { @@ -197,7 +197,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat // -----calculate I component. uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, (int32_t)angleRate[axis]) : pidProfile->I8[axis]; - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f); // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings @@ -232,7 +232,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat // Filter delta if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); + DTerm = constrainf(DTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); diff --git a/src/main/mw.c b/src/main/mw.c index 0f999d81c1..66555404dc 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -184,8 +184,10 @@ float calculateRate(int axis, int16_t rc) { angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f; } - - return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection + if (currentProfile->pidProfile.pidController == PID_CONTROLLER_INTEGER) + return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection + else + return constrainf(angleRate / 4.1f, -1997.0f, 1997.0f); // Rate limit protection (deg/sec) } void scaleRcCommandToFpvCamAngle(void) { From 5e32ced94db8196ad618c701ede887f824caa199 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 02:00:57 +0200 Subject: [PATCH 051/456] Add FPV ANGLE MIX as a mode ident --- src/main/io/rc_controls.h | 1 + src/main/io/serial_msp.c | 3 +++ src/main/mw.c | 2 +- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 75cbe4c8ef..be5e7311da 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -50,6 +50,7 @@ typedef enum { BOXFAILSAFE, BOXAIRMODE, BOX3DDISABLESWITCH, + BOXFPVANGLEMIX, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index dd05efcb4d..9a5b96ff5b 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -146,6 +146,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXFAILSAFE, "FAILSAFE;", 27 }, { BOXAIRMODE, "AIR MODE;", 28 }, { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 29}, + { BOXFPVANGLEMIX, "FPV ANGLE MIX;", 30}, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -474,6 +475,8 @@ void mspInit(serialConfig_t *serialConfig) if (!feature(FEATURE_AIRMODE)) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH; + activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; + if (sensors(SENSOR_BARO)) { activeBoxIds[activeBoxIdCount++] = BOXBARO; } diff --git a/src/main/mw.c b/src/main/mw.c index 66555404dc..05c8d1a08f 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -229,7 +229,7 @@ void processRcCommand(void) for (axis = 0; axis < 3; axis++) angleRate[axis] = calculateRate(axis, rcCommand[axis]); // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) - if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) { + if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) { scaleRcCommandToFpvCamAngle(); } From b854ae7faf4d0a28b0a11d206a97f8c07b8d315b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 02:45:47 +0200 Subject: [PATCH 052/456] Fix fast PWM motor update --- src/main/main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index 0daf402768..7e6e6f182b 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -310,12 +310,12 @@ void init(void) featureClear(FEATURE_ONESHOT125); } - bool use_unsyncedPwm = masterConfig.use_unsyncedPwm; + bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL; // Configurator feature abused for enabling Fast PWM pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED); pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol; - pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; + pwm_params.motorPwmRate = use_unsyncedPwm ? masterConfig.motor_pwm_rate : 0; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; if (feature(FEATURE_3D)) pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; From 9cac1c9a8ac0e3c8ce0e1047d45cef1bd8e94a01 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 02:59:37 +0200 Subject: [PATCH 053/456] Add BRUSHED to unsynced to prevent changing motor_pwm_rate --- src/main/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/main.c b/src/main/main.c index 7e6e6f182b..944faf0fac 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -310,7 +310,7 @@ void init(void) featureClear(FEATURE_ONESHOT125); } - bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL; + bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL || masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED; // Configurator feature abused for enabling Fast PWM pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED); From 19a814afbb7034b0f295a605879d0c8402e7d4fb Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 03:16:04 +0200 Subject: [PATCH 054/456] Seperate float and int accumulation logic --- src/main/config/config.c | 4 ++-- src/main/flight/pid.c | 29 +++++++++-------------------- 2 files changed, 11 insertions(+), 22 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 0caa5c62e7..a9723ecaa9 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -220,8 +220,8 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 80; - pidProfile->rollPitchItermIgnoreRate = 180; - pidProfile->yawItermIgnoreRate = 35; + pidProfile->rollPitchItermIgnoreRate = 200; + pidProfile->yawItermIgnoreRate = 50; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->dynamic_pid = 1; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f90f1f48b0..8de667012c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -76,20 +76,6 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) targetPidLooptime = gyro.targetLooptime * pidProcessDenom; } -uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) -{ - uint16_t dynamicKi; - uint16_t resetRate; - - resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - - uint16_t dynamicFactor = (1 << 8) - constrain((ABS(angleRate) << 6) / resetRate, 0, 1 << 8); - - dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - - return dynamicKi; -} - void pidResetErrorGyroState(void) { int axis; @@ -195,9 +181,11 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat } // -----calculate I component. - uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, (int32_t)angleRate[axis]) : pidProfile->I8[axis]; + // Prevent strong Iterm accumulation during stick inputs + float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + float antiWindupScaler = constrainf(1.0f - (1.5f * (ABS(angleRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * errorLimiter * RateError * getdT() * kI, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * errorLimiter * RateError * getdT() * antiWindupScaler, -250.0f, 250.0f); // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings @@ -330,11 +318,12 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Time correction (to avoid different I scaling for different builds based on average cycle time) // is normalized to cycle time = 2048. - uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile, AngleRateTmp) : pidProfile->I8[axis]; + // Prevent Accumulation + uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + uint16_t dynamicFactor = (1 << 8) - constrain(((ABS((int32_t)angleRate) << 6) / resetRate), 0, 1 << 8); + uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - int32_t rateErrorLimited = errorLimiter * RateError; - - errorGyroI[axis] = errorGyroI[axis] + ((rateErrorLimited * (uint16_t)targetPidLooptime) >> 11) * kI; + errorGyroI[axis] = errorGyroI[axis] + ((uint16_t)targetPidLooptime >> 11) * dynamicKi; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings From e40942366e555acdbb0c6b23b3c47f192728e072 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 16 Jul 2016 13:32:54 +1000 Subject: [PATCH 055/456] Reverting serial_4way to 2.9 code --- src/main/io/serial_4way.c | 1019 ++++++++++++++---------- src/main/io/serial_4way.h | 131 +-- src/main/io/serial_4way_avrootloader.c | 329 ++++---- src/main/io/serial_4way_avrootloader.h | 15 +- src/main/io/serial_4way_impl.h | 25 +- src/main/io/serial_4way_stk500v2.c | 311 ++++---- src/main/io/serial_4way_stk500v2.h | 5 +- 7 files changed, 999 insertions(+), 836 deletions(-) diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 7b783422fa..b065dfd91c 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -25,169 +25,247 @@ #include "platform.h" #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "drivers/serial.h" +#include "drivers/buf_writer.h" #include "drivers/gpio.h" -#include "drivers/io.h" -#include "drivers/io_impl.h" #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "drivers/light_led.h" #include "drivers/system.h" +#include "drivers/buf_writer.h" #include "flight/mixer.h" #include "io/beeper.h" #include "io/serial_msp.h" +#include "io/serial_msp.h" #include "io/serial_4way.h" #include "io/serial_4way_impl.h" + +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #include "io/serial_4way_avrootloader.h" +#endif +#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) #include "io/serial_4way_stk500v2.h" +#endif #define USE_TXRX_LED -#if defined(USE_TXRX_LED) && defined(LED0) -#define RX_LED_OFF LED0_OFF -#define RX_LED_ON LED0_ON +#ifdef USE_TXRX_LED +#define RX_LED_OFF LED0_OFF +#define RX_LED_ON LED0_ON #ifdef LED1 -#define TX_LED_OFF LED1_OFF -#define TX_LED_ON LED1_ON +#define TX_LED_OFF LED1_OFF +#define TX_LED_ON LED1_ON #else -#define TX_LED_OFF LED0_OFF -#define TX_LED_ON LED0_ON +#define TX_LED_OFF LED0_OFF +#define TX_LED_ON LED0_ON #endif #else -# define RX_LED_OFF do {} while(0) -# define RX_LED_ON do {} while(0) -# define TX_LED_OFF do {} while(0) -# define TX_LED_ON do {} while(0) +#define RX_LED_OFF +#define RX_LED_ON +#define TX_LED_OFF +#define TX_LED_ON #endif #define SERIAL_4WAY_INTERFACE_NAME_STR "m4wFCIntf" -#define SERIAL_4WAY_VER_MAIN 14 -#define SERIAL_4WAY_VER_SUB_1 4 -#define SERIAL_4WAY_VER_SUB_2 4 +// *** change to adapt Revision +#define SERIAL_4WAY_VER_MAIN 14 +#define SERIAL_4WAY_VER_SUB_1 (uint8_t) 4 +#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 04 + #define SERIAL_4WAY_PROTOCOL_VER 106 +// *** end -#if SERIAL_4WAY_VER_MAIN > 24 -# error "SERIAL_4WAY_VER_MAIN * 10 + SERIAL_4WAY_VER_SUB_1 must fit in uint8_t" -#endif -#if SERIAL_4WAY_VER_SUB_1 >= 10 -# warning "SERIAL_4WAY_VER_SUB_1 should be 0-9" +#if (SERIAL_4WAY_VER_MAIN > 24) +#error "beware of SERIAL_4WAY_VER_SUB_1 is uint8_t" #endif -#if SERIAL_4WAY_VER_SUB_2 >= 100 -# warning "SERIAL_4WAY_VER_SUB_2 should be <= 99 (9.9)" -#endif +#define SERIAL_4WAY_VERSION (uint16_t) ((SERIAL_4WAY_VER_MAIN * 1000) + (SERIAL_4WAY_VER_SUB_1 * 100) + SERIAL_4WAY_VER_SUB_2) -#define SERIAL_4WAY_VERSION_HI (uint8_t)(SERIAL_4WAY_VER_MAIN * 10 + SERIAL_4WAY_VER_SUB_1) -#define SERIAL_4WAY_VERSION_LO (uint8_t)(SERIAL_4WAY_VER_SUB_2) +#define SERIAL_4WAY_VERSION_HI (uint8_t) (SERIAL_4WAY_VERSION / 100) +#define SERIAL_4WAY_VERSION_LO (uint8_t) (SERIAL_4WAY_VERSION % 100) static uint8_t escCount; -uint8_t escSelected; + escHardware_t escHardware[MAX_PWM_MOTORS]; -bool esc4wayExitRequested = false; +uint8_t selected_esc; -static escDeviceInfo_t deviceInfo; +uint8_32_u DeviceInfo; -static bool isMcuConnected(void) +#define DeviceInfoSize 4 + +inline bool isMcuConnected(void) { - return deviceInfo.signature != 0; + return (DeviceInfo.bytes[0] > 0); } -static void setDisconnected(void) -{ - deviceInfo.signature = 0; -} - -bool isEscHi(uint8_t selEsc) +inline bool isEscHi(uint8_t selEsc) { return (IORead(escHardware[selEsc].io) != Bit_RESET); } - -bool isEscLo(uint8_t selEsc) +inline bool isEscLo(uint8_t selEsc) { return (IORead(escHardware[selEsc].io) == Bit_RESET); } -void setEscHi(uint8_t selEsc) +inline void setEscHi(uint8_t selEsc) { IOHi(escHardware[selEsc].io); } -void setEscLo(uint8_t selEsc) +inline void setEscLo(uint8_t selEsc) { IOLo(escHardware[selEsc].io); } -void setEscInput(uint8_t selEsc) +inline void setEscInput(uint8_t selEsc) { IOConfigGPIO(escHardware[selEsc].io, IOCFG_IPU); } -void setEscOutput(uint8_t selEsc) +inline void setEscOutput(uint8_t selEsc) { IOConfigGPIO(escHardware[selEsc].io, IOCFG_OUT_PP); } -// Initialize 4way ESC interface -// initializes internal structures -// returns number of ESCs available -int esc4wayInit(void) +uint8_t esc4wayInit(void) { // StopPwmAllMotors(); + pwmDisableMotors(); + escCount = 0; memset(&escHardware, 0, sizeof(escHardware)); pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration(); - int escIdx = 0; for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) { - escHardware[escIdx].io = IOGetByTag(pwmOutputConfiguration->portConfigurations[i].timerHardware->tag); - escIdx++; + escHardware[escCount].io = IOGetByTag(pwmOutputConfiguration->portConfigurations[i].timerHardware->tag); + setEscInput(escCount); + setEscHi(escCount); + escCount++; } } } - escCount = escIdx; return escCount; } -// stat BLHeli 4way interface -// sets all ESC lines as output + hi -void esc4wayStart(void) -{ - pwmDisableMotors(); // prevent updating PWM registers - for (int i = 0; i < escCount; i++) { - setEscInput(i); - setEscHi(i); - } -} - -// stops BLHeli 4way interface -// returns all claimed pins back to PWM drivers, reenables PWM void esc4wayRelease(void) { - for(int i = 0; i < escCount; i++) { - IOConfigGPIO(escHardware[i].io, IOCFG_AF_PP); // see pwmOutConfig() - setEscOutput(i); - setEscLo(i); + while (escCount > 0) { + escCount--; + IOConfigGPIO(escHardware[escCount].io, IOCFG_AF_PP); + setEscLo(escCount); } - escCount = 0; pwmEnableMotors(); } -// BLHeliSuite packet framing -// for reference, see 'Manuals/BLHeliSuite 4w-if protocol.pdf' from BLHeliSuite + +#define SET_DISCONNECTED DeviceInfo.words[0] = 0 + +#define INTF_MODE_IDX 3 // index for DeviceInfostate + +// Interface related only +// establish and test connection to the Interface + // Send Structure -// ESC CMD ADDR_H ADDR_L PARAM_LEN PARAM (256B if len == 0) CRC16_Hi CRC16_Lo +// ESC + CMD PARAM_LEN [PARAM (if len > 0)] CRC16_Hi CRC16_Lo // Return -// ESC CMD ADDR_H ADDR_L PARAM_LEN PARAM (256B if len == 0) + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo +// ESC CMD PARAM_LEN [PARAM (if len > 0)] + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo -// esc4wayCmd_e in public header +#define cmd_Remote_Escape 0x2E // '.' +#define cmd_Local_Escape 0x2F // '/' -typedef enum { - // not commands, but keep naming consistent with BLHeli suite - cmd_Remote_Escape = 0x2E, // '.' - cmd_Local_Escape = 0x2F, // '/' -} syn_4way_e; +// Test Interface still present +#define cmd_InterfaceTestAlive 0x30 // '0' alive +// RETURN: ACK +// get Protocol Version Number 01..255 +#define cmd_ProtocolGetVersion 0x31 // '1' version +// RETURN: uint8_t VersionNumber + ACK + +// get Version String +#define cmd_InterfaceGetName 0x32 // '2' name +// RETURN: String + ACK + +//get Version Number 01..255 +#define cmd_InterfaceGetVersion 0x33 // '3' version +// RETURN: uint8_t AVersionNumber + ACK + + +// Exit / Restart Interface - can be used to switch to Box Mode +#define cmd_InterfaceExit 0x34 // '4' exit +// RETURN: ACK + +// Reset the Device connected to the Interface +#define cmd_DeviceReset 0x35 // '5' reset +// RETURN: ACK + +// Get the Device ID connected +// #define cmd_DeviceGetID 0x36 //'6' device id removed since 06/106 +// RETURN: uint8_t DeviceID + ACK + +// Initialize Flash Access for Device connected +#define cmd_DeviceInitFlash 0x37 // '7' init flash access +// RETURN: ACK + +// Erase the whole Device Memory of connected Device +#define cmd_DeviceEraseAll 0x38 // '8' erase all +// RETURN: ACK + +// Erase one Page of Device Memory of connected Device +#define cmd_DevicePageErase 0x39 // '9' page erase +// PARAM: uint8_t APageNumber +// RETURN: ACK + +// Read to Buffer from Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceRead 0x3A // ':' read Device +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255] +// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK + +// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceWrite 0x3B // ';' write +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] +// RETURN: ACK + +// Set C2CK low infinite ) permanent Reset state +#define cmd_DeviceC2CK_LOW 0x3C // '<' +// RETURN: ACK + +// Read to Buffer from Device Memory of connected Device //Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceReadEEprom 0x3D // '=' read Device +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255] +// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK + +// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceWriteEEprom 0x3E // '>' write +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] +// RETURN: ACK + +// Set Interface Mode +#define cmd_InterfaceSetMode 0x3F // '?' +// #define imC2 0 +// #define imSIL_BLB 1 +// #define imATM_BLB 2 +// #define imSK 3 +// PARAM: uint8_t Mode +// RETURN: ACK or ACK_I_INVALID_CHANNEL + +// responses +#define ACK_OK 0x00 +// #define ACK_I_UNKNOWN_ERROR 0x01 +#define ACK_I_INVALID_CMD 0x02 +#define ACK_I_INVALID_CRC 0x03 +#define ACK_I_VERIFY_ERROR 0x04 +// #define ACK_D_INVALID_COMMAND 0x05 +// #define ACK_D_COMMAND_FAILED 0x06 +// #define ACK_D_UNKNOWN_ERROR 0x07 + +#define ACK_I_INVALID_CHANNEL 0x08 +#define ACK_I_INVALID_PARAM 0x09 +#define ACK_D_GENERAL_ERROR 0x0F /* Copyright (c) 2002, 2003, 2004 Marek Michalkiewicz Copyright (c) 2005, 2007 Joerg Wunsch @@ -221,12 +299,11 @@ typedef enum { CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) -{ +uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) { int i; crc = crc ^ ((uint16_t)data << 8); - for (i = 0; i < 8; i++){ + for (i=0; i < 8; i++){ if (crc & 0x8000) crc = (crc << 1) ^ 0x1021; else @@ -236,388 +313,514 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) } // * End copyright -static uint16_t signaturesAtmel[] = {0x9307, 0x930A, 0x930F, 0x940B, 0}; -static uint16_t signaturesSilabs[] = {0xF310, 0xF330, 0xF410, 0xF390, 0xF850, 0xE8B1, 0xE8B2, 0}; -static bool signatureMatch(uint16_t signature, uint16_t *list) +#define ATMEL_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x9307) || (pDeviceInfo->words[0] == 0x930A) || \ + (pDeviceInfo->words[0] == 0x930F) || (pDeviceInfo->words[0] == 0x940B)) + +#define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] == 0xF310)||(pDeviceInfo->words[0] ==0xF330) || \ + (pDeviceInfo->words[0] == 0xF410) || (pDeviceInfo->words[0] == 0xF390) || \ + (pDeviceInfo->words[0] == 0xF850) || (pDeviceInfo->words[0] == 0xE8B1) || \ + (pDeviceInfo->words[0] == 0xE8B2)) + +static uint8_t CurrentInterfaceMode; + +static uint8_t Connect(uint8_32_u *pDeviceInfo) { - for(; *list; list++) - if(signature == *list) - return true; - return false; -} - -static uint8_t currentInterfaceMode; - -// Try connecting to device -// 3 attempts are made, trying both STK and BL protocols. -static uint8_t connect(escDeviceInfo_t *pDeviceInfo) -{ - for (int try = 0; try < 3; try++) { -#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - if (Stk_ConnectEx(pDeviceInfo) && signatureMatch(pDeviceInfo->signature, signaturesAtmel)) { - currentInterfaceMode = imSK; + for (uint8_t I = 0; I < 3; ++I) { + #if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) + if (Stk_ConnectEx(pDeviceInfo) && ATMEL_DEVICE_MATCH) { + CurrentInterfaceMode = imSK; return 1; + } else { + if (BL_ConnectEx(pDeviceInfo)) { + if SILABS_DEVICE_MATCH { + CurrentInterfaceMode = imSIL_BLB; + return 1; + } else if ATMEL_DEVICE_MATCH { + CurrentInterfaceMode = imATM_BLB; + return 1; + } + } } -#endif -#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) if (BL_ConnectEx(pDeviceInfo)) { - if(signatureMatch(pDeviceInfo->signature, signaturesSilabs)) { - currentInterfaceMode = imSIL_BLB; + if SILABS_DEVICE_MATCH { + CurrentInterfaceMode = imSIL_BLB; return 1; - } - if(signatureMatch(pDeviceInfo->signature, signaturesAtmel)) { - currentInterfaceMode = imATM_BLB; + } else if ATMEL_DEVICE_MATCH { + CurrentInterfaceMode = imATM_BLB; return 1; } } -#endif + #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if (Stk_ConnectEx(pDeviceInfo)) { + CurrentInterfaceMode = imSK; + if ATMEL_DEVICE_MATCH return 1; + } + #endif } return 0; } static serialPort_t *port; -static uint16_t crcIn, crcOut; -static uint8_t readByte(void) +static uint8_t ReadByte(void) { // need timeout? while (!serialRxBytesWaiting(port)); return serialRead(port); } -static uint8_t readByteCrc(void) +static uint8_16_u CRC_in; +static uint8_t ReadByteCrc(void) { - uint8_t b = readByte(); - crcIn = _crc_xmodem_update(crcIn, b); + uint8_t b = ReadByte(); + CRC_in.word = _crc_xmodem_update(CRC_in.word, b); return b; } -static void writeByte(uint8_t b) +static void WriteByte(uint8_t b) { serialWrite(port, b); } -static void writeByteCrc(uint8_t b) +static uint8_16_u CRCout; +static void WriteByteCrc(uint8_t b) { - writeByte(b); - crcOut = _crc_xmodem_update(crcOut, b); + WriteByte(b); + CRCout.word = _crc_xmodem_update(CRCout.word, b); } -// handle 4way interface on serial port -// esc4wayStart / esc4wayRelease in called internally -// 256 bytes buffer is allocated on stack -void esc4wayProcess(serialPort_t *serial) +void esc4wayProcess(serialPort_t *mspPort) { - uint8_t command; - uint16_t addr; - int inLen; - int outLen; - uint8_t paramBuf[256]; - uint8_t replyAck; - esc4wayStart(); + uint8_t ParamBuf[256]; + uint8_t ESC; + uint8_t I_PARAM_LEN; + uint8_t CMD; + uint8_t ACK_OUT; + uint8_16_u CRC_check; + uint8_16_u Dummy; + uint8_t O_PARAM_LEN; + uint8_t *O_PARAM; + uint8_t *InBuff; + ioMem_t ioMem; - port = serial; + port = mspPort; -#ifdef BEEPER + // Start here with UART Main loop + #ifdef BEEPER // fix for buzzer often starts beeping continuously when the ESCs are read // switch beeper silent here beeperSilence(); -#endif + #endif + bool isExitScheduled = false; - esc4wayExitRequested = false; - while(!esc4wayExitRequested) { + while(1) { // restart looking for new sequence from host - crcIn = 0; - - uint8_t esc = readByteCrc(); - if(esc != cmd_Local_Escape) - continue; // wait for sync character + do { + CRC_in.word = 0; + ESC = ReadByteCrc(); + } while (ESC != cmd_Local_Escape); RX_LED_ON; - command = readByteCrc(); - addr = readByteCrc() << 8; - addr |= readByteCrc(); + Dummy.word = 0; + O_PARAM = &Dummy.bytes[0]; + O_PARAM_LEN = 1; + CMD = ReadByteCrc(); + ioMem.D_FLASH_ADDR_H = ReadByteCrc(); + ioMem.D_FLASH_ADDR_L = ReadByteCrc(); + I_PARAM_LEN = ReadByteCrc(); - inLen = readByteCrc(); - if(inLen == 0) - inLen = 0x100; // len ==0 -> param is 256B + InBuff = ParamBuf; + uint8_t i = I_PARAM_LEN; + do { + *InBuff = ReadByteCrc(); + InBuff++; + i--; + } while (i != 0); - for(int i = 0; i < inLen; i++) - paramBuf[i] = readByteCrc(); + CRC_check.bytes[1] = ReadByte(); + CRC_check.bytes[0] = ReadByte(); - readByteCrc(); readByteCrc(); // update input CRC - - outLen = 0; // output handling code will send single zero byte if necessary - replyAck = esc4wayAck_OK; - - if(crcIn != 0) // CRC of correct message == 0 - replyAck = esc4wayAck_I_INVALID_CRC; - - TX_LED_ON; - if (replyAck == esc4wayAck_OK) - replyAck = esc4wayProcessCmd(command, addr, paramBuf, inLen, &outLen); RX_LED_OFF; - // send single '\0' byte is output when length is zero (len ==0 -> 256 bytes) - if(!outLen) { - paramBuf[0] = 0; - outLen = 1; + if(CRC_check.word == CRC_in.word) { + ACK_OUT = ACK_OK; + } else { + ACK_OUT = ACK_I_INVALID_CRC; } - crcOut = 0; + if (ACK_OUT == ACK_OK) + { + // wtf.D_FLASH_ADDR_H=Adress_H; + // wtf.D_FLASH_ADDR_L=Adress_L; + ioMem.D_PTR_I = ParamBuf; + + switch(CMD) { + // ******* Interface related stuff ******* + case cmd_InterfaceTestAlive: + { + if (isMcuConnected()){ + switch(CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + case imSIL_BLB: + { + if (!BL_SendCMDKeepAlive()) { // SetStateDisconnected() included + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_SignOn()) { // SetStateDisconnected(); + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_D_GENERAL_ERROR; + } + if ( ACK_OUT != ACK_OK) SET_DISCONNECTED; + } + break; + } + case cmd_ProtocolGetVersion: + { + // Only interface itself, no matter what Device + Dummy.bytes[0] = SERIAL_4WAY_PROTOCOL_VER; + break; + } + + case cmd_InterfaceGetName: + { + // Only interface itself, no matter what Device + // O_PARAM_LEN=16; + O_PARAM_LEN = strlen(SERIAL_4WAY_INTERFACE_NAME_STR); + O_PARAM = (uint8_t *)SERIAL_4WAY_INTERFACE_NAME_STR; + break; + } + + case cmd_InterfaceGetVersion: + { + // Only interface itself, no matter what Device + // Dummy = iUart_res_InterfVersion; + O_PARAM_LEN = 2; + Dummy.bytes[0] = SERIAL_4WAY_VERSION_HI; + Dummy.bytes[1] = SERIAL_4WAY_VERSION_LO; + break; + } + case cmd_InterfaceExit: + { + isExitScheduled = true; + break; + } + case cmd_InterfaceSetMode: + { + #if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if ((ParamBuf[0] <= imSK) && (ParamBuf[0] >= imSIL_BLB)) { + #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + if ((ParamBuf[0] <= imATM_BLB) && (ParamBuf[0] >= imSIL_BLB)) { + #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if (ParamBuf[0] == imSK) { + #endif + CurrentInterfaceMode = ParamBuf[0]; + } else { + ACK_OUT = ACK_I_INVALID_PARAM; + } + break; + } + + case cmd_DeviceReset: + { + if (ParamBuf[0] < escCount) { + // Channel may change here + selected_esc = ParamBuf[0]; + } + else { + ACK_OUT = ACK_I_INVALID_CHANNEL; + break; + } + switch (CurrentInterfaceMode) + { + case imSIL_BLB: + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + { + BL_SendCMDRunRestartBootloader(&DeviceInfo); + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + break; + } + #endif + } + SET_DISCONNECTED; + break; + } + case cmd_DeviceInitFlash: + { + SET_DISCONNECTED; + if (ParamBuf[0] < escCount) { + //Channel may change here + //ESC_LO or ESC_HI; Halt state for prev channel + selected_esc = ParamBuf[0]; + } else { + ACK_OUT = ACK_I_INVALID_CHANNEL; + break; + } + O_PARAM_LEN = DeviceInfoSize; //4 + O_PARAM = (uint8_t *)&DeviceInfo; + if(Connect(&DeviceInfo)) { + DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode; + } else { + SET_DISCONNECTED; + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case cmd_DeviceEraseAll: + { + switch(CurrentInterfaceMode) + { + case imSK: + { + if (!Stk_Chip_Erase()) ACK_OUT=ACK_D_GENERAL_ERROR; + break; + } + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + break; + } + #endif + + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case cmd_DevicePageErase: + { + switch (CurrentInterfaceMode) + { + case imSIL_BLB: + { + Dummy.bytes[0] = ParamBuf[0]; + //Address = Page * 512 + ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 1); + ioMem.D_FLASH_ADDR_L = 0; + if (!BL_PageErase(&ioMem)) ACK_OUT = ACK_D_GENERAL_ERROR; + break; + } + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + break; + } + #endif + + //*** Device Memory Read Ops *** + case cmd_DeviceRead: + { + ioMem.D_NUM_BYTES = ParamBuf[0]; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch(CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + case imATM_BLB: + { + if(!BL_ReadFlash(CurrentInterfaceMode, &ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if(!Stk_ReadFlash(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + if (ACK_OUT == ACK_OK) + { + O_PARAM_LEN = ioMem.D_NUM_BYTES; + O_PARAM = (uint8_t *)&ParamBuf; + } + break; + } + + case cmd_DeviceReadEEprom: + { + ioMem.D_NUM_BYTES = ParamBuf[0]; + /* + wtf.D_FLASH_ADDR_H = Adress_H; + wtf.D_FLASH_ADDR_L = Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + { + if (!BL_ReadEEprom(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_ReadEEprom(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + if(ACK_OUT == ACK_OK) + { + O_PARAM_LEN = ioMem.D_NUM_BYTES; + O_PARAM = (uint8_t *)&ParamBuf; + } + break; + } + + //*** Device Memory Write Ops *** + case cmd_DeviceWrite: + { + ioMem.D_NUM_BYTES = I_PARAM_LEN; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + case imATM_BLB: + { + if (!BL_WriteFlash(&ioMem)) { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_WriteFlash(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + } + break; + } + + case cmd_DeviceWriteEEprom: + { + ioMem.D_NUM_BYTES = I_PARAM_LEN; + ACK_OUT = ACK_D_GENERAL_ERROR; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + { + ACK_OUT = ACK_I_INVALID_CMD; + break; + } + case imATM_BLB: + { + if (BL_WriteEEprom(&ioMem)) + { + ACK_OUT = ACK_OK; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (Stk_WriteEEprom(&ioMem)) + { + ACK_OUT = ACK_OK; + } + break; + } + #endif + } + break; + } + default: + { + ACK_OUT = ACK_I_INVALID_CMD; + } + } + } + + CRCout.word = 0; + + TX_LED_ON; serialBeginWrite(port); - writeByteCrc(cmd_Remote_Escape); - writeByteCrc(command); - writeByteCrc(addr >> 8); - writeByteCrc(addr & 0xff); - writeByteCrc(outLen & 0xff); // only low byte is send, 0x00 -> 256B - for(int i = 0; i < outLen % 0x100; i++) - writeByteCrc(paramBuf[i]); - writeByteCrc(replyAck); - writeByte(crcOut >> 8); - writeByte(crcOut & 0xff); + WriteByteCrc(cmd_Remote_Escape); + WriteByteCrc(CMD); + WriteByteCrc(ioMem.D_FLASH_ADDR_H); + WriteByteCrc(ioMem.D_FLASH_ADDR_L); + WriteByteCrc(O_PARAM_LEN); + + i=O_PARAM_LEN; + do { + WriteByteCrc(*O_PARAM); + O_PARAM++; + i--; + } while (i > 0); + + WriteByteCrc(ACK_OUT); + WriteByte(CRCout.bytes[1]); + WriteByte(CRCout.bytes[0]); serialEndWrite(port); - -#ifdef STM32F4 - delay(50); -#endif TX_LED_OFF; - } - - esc4wayRelease(); + if (isExitScheduled) { + esc4wayRelease(); + return; + } + }; } -// handle 4Way interface command -// command - received command, will be sent back in reply -// addr - from received header -// data - buffer used both for received parameters and returned data. -// Should be 256B long ; TODO - implement limited buffer size -// inLen - received input length -// outLen - size of data to return, max 256, initialized to zero -// single '\0' byte will be send if outLen is zero (protocol limitation) -esc4wayAck_e esc4wayProcessCmd(esc4wayCmd_e command, uint16_t addr, uint8_t *data, int inLen, int *outLen) -{ - ioMem_t ioMem; - ioMem.addr = addr; // default flash operation address - ioMem.data = data; // command data buffer is used for read and write commands - switch(command) { -// ******* Interface related stuff ******* - case cmd_InterfaceTestAlive: - if (!isMcuConnected()) - return esc4wayAck_OK; - - switch(currentInterfaceMode) { -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case imATM_BLB: - case imSIL_BLB: - if (BL_SendCMDKeepAlive()) - return esc4wayAck_OK; - break; -#endif -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case imSK: - if (Stk_SignOn()) - return esc4wayAck_OK; - break; -#endif - } - setDisconnected(); - return esc4wayAck_D_GENERAL_ERROR; - - case cmd_ProtocolGetVersion: - // Only interface itself, no matter what Device - data[0] = SERIAL_4WAY_PROTOCOL_VER; - *outLen = 1; - return esc4wayAck_OK; - - case cmd_InterfaceGetName: - // Only interface itself, no matter what Device - // outLen=16; - memcpy(data, SERIAL_4WAY_INTERFACE_NAME_STR, strlen(SERIAL_4WAY_INTERFACE_NAME_STR)); - *outLen = strlen(SERIAL_4WAY_INTERFACE_NAME_STR); - return esc4wayAck_OK; - - case cmd_InterfaceGetVersion: - // Only interface itself, no matter what Device - data[0] = SERIAL_4WAY_VERSION_HI; - data[1] = SERIAL_4WAY_VERSION_LO; - *outLen = 2; - return esc4wayAck_OK; - - case cmd_InterfaceExit: - esc4wayExitRequested = true; - return esc4wayAck_OK; - - case cmd_InterfaceSetMode: - switch(data[0]) { -#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) - case imSIL_BLB: - case imATM_BLB: -#endif -#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - case imSK: -#endif - currentInterfaceMode = data[0]; - break; - default: - return esc4wayAck_I_INVALID_PARAM; - } - return esc4wayAck_OK; - - case cmd_DeviceReset: - if(data[0] >= escCount) - return esc4wayAck_I_INVALID_CHANNEL; - // Channel may change here - escSelected = data[0]; - switch (currentInterfaceMode) { -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case imSIL_BLB: - case imATM_BLB: - BL_SendCMDRunRestartBootloader(); - break; -#endif -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case imSK: - break; -#endif - } - setDisconnected(); - return esc4wayAck_OK; - - case cmd_DeviceInitFlash: { - setDisconnected(); - if (data[0] >= escCount) - return esc4wayAck_I_INVALID_CHANNEL; - //Channel may change here - //ESC_LO or ESC_HI; Halt state for prev channel - int replyAck = esc4wayAck_OK; - escSelected = data[0]; - if(!connect(&deviceInfo)) { - setDisconnected(); - replyAck = esc4wayAck_D_GENERAL_ERROR; - } - deviceInfo.interfaceMode = currentInterfaceMode; - memcpy(data, &deviceInfo, sizeof(deviceInfo)); - *outLen = sizeof(deviceInfo); - return replyAck; - } - -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case cmd_DeviceEraseAll: - switch(currentInterfaceMode) { - case imSK: - if (!Stk_Chip_Erase()) - return esc4wayAck_D_GENERAL_ERROR; - break; - default: - return esc4wayAck_I_INVALID_CMD; - } - return esc4wayAck_OK; -#endif - -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case cmd_DevicePageErase: - switch (currentInterfaceMode) { - case imSIL_BLB: - *outLen = 1; // return block number (from incoming packet) - // Address = Page * 512 - ioMem.addr = data[0] << 9; - if (!BL_PageErase(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - default: - return esc4wayAck_I_INVALID_CMD; - } - return esc4wayAck_OK; -#endif - -//*** Device Memory Read Ops *** - -// macros to mix interface with (single bit) memory type for switch statement -#define M_FLASH 0 -#define M_EEPROM 1 -#define INTFMEM(interface, memory) (((interface) << 1) | (memory)) - - case cmd_DeviceReadEEprom: - case cmd_DeviceRead: { - int len = data[0]; - if(len == 0) - len = 0x100; - ioMem.len = len; - switch(INTFMEM(currentInterfaceMode, (command == cmd_DeviceRead) ? M_FLASH : M_EEPROM)) { -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case INTFMEM(imSIL_BLB, M_FLASH): - if(!BL_ReadFlashSIL(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imATM_BLB, M_FLASH): - if(!BL_ReadFlashATM(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imATM_BLB, M_EEPROM): - if(!BL_ReadEEprom(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - // INTFMEM(imSIL_BLB, M_EEPROM): no eeprom on Silabs -#endif -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case INTFMEM(imSK, M_FLASH): - if(!Stk_ReadFlash(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imSK, M_EEPROM): - if (!Stk_ReadEEprom(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; -#endif - default: - return esc4wayAck_I_INVALID_CMD; - } - *outLen = ioMem.len; - return esc4wayAck_OK; - } - -//*** Device Memory Write Ops *** - case cmd_DeviceWrite: - case cmd_DeviceWriteEEprom: - ioMem.len = inLen; - switch (INTFMEM(currentInterfaceMode, (command == cmd_DeviceWrite) ? M_FLASH : M_EEPROM)) { -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - case INTFMEM(imSIL_BLB, M_FLASH): - case INTFMEM(imATM_BLB, M_FLASH): - if (!BL_WriteFlash(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imATM_BLB, M_EEPROM): - if (!BL_WriteEEprom(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; -#endif -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - case INTFMEM(imSK, M_FLASH): - if (!Stk_WriteFlash(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; - case INTFMEM(imSK, M_EEPROM): - if (!Stk_WriteEEprom(&ioMem)) - return esc4wayAck_D_GENERAL_ERROR; - break; -#endif - default: - return esc4wayAck_I_INVALID_CMD; - } - return esc4wayAck_OK; -#undef M_FLASH -#undef M_EEPROM -#undef INTFMEM - default: - return esc4wayAck_I_INVALID_CMD; - } - // should not get here -} #endif diff --git a/src/main/io/serial_4way.h b/src/main/io/serial_4way.h index 6c86b44c84..b1bba5ff69 100644 --- a/src/main/io/serial_4way.h +++ b/src/main/io/serial_4way.h @@ -15,127 +15,36 @@ * along with Cleanflight. If not, see . * Author: 4712 */ - #pragma once -#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +#include "serial_4way_impl.h" #define USE_SERIAL_4WAY_BLHELI_BOOTLOADER #define USE_SERIAL_4WAY_SK_BOOTLOADER -typedef enum { - imC2 = 0, - imSIL_BLB = 1, - imATM_BLB = 2, - imSK = 3, -} esc4wayInterfaceMode_e; +#define imC2 0 +#define imSIL_BLB 1 +#define imATM_BLB 2 +#define imSK 3 -typedef enum { -// Test Interface still present - cmd_InterfaceTestAlive = 0x30, // '0' alive -// RETURN: ACK +extern uint8_t selected_esc; -// get Protocol Version Number 01..255 - cmd_ProtocolGetVersion = 0x31, // '1' version -// RETURN: uint8_t VersionNumber + ACK +extern ioMem_t ioMem; -// get Version String - cmd_InterfaceGetName = 0x32, // '2' name -// RETURN: String + ACK +typedef union __attribute__ ((packed)) { + uint8_t bytes[2]; + uint16_t word; +} uint8_16_u; -//get Version Number 01..255 - cmd_InterfaceGetVersion = 0x33, // '3' version -// RETURN: uint16_t VersionNumber + ACK +typedef union __attribute__ ((packed)) { + uint8_t bytes[4]; + uint16_t words[2]; + uint32_t dword; +} uint8_32_u; -// Exit / Restart Interface - can be used to switch to Box Mode - cmd_InterfaceExit = 0x34, // '4' exit -// RETURN: ACK +//extern uint8_32_u DeviceInfo; -// Reset the Device connected to the Interface - cmd_DeviceReset = 0x35, // '5' reset -// PARAM: uint8_t escId -// RETURN: ACK - -// Get the Device ID connected -// cmd_DeviceGetID = 0x36, // '6' device id; removed since 06/106 -// RETURN: uint8_t DeviceID + ACK - -// Initialize Flash Access for Device connected -// Autodetects interface protocol; retruns device signature and protocol - cmd_DeviceInitFlash = 0x37, // '7' init flash access -// PARAM: uint8_t escId -// RETURN: uint8_t deviceInfo[4] + ACK - -// Erase the whole Device Memory of connected Device - cmd_DeviceEraseAll = 0x38, // '8' erase all -// RETURN: ACK - -// Erase one Page of Device Memory of connected Device - cmd_DevicePageErase = 0x39, // '9' page erase -// PARAM: uint8_t PageNumber (512B pages) -// RETURN: APageNumber ACK - -// Read to Buffer from FLASH Memory of connected Device -// Buffer Len is Max 256 Bytes, 0 means 256 Bytes - cmd_DeviceRead = 0x3A, // ':' read Device -// PARAM: [ADRESS] uint8_t BuffLen -// RETURN: [ADRESS, len] Buffer[0..256] ACK - -// Write Buffer to FLASH Memory of connected Device -// Buffer Len is Max 256 Bytes, 0 means 256 Bytes - cmd_DeviceWrite = 0x3B, // ';' write -// PARAM: [ADRESS + BuffLen] Buffer[1..256] -// RETURN: ACK - -// Set C2CK low infinite - permanent Reset state (unimplemented) - cmd_DeviceC2CK_LOW = 0x3C, // '<' -// RETURN: ACK - -// Read to Buffer from EEPROM Memory of connected Device -// Buffer Len is Max 256 Bytes, 0 means 256 Bytes - cmd_DeviceReadEEprom = 0x3D, // '=' read Device -// PARAM: [ADRESS] uint8_t BuffLen -// RETURN: [ADRESS + BuffLen] + Buffer[1..256] ACK - -// Write Buffer to EEPROM Memory of connected Device -// Buffer Len is Max 256 Bytes, 0 means 256 Bytes - cmd_DeviceWriteEEprom = 0x3E, // '>' write -// PARAM: [ADRESS + BuffLen] Buffer[1..256] -// RETURN: ACK - -// Set Interface Mode - cmd_InterfaceSetMode = 0x3F, // '?' -// PARAM: uint8_t Mode (interfaceMode_e) -// RETURN: ACK -} esc4wayCmd_e; - -// responses -typedef enum { - esc4wayAck_OK = 0x00, -// esc4wayAck_I_UNKNOWN_ERROR = 0x01, - esc4wayAck_I_INVALID_CMD = 0x02, - esc4wayAck_I_INVALID_CRC = 0x03, - esc4wayAck_I_VERIFY_ERROR = 0x04, -// esc4wayAck_D_INVALID_COMMAND = 0x05, -// esc4wayAck_D_COMMAND_FAILED = 0x06, -// esc4wayAck_D_UNKNOWN_ERROR = 0x07, - esc4wayAck_I_INVALID_CHANNEL = 0x08, - esc4wayAck_I_INVALID_PARAM = 0x09, - esc4wayAck_D_GENERAL_ERROR = 0x0f, -} esc4wayAck_e; - -typedef struct escDeviceInfo_s { - uint16_t signature; // lower 16 bit of signature - uint8_t signature2; // top 8 bit of signature for SK / BootMsg last char from BL - uint8_t interfaceMode; -} escDeviceInfo_t; - -extern bool esc4wayExitRequested; // flag that exit was requested. Set by esc4wayProcessCmd, used internally by esc4wayProcess - -int esc4wayInit(void); -void esc4wayStart(void); +bool isMcuConnected(void); +uint8_t esc4wayInit(void); +void esc4wayProcess(serialPort_t *mspPort); void esc4wayRelease(void); -void esc4wayProcess(serialPort_t *serial); -esc4wayAck_e esc4wayProcessCmd(esc4wayCmd_e command, uint16_t addr, uint8_t *data, int inLen, int *outLen); - -#endif diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 01a598e8b2..2a10dcdb4a 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -24,271 +24,289 @@ #include #include "platform.h" -#include "common/utils.h" +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "drivers/system.h" #include "drivers/serial.h" -#include "drivers/buf_writer.h" #include "drivers/pwm_mapping.h" -#include "drivers/gpio.h" #include "io/serial.h" #include "io/serial_msp.h" #include "io/serial_4way.h" #include "io/serial_4way_impl.h" #include "io/serial_4way_avrootloader.h" +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER -#if defined(USE_SERIAL_4WAY_BLHELI_INTERFACE) && defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) // Bootloader commands // RunCmd #define RestartBootloader 0 -#define ExitBootloader 1 +#define ExitBootloader 1 -#define CMD_RUN 0x00 -#define CMD_PROG_FLASH 0x01 -#define CMD_ERASE_FLASH 0x02 +#define CMD_RUN 0x00 +#define CMD_PROG_FLASH 0x01 +#define CMD_ERASE_FLASH 0x02 #define CMD_READ_FLASH_SIL 0x03 -#define CMD_VERIFY_FLASH 0x03 -#define CMD_READ_EEPROM 0x04 -#define CMD_PROG_EEPROM 0x05 -#define CMD_READ_SRAM 0x06 +#define CMD_VERIFY_FLASH 0x03 +#define CMD_READ_EEPROM 0x04 +#define CMD_PROG_EEPROM 0x05 +#define CMD_READ_SRAM 0x06 #define CMD_READ_FLASH_ATM 0x07 -#define CMD_KEEP_ALIVE 0xFD -#define CMD_SET_ADDRESS 0xFF -#define CMD_SET_BUFFER 0xFE +#define CMD_KEEP_ALIVE 0xFD +#define CMD_SET_ADDRESS 0xFF +#define CMD_SET_BUFFER 0xFE -#define CMD_BOOTINIT 0x07 -#define CMD_BOOTSIGN 0x08 +#define CMD_BOOTINIT 0x07 +#define CMD_BOOTSIGN 0x08 // Bootloader result codes -#define BR_SUCCESS 0x30 -#define BR_ERRORCOMMAND 0xC1 -#define BR_ERRORCRC 0xC2 -#define BR_NONE 0xFF +#define brSUCCESS 0x30 +#define brERRORCOMMAND 0xC1 +#define brERRORCRC 0xC2 +#define brNONE 0xFF -#define START_BIT_TIMEOUT 2000 // 2ms -#define BIT_TIME 52 // 52uS -#define BIT_TIME_HALVE (BIT_TIME >> 1) // 26uS -#define BIT_TIME_3_4 (BIT_TIME_HALVE + (BIT_TIME_HALVE >> 1)) // 39uS -#define START_BIT_TIME (BIT_TIME_3_4) +#define START_BIT_TIMEOUT_MS 2 -static int suart_getc(void) +#define BIT_TIME (52) //52uS +#define BIT_TIME_HALVE (BIT_TIME >> 1) //26uS +#define START_BIT_TIME (BIT_TIME_HALVE + 1) +//#define STOP_BIT_TIME ((BIT_TIME * 9) + BIT_TIME_HALVE) + +static uint8_t suart_getc_(uint8_t *bt) { uint32_t btime; uint32_t start_time; - uint32_t wait_time = micros() + START_BIT_TIMEOUT; + uint32_t wait_time = millis() + START_BIT_TIMEOUT_MS; while (ESC_IS_HI) { // check for startbit begin - if (micros() >= wait_time) { - return -1; + if (millis() >= wait_time) { + return 0; } } // start bit start_time = micros(); btime = start_time + START_BIT_TIME; uint16_t bitmask = 0; - for(int bit = 0; bit < 10; bit++) { - while (cmp32(micros(), btime) < 0); + uint8_t bit = 0; + while (micros() < btime); + while(1) { if (ESC_IS_HI) + { bitmask |= (1 << bit); + } btime = btime + BIT_TIME; + bit++; + if (bit == 10) break; + while (micros() < btime); } // check start bit and stop bit - if ((bitmask & (1 << 0)) || (!(bitmask & (1 << 9)))) { - return -1; + if ((bitmask & 1) || (!(bitmask & (1 << 9)))) { + return 0; } - return bitmask >> 1; + *bt = bitmask >> 1; + return 1; } -static void suart_putc(uint8_t byte) +static void suart_putc_(uint8_t *tx_b) { - // send one idle bit first (stopbit from previous byte) - uint16_t bitmask = (byte << 2) | (1 << 0) | (1 << 10); + // shift out stopbit first + uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10); uint32_t btime = micros(); while(1) { - if(bitmask & 1) + if(bitmask & 1) { ESC_SET_HI; // 1 - else + } + else { ESC_SET_LO; // 0 + } btime = btime + BIT_TIME; - bitmask >>= 1; - if (bitmask == 0) - break; // stopbit shifted out - but don't wait - while (cmp32(micros(), btime) < 0); + bitmask = (bitmask >> 1); + if (bitmask == 0) break; // stopbit shifted out - but don't wait + while (micros() < btime); } } -static uint16_t crc16Byte(uint16_t from, uint8_t byte) +static uint8_16_u CRC_16; +static uint8_16_u LastCRC_16; + +static void ByteCrc(uint8_t *bt) { - uint16_t crc16 = from; - for (int i = 0; i < 8; i++) { - if (((byte & 0x01) ^ (crc16 & 0x0001)) != 0) { - crc16 >>= 1; - crc16 ^= 0xA001; + uint8_t xb = *bt; + for (uint8_t i = 0; i < 8; i++) + { + if (((xb & 0x01) ^ (CRC_16.word & 0x0001)) !=0 ) { + CRC_16.word = CRC_16.word >> 1; + CRC_16.word = CRC_16.word ^ 0xA001; } else { - crc16 >>= 1; + CRC_16.word = CRC_16.word >> 1; } - byte >>= 1; + xb = xb >> 1; } - return crc16; } -static uint8_t BL_ReadBuf(uint8_t *pstring, int len, bool checkCrc) +static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len) { - int crc = 0; - int c; + // len 0 means 256 + CRC_16.word = 0; + LastCRC_16.word = 0; + uint8_t LastACK = brNONE; + do { + if(!suart_getc_(pstring)) goto timeout; + ByteCrc(pstring); + pstring++; + len--; + } while(len > 0); - uint8_t lastACK = BR_NONE; - for(int i = 0; i < len; i++) { - int c; - if ((c = suart_getc()) < 0) goto timeout; - crc = crc16Byte(crc, c); - pstring[i] = c; - } - - if(checkCrc) { - // With CRC read 3 more - for(int i = 0; i < 2; i++) { // checksum 2 CRC bytes - if ((c = suart_getc()) < 0) goto timeout; - crc = crc16Byte(crc, c); + if(isMcuConnected()) { + //With CRC read 3 more + if(!suart_getc_(&LastCRC_16.bytes[0])) goto timeout; + if(!suart_getc_(&LastCRC_16.bytes[1])) goto timeout; + if(!suart_getc_(&LastACK)) goto timeout; + if (CRC_16.word != LastCRC_16.word) { + LastACK = brERRORCRC; } - if((c = suart_getc()) < 0) goto timeout; - lastACK = c; - if (crc != 0) // CRC of correct message is 0 - lastACK = BR_ERRORCRC; } else { - if((c = suart_getc()) < 0) goto timeout; - lastACK = c; + if(!suart_getc_(&LastACK)) goto timeout; } timeout: - return (lastACK == BR_SUCCESS); + return (LastACK == brSUCCESS); } -static void BL_SendBuf(uint8_t *pstring, int len, bool appendCrc) +static void BL_SendBuf(uint8_t *pstring, uint8_t len) { ESC_OUTPUT; - uint16_t crc = 0; - for(int i = 0; i < len; i++) { - suart_putc(pstring[i]); - crc = crc16Byte(crc, pstring[i]); - } - if (appendCrc) { - suart_putc(crc & 0xff); - suart_putc(crc >> 8); + CRC_16.word=0; + do { + suart_putc_(pstring); + ByteCrc(pstring); + pstring++; + len--; + } while (len > 0); + + if (isMcuConnected()) { + suart_putc_(&CRC_16.bytes[0]); + suart_putc_(&CRC_16.bytes[1]); } ESC_INPUT; } -uint8_t BL_ConnectEx(escDeviceInfo_t *pDeviceInfo) +uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo) { -#define BOOT_MSG_LEN 4 -#define DevSignHi (BOOT_MSG_LEN) -#define DevSignLo (BOOT_MSG_LEN + 1) + #define BootMsgLen 4 + #define DevSignHi (BootMsgLen) + #define DevSignLo (BootMsgLen+1) - memset(pDeviceInfo, 0, sizeof(*pDeviceInfo)); - uint8_t bootInfo[BOOT_MSG_LEN + 4]; - static const uint8_t bootMsgCheck[BOOT_MSG_LEN - 1] = "471"; + //DeviceInfo.dword=0; is set before + uint8_t BootInfo[9]; + uint8_t BootMsg[BootMsgLen-1] = "471"; // x * 0 + 9 #if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) - // SK message was sent during autodetection, use longer preamble - uint8_t bootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + BL_SendBuf(BootInit, 21); #else - uint8_t bootInit[] = { 0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + BL_SendBuf(BootInit, 17); #endif - BL_SendBuf(bootInit, sizeof(bootInit), false); - if (!BL_ReadBuf(bootInfo, sizeof(bootInfo), false)) + if (!BL_ReadBuf(BootInfo, BootMsgLen + 4)) { return 0; + } // BootInfo has no CRC (ACK byte already analyzed... ) // Format = BootMsg("471c") SIGNATURE_001, SIGNATURE_002, BootVersion (always 6), BootPages (,ACK) - if(memcmp(bootInfo, bootMsgCheck, sizeof(bootMsgCheck)) != 0) // Check only the first 3 letters -> 471x OK - return 0; + for (uint8_t i = 0; i < (BootMsgLen - 1); i++) { // Check only the first 3 letters -> 471x OK + if (BootInfo[i] != BootMsg[i]) { + return (0); + } + } - pDeviceInfo->signature2 = bootInfo[BOOT_MSG_LEN - 1]; // taken from bootloaderMsg part, ascii 'c' now - pDeviceInfo->signature = (bootInfo[DevSignHi] << 8) | bootInfo[DevSignLo]; // SIGNATURE_001, SIGNATURE_002 - return 1; + //only 2 bytes used $1E9307 -> 0x9307 + pDeviceInfo->bytes[2] = BootInfo[BootMsgLen - 1]; + pDeviceInfo->bytes[1] = BootInfo[DevSignHi]; + pDeviceInfo->bytes[0] = BootInfo[DevSignLo]; + return (1); } -static uint8_t BL_GetACK(int timeout) +static uint8_t BL_GetACK(uint32_t Timeout) { - int c; - while ((c = suart_getc()) < 0) - if(--timeout < 0) // timeout=1 -> 1 retry - return BR_NONE; - return c; + uint8_t LastACK = brNONE; + while (!(suart_getc_(&LastACK)) && (Timeout)) { + Timeout--; + } ; + return (LastACK); } -uint8_t BL_SendCMDKeepAlive(void) +uint8_t BL_SendCMDKeepAlive(void) { uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0}; - BL_SendBuf(sCMD, sizeof(sCMD), true); - if (BL_GetACK(1) != BR_ERRORCOMMAND) + BL_SendBuf(sCMD, 2); + if (BL_GetACK(1) != brERRORCOMMAND) { return 0; + } return 1; } -void BL_SendCMDRunRestartBootloader(void) +void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo) { uint8_t sCMD[] = {RestartBootloader, 0}; - BL_SendBuf(sCMD, sizeof(sCMD), true); // sends simply 4 x 0x00 (CRC = 00) + pDeviceInfo->bytes[0] = 1; + BL_SendBuf(sCMD, 2); //sends simply 4 x 0x00 (CRC =00) return; } static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr { // skip if adr == 0xFFFF - if(pMem->addr == 0xffff) - return 1; - uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->addr >> 8, pMem->addr & 0xff }; - BL_SendBuf(sCMD, sizeof(sCMD), true); - return BL_GetACK(2) == BR_SUCCESS; + if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; + uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->D_FLASH_ADDR_H, pMem->D_FLASH_ADDR_L }; + BL_SendBuf(sCMD, 4); + return (BL_GetACK(2) == brSUCCESS); } static uint8_t BL_SendCMDSetBuffer(ioMem_t *pMem) { - uint16_t len = pMem->len; - uint8_t sCMD[] = {CMD_SET_BUFFER, 0, len >> 8, len & 0xff}; - BL_SendBuf(sCMD, sizeof(sCMD), true); - if (BL_GetACK(2) != BR_NONE) - return 0; - BL_SendBuf(pMem->data, len, true); - return BL_GetACK(40) == BR_SUCCESS; + uint8_t sCMD[] = {CMD_SET_BUFFER, 0, 0, pMem->D_NUM_BYTES}; + if (pMem->D_NUM_BYTES == 0) { + // set high byte + sCMD[2] = 1; + } + BL_SendBuf(sCMD, 4); + if (BL_GetACK(2) != brNONE) return 0; + BL_SendBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES); + return (BL_GetACK(40) == brSUCCESS); } static uint8_t BL_ReadA(uint8_t cmd, ioMem_t *pMem) { - if(!BL_SendCMDSetAddress(pMem)) - return 0; - unsigned len = pMem->len; - uint8_t sCMD[] = {cmd, len & 0xff}; // 0x100 is sent a 0x00 here - BL_SendBuf(sCMD, sizeof(sCMD), true); - return BL_ReadBuf(pMem->data, len, true); + if (BL_SendCMDSetAddress(pMem)) { + uint8_t sCMD[] = {cmd, pMem->D_NUM_BYTES}; + BL_SendBuf(sCMD, 2); + return (BL_ReadBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES )); + } + return 0; } static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout) { - if(!BL_SendCMDSetAddress(pMem)) - return 0; - if (!BL_SendCMDSetBuffer(pMem)) - return 0; - uint8_t sCMD[] = {cmd, 0x01}; - BL_SendBuf(sCMD, sizeof(sCMD), true); - return BL_GetACK(timeout) == BR_SUCCESS; + if (BL_SendCMDSetAddress(pMem)) { + if (!BL_SendCMDSetBuffer(pMem)) return 0; + uint8_t sCMD[] = {cmd, 0x01}; + BL_SendBuf(sCMD, 2); + return (BL_GetACK(timeout) == brSUCCESS); + } + return 0; } -uint8_t BL_ReadFlashATM(ioMem_t *pMem) +uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem) { - return BL_ReadA(CMD_READ_FLASH_ATM, pMem); + if(interface_mode == imATM_BLB) { + return BL_ReadA(CMD_READ_FLASH_ATM, pMem); + } else { + return BL_ReadA(CMD_READ_FLASH_SIL, pMem); + } } - -uint8_t BL_ReadFlashSIL(ioMem_t *pMem) -{ - return BL_ReadA(CMD_READ_FLASH_SIL, pMem); -} - - + + uint8_t BL_ReadEEprom(ioMem_t *pMem) { return BL_ReadA(CMD_READ_EEPROM, pMem); @@ -296,22 +314,23 @@ uint8_t BL_ReadEEprom(ioMem_t *pMem) uint8_t BL_PageErase(ioMem_t *pMem) { - if(!BL_SendCMDSetAddress(pMem)) - return 0; - - uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01}; - BL_SendBuf(sCMD, sizeof(sCMD), true); - return BL_GetACK(40 * 1000 / START_BIT_TIMEOUT) == BR_SUCCESS; + if (BL_SendCMDSetAddress(pMem)) { + uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01}; + BL_SendBuf(sCMD, 2); + return (BL_GetACK((40 / START_BIT_TIMEOUT_MS)) == brSUCCESS); + } + return 0; } uint8_t BL_WriteEEprom(ioMem_t *pMem) { - return BL_WriteA(CMD_PROG_EEPROM, pMem, 3000 * 1000 / START_BIT_TIMEOUT); + return BL_WriteA(CMD_PROG_EEPROM, pMem, (3000 / START_BIT_TIMEOUT_MS)); } uint8_t BL_WriteFlash(ioMem_t *pMem) { - return BL_WriteA(CMD_PROG_FLASH, pMem, 40 * 1000 / START_BIT_TIMEOUT); + return BL_WriteA(CMD_PROG_FLASH, pMem, (40 / START_BIT_TIMEOUT_MS)); } #endif +#endif diff --git a/src/main/io/serial_4way_avrootloader.h b/src/main/io/serial_4way_avrootloader.h index 9ed08db61c..39cfaaa3d9 100644 --- a/src/main/io/serial_4way_avrootloader.h +++ b/src/main/io/serial_4way_avrootloader.h @@ -20,17 +20,12 @@ #pragma once -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER - void BL_SendBootInit(void); -uint8_t BL_ConnectEx(escDeviceInfo_t *pDeviceInfo); +uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo); uint8_t BL_SendCMDKeepAlive(void); -uint8_t BL_WriteEEprom(ioMem_t *pMem); -uint8_t BL_ReadEEprom(ioMem_t *pMem); uint8_t BL_PageErase(ioMem_t *pMem); +uint8_t BL_ReadEEprom(ioMem_t *pMem); +uint8_t BL_WriteEEprom(ioMem_t *pMem); uint8_t BL_WriteFlash(ioMem_t *pMem); -uint8_t BL_ReadFlashATM(ioMem_t *pMem); -uint8_t BL_ReadFlashSIL(ioMem_t *pMem); -void BL_SendCMDRunRestartBootloader(void); - -#endif +uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem); +void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo); diff --git a/src/main/io/serial_4way_impl.h b/src/main/io/serial_4way_impl.h index ca1012b284..a8f83bfae8 100644 --- a/src/main/io/serial_4way_impl.h +++ b/src/main/io/serial_4way_impl.h @@ -15,12 +15,15 @@ * along with Cleanflight. If not, see . * Author: 4712 */ +#pragma once + +#include "drivers/io.h" typedef struct { IO_t io; } escHardware_t; -extern uint8_t escSelected; +extern uint8_t selected_esc; bool isEscHi(uint8_t selEsc); bool isEscLo(uint8_t selEsc); @@ -29,15 +32,17 @@ void setEscLo(uint8_t selEsc); void setEscInput(uint8_t selEsc); void setEscOutput(uint8_t selEsc); -#define ESC_IS_HI isEscHi(escSelected) -#define ESC_IS_LO isEscLo(escSelected) -#define ESC_SET_HI setEscHi(escSelected) -#define ESC_SET_LO setEscLo(escSelected) -#define ESC_INPUT setEscInput(escSelected) -#define ESC_OUTPUT setEscOutput(escSelected) +#define ESC_IS_HI isEscHi(selected_esc) +#define ESC_IS_LO isEscLo(selected_esc) +#define ESC_SET_HI setEscHi(selected_esc) +#define ESC_SET_LO setEscLo(selected_esc) +#define ESC_INPUT setEscInput(selected_esc) +#define ESC_OUTPUT setEscOutput(selected_esc) typedef struct ioMem_s { - uint16_t len; - uint16_t addr; - uint8_t *data; + uint8_t D_NUM_BYTES; + uint8_t D_FLASH_ADDR_H; + uint8_t D_FLASH_ADDR_L; + uint8_t *D_PTR_I; } ioMem_t; + diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c index 1c32e2c6bb..342a0c8b06 100644 --- a/src/main/io/serial_4way_stk500v2.c +++ b/src/main/io/serial_4way_stk500v2.c @@ -23,38 +23,33 @@ #include #include "platform.h" -#include "common/utils.h" -#include "drivers/gpio.h" -#include "drivers/buf_writer.h" -#include "drivers/pwm_mapping.h" +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include "drivers/serial.h" -#include "drivers/system.h" #include "config/config.h" #include "io/serial.h" #include "io/serial_msp.h" #include "io/serial_4way.h" #include "io/serial_4way_impl.h" #include "io/serial_4way_stk500v2.h" +#include "drivers/system.h" +#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER -#if defined(USE_SERIAL_4WAY_BLHELI_INTERFACE) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) +#define BIT_LO_US (32) //32uS +#define BIT_HI_US (2*BIT_LO_US) +static uint8_t StkInBuf[16]; -#define BIT_LO_US 32 //32uS -#define BIT_HI_US (2 * BIT_LO_US) - -static uint8_t stkInBuf[16]; - -#define STK_BIT_TIMEOUT 250 // micro seconds +#define STK_BIT_TIMEOUT 250 // micro seconds #define STK_WAIT_TICKS (1000 / STK_BIT_TIMEOUT) // per ms #define STK_WAITCYLCES (STK_WAIT_TICKS * 35) // 35ms -#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5ms -#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) // 5s +#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5 ms +#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) //5 s -#define WaitPinLo while (ESC_IS_HI) { if (cmp32(micros(), timeout_timer) > 0) goto timeout; } -#define WaitPinHi while (ESC_IS_LO) { if (cmp32(micros(), timeout_timer) > 0) goto timeout; } +#define WaitPinLo while (ESC_IS_HI) {if (micros() > timeout_timer) goto timeout;} +#define WaitPinHi while (ESC_IS_LO) {if (micros() > timeout_timer) goto timeout;} -static uint32_t lastBitTime; -static uint32_t hiLoTsh; +static uint32_t LastBitTime; +static uint32_t HiLoTsh; static uint8_t SeqNumber; static uint8_t StkCmd; @@ -77,10 +72,10 @@ static uint8_t ckSumOut; #define STATUS_CMD_OK 0x00 -#define CmdFlashEepromRead 0xA0 -#define EnterIspCmd1 0xAC -#define EnterIspCmd2 0x53 -#define SPI_SIGNATURE_READ 0x30 +#define CmdFlashEepromRead 0xA0 +#define EnterIspCmd1 0xAC +#define EnterIspCmd2 0x53 +#define signature_r 0x30 #define delay_us(x) delayMicroseconds(x) #define IRQ_OFF // dummy @@ -89,7 +84,7 @@ static uint8_t ckSumOut; static void StkSendByte(uint8_t dat) { ckSumOut ^= dat; - for (uint8_t i = 0; i < 8; i++) { + for (uint8_t i = 0; i < 8; i++) { if (dat & 0x01) { // 1-bits are encoded as 64.0us high, 72.8us low (135.8us total). ESC_SET_HI; @@ -111,7 +106,7 @@ static void StkSendByte(uint8_t dat) } } -static void StkSendPacketHeader(uint16_t len) +static void StkSendPacketHeader(void) { IRQ_OFF; ESC_OUTPUT; @@ -121,9 +116,6 @@ static void StkSendPacketHeader(uint16_t len) ckSumOut = 0; StkSendByte(MESSAGE_START); StkSendByte(++SeqNumber); - StkSendByte(len >> 8); - StkSendByte(len & 0xff); - StkSendByte(TOKEN); } static void StkSendPacketFooter(void) @@ -135,14 +127,16 @@ static void StkSendPacketFooter(void) IRQ_ON; } + + static int8_t ReadBit(void) { uint32_t btimer = micros(); uint32_t timeout_timer = btimer + STK_BIT_TIMEOUT; WaitPinLo; WaitPinHi; - lastBitTime = micros() - btimer; - if (lastBitTime <= hiLoTsh) { + LastBitTime = micros() - btimer; + if (LastBitTime <= HiLoTsh) { timeout_timer = timeout_timer + STK_BIT_TIMEOUT; WaitPinLo; WaitPinHi; @@ -155,27 +149,30 @@ timeout: return -1; } -static int ReadByte(void) +static uint8_t ReadByte(uint8_t *bt) { - uint8_t byte = 0; - for (int i = 0; i < 8; i++) { + *bt = 0; + for (uint8_t i = 0; i < 8; i++) { int8_t bit = ReadBit(); - if (bit < 0) - return -1; // timeout - byte |= bit << i; + if (bit == -1) goto timeout; + if (bit == 1) { + *bt |= (1 << i); + } } - ckSumIn ^= byte; - return byte; + ckSumIn ^=*bt; + return 1; +timeout: + return 0; } static uint8_t StkReadLeader(void) { // Reset learned timing - hiLoTsh = BIT_HI_US + BIT_LO_US; + HiLoTsh = BIT_HI_US + BIT_LO_US; // Wait for the first bit - int waitcycl; //250uS each + uint32_t waitcycl; //250uS each if((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) { waitcycl = STK_WAITCYLCES_EXT; @@ -184,54 +181,57 @@ static uint8_t StkReadLeader(void) } else { waitcycl= STK_WAITCYLCES; } - while(ReadBit() < 0 && --waitcycl > 0); - if (waitcycl <= 0) - goto timeout; - - // Skip the first bits - for (int i = 0; i < 10; i++) { - if (ReadBit() < 0) - goto timeout; + for ( ; waitcycl >0 ; waitcycl--) { + //check is not timeout + if (ReadBit() >- 1) break; } - // learn timing (0.75 * lastBitTime) - hiLoTsh = (lastBitTime >> 1) + (lastBitTime >> 2); + //Skip the first bits + if (waitcycl == 0){ + goto timeout; + } + + for (uint8_t i = 0; i < 10; i++) { + if (ReadBit() == -1) goto timeout; + } + + // learn timing + HiLoTsh = (LastBitTime >> 1) + (LastBitTime >> 2); // Read until we get a 0 bit - int bit; + int8_t bit; do { bit = ReadBit(); - if (bit < 0) - goto timeout; + if (bit == -1) goto timeout; } while (bit > 0); return 1; timeout: return 0; } -static uint8_t StkRcvPacket(uint8_t *pstring, int maxLen) +static uint8_t StkRcvPacket(uint8_t *pstring) { - int byte; - int len; + uint8_t bt = 0; + uint8_16_u Len; IRQ_OFF; if (!StkReadLeader()) goto Err; ckSumIn=0; - if ((byte = ReadByte()) < 0 || (byte != MESSAGE_START)) goto Err; - if ((byte = ReadByte()) < 0 || (byte != SeqNumber)) goto Err; - len = ReadByte() << 8; - len |= ReadByte(); - if(len < 1 || len >= 256 + 4) // will catch timeout too; limit length to max expected size - goto Err; - if ((byte = ReadByte()) < 0 || (byte != TOKEN)) goto Err; - if ((byte = ReadByte()) < 0 || (byte != StkCmd)) goto Err; - if ((byte = ReadByte()) < 0 || (byte != STATUS_CMD_OK)) goto Err; - for (int i = 0; i < len - 2; i++) { - if ((byte = ReadByte()) < 0) goto Err; - if(i < maxLen) // limit saved length (buffer is only 256B, but memory read reply contains additional status + 1 unknown byte) - pstring[i] = byte; + if (!ReadByte(&bt) || (bt != MESSAGE_START)) goto Err; + if (!ReadByte(&bt) || (bt != SeqNumber)) goto Err; + ReadByte(&Len.bytes[1]); + if (Len.bytes[1] > 1) goto Err; + ReadByte(&Len.bytes[0]); + if (Len.bytes[0] < 1) goto Err; + if (!ReadByte(&bt) || (bt != TOKEN)) goto Err; + if (!ReadByte(&bt) || (bt != StkCmd)) goto Err; + if (!ReadByte(&bt) || (bt != STATUS_CMD_OK)) goto Err; + for (uint16_t i = 0; i < (Len.word - 2); i++) + { + if (!ReadByte(pstring)) goto Err; + pstring++; } - ReadByte(); // read checksum + ReadByte(&bt); if (ckSumIn != 0) goto Err; IRQ_ON; return 1; @@ -240,26 +240,27 @@ Err: return 0; } -static uint8_t _CMD_SPI_MULTI_EX(uint8_t * resByte, uint8_t subcmd, uint16_t addr) +static uint8_t _CMD_SPI_MULTI_EX(volatile uint8_t * ResByte,uint8_t Cmd,uint8_t AdrHi,uint8_t AdrLo) { - StkCmd = CMD_SPI_MULTI; - StkSendPacketHeader(8); + StkCmd= CMD_SPI_MULTI; + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(8); // lo byte Msg len + StkSendByte(TOKEN); StkSendByte(CMD_SPI_MULTI); - StkSendByte(4); // NumTX - StkSendByte(4); // NumRX - StkSendByte(0); // RxStartAdr - StkSendByte(subcmd); // {TxData} Cmd - StkSendByte(addr >> 8); // {TxData} AdrHi - StkSendByte(addr & 0xff); // {TxData} AdrLow - StkSendByte(0); // {TxData} 0 + StkSendByte(4); // NumTX + StkSendByte(4); // NumRX + StkSendByte(0); // RxStartAdr + StkSendByte(Cmd); // {TxData} Cmd + StkSendByte(AdrHi); // {TxData} AdrHi + StkSendByte(AdrLo); // {TxData} AdrLoch + StkSendByte(0); // {TxData} 0 StkSendPacketFooter(); - if (StkRcvPacket(stkInBuf, sizeof(stkInBuf))) { // NumRX + 3 - if ((stkInBuf[0] == 0x00) - && ((stkInBuf[1] == subcmd) || (stkInBuf[1] == 0x00 /* ignore zero returns */)) - && (stkInBuf[2] == 0x00)) { - *resByte = stkInBuf[3]; - } - return 1; + if (StkRcvPacket((void *)StkInBuf)) { // NumRX + 3 + if ((StkInBuf[0] == 0x00) && ((StkInBuf[1] == Cmd)||(StkInBuf[1] == 0x00)/* ignore zero returns */) &&(StkInBuf[2] == 0x00)) { + *ResByte = StkInBuf[3]; + } + return 1; } return 0; } @@ -267,37 +268,61 @@ static uint8_t _CMD_SPI_MULTI_EX(uint8_t * resByte, uint8_t subcmd, uint16_t add static uint8_t _CMD_LOAD_ADDRESS(ioMem_t *pMem) { // ignore 0xFFFF - // assume address is set before and we read or write the immediately following memory - if((pMem->addr == 0xffff)) - return 1; + // assume address is set before and we read or write the immediately following package + if((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; StkCmd = CMD_LOAD_ADDRESS; - StkSendPacketHeader(5); + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(5); // lo byte Msg len + StkSendByte(TOKEN); StkSendByte(CMD_LOAD_ADDRESS); StkSendByte(0); StkSendByte(0); - StkSendByte(pMem->addr >> 8); - StkSendByte(pMem->addr & 0xff); + StkSendByte(pMem->D_FLASH_ADDR_H); + StkSendByte(pMem->D_FLASH_ADDR_L); StkSendPacketFooter(); - return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); + return (StkRcvPacket((void *)StkInBuf)); } static uint8_t _CMD_READ_MEM_ISP(ioMem_t *pMem) { - StkSendPacketHeader(4); + uint8_t LenHi; + if (pMem->D_NUM_BYTES>0) { + LenHi=0; + } else { + LenHi=1; + } + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(4); // lo byte Msg len + StkSendByte(TOKEN); StkSendByte(StkCmd); - StkSendByte(pMem->len >> 8); - StkSendByte(pMem->len & 0xff); + StkSendByte(LenHi); + StkSendByte(pMem->D_NUM_BYTES); StkSendByte(CmdFlashEepromRead); StkSendPacketFooter(); - return StkRcvPacket(pMem->data, pMem->len); + return (StkRcvPacket(pMem->D_PTR_I)); } static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem) { - StkSendPacketHeader(pMem->len + 10); + uint8_16_u Len; + uint8_t LenLo = pMem->D_NUM_BYTES; + uint8_t LenHi; + if (LenLo) { + LenHi = 0; + Len.word = LenLo + 10; + } else { + LenHi = 1; + Len.word = 256 + 10; + } + StkSendPacketHeader(); + StkSendByte(Len.bytes[1]); // high byte Msg len + StkSendByte(Len.bytes[0]); // low byte Msg len + StkSendByte(TOKEN); StkSendByte(StkCmd); - StkSendByte(pMem->len >> 8); - StkSendByte(pMem->len & 0xff); + StkSendByte(LenHi); + StkSendByte(LenLo); StkSendByte(0); // mode StkSendByte(0); // delay StkSendByte(0); // cmd1 @@ -305,82 +330,92 @@ static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem) StkSendByte(0); // cmd3 StkSendByte(0); // poll1 StkSendByte(0); // poll2 - for(int i = 0; i < pMem->len; i++) - StkSendByte(pMem->data[i]); + do { + StkSendByte(*pMem->D_PTR_I); + pMem->D_PTR_I++; + LenLo--; + } while (LenLo); StkSendPacketFooter(); - return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); + return StkRcvPacket((void *)StkInBuf); } uint8_t Stk_SignOn(void) { - StkCmd = CMD_SIGN_ON; - StkSendPacketHeader(1); + StkCmd=CMD_SIGN_ON; + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(1); // lo byte Msg len + StkSendByte(TOKEN); StkSendByte(CMD_SIGN_ON); StkSendPacketFooter(); - return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); + return (StkRcvPacket((void *) StkInBuf)); } -uint8_t Stk_ConnectEx(escDeviceInfo_t *pDeviceInfo) +uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo) { - if (!Stk_SignOn()) - return 0; - uint8_t signature[3]; // device signature, MSB first - for(unsigned i = 0; i < sizeof(signature); i++) { - if (!_CMD_SPI_MULTI_EX(&signature[i], SPI_SIGNATURE_READ, i)) - return 0; + if (Stk_SignOn()) { + if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[1], signature_r,0,1)) { + if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[0], signature_r,0,2)) { + return 1; + } + } } - // convert signature to little endian - pDeviceInfo->signature = (signature[1] << 8) | signature[2]; - pDeviceInfo->signature2 = signature[0]; - return 1; + return 0; } uint8_t Stk_Chip_Erase(void) { StkCmd = CMD_CHIP_ERASE_ISP; - StkSendPacketHeader(7); - StkSendByte(StkCmd); + StkSendPacketHeader(); + StkSendByte(0); // high byte Msg len + StkSendByte(7); // low byte Msg len + StkSendByte(TOKEN); + StkSendByte(CMD_CHIP_ERASE_ISP); StkSendByte(20); // ChipErase_eraseDelay atmega8 - StkSendByte(0); // ChipErase_pollMethod atmega8 + StkSendByte(0); // ChipErase_pollMethod atmega8 StkSendByte(0xAC); StkSendByte(0x88); StkSendByte(0x13); StkSendByte(0x76); StkSendPacketFooter(); - return StkRcvPacket(stkInBuf, sizeof(stkInBuf)); + return (StkRcvPacket(StkInBuf)); } uint8_t Stk_ReadFlash(ioMem_t *pMem) { - if (!_CMD_LOAD_ADDRESS(pMem)) - return 0; - StkCmd = CMD_READ_FLASH_ISP; - return _CMD_READ_MEM_ISP(pMem); + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_READ_FLASH_ISP; + return (_CMD_READ_MEM_ISP(pMem)); + } + return 0; } uint8_t Stk_ReadEEprom(ioMem_t *pMem) { - if (!_CMD_LOAD_ADDRESS(pMem)) - return 0; - StkCmd = CMD_READ_EEPROM_ISP; - return _CMD_READ_MEM_ISP(pMem); + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_READ_EEPROM_ISP; + return (_CMD_READ_MEM_ISP(pMem)); + } + return 0; } uint8_t Stk_WriteFlash(ioMem_t *pMem) { - if (!_CMD_LOAD_ADDRESS(pMem)) - return 0; - StkCmd = CMD_PROGRAM_FLASH_ISP; - return _CMD_PROGRAM_MEM_ISP(pMem); + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_PROGRAM_FLASH_ISP; + return (_CMD_PROGRAM_MEM_ISP(pMem)); + } + return 0; } uint8_t Stk_WriteEEprom(ioMem_t *pMem) { - if (!_CMD_LOAD_ADDRESS(pMem)) - return 0; - StkCmd = CMD_PROGRAM_EEPROM_ISP; - return _CMD_PROGRAM_MEM_ISP(pMem); + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_PROGRAM_EEPROM_ISP; + return (_CMD_PROGRAM_MEM_ISP(pMem)); + } + return 0; } - +#endif #endif diff --git a/src/main/io/serial_4way_stk500v2.h b/src/main/io/serial_4way_stk500v2.h index 8f2fddcf40..80ca89826d 100644 --- a/src/main/io/serial_4way_stk500v2.h +++ b/src/main/io/serial_4way_stk500v2.h @@ -17,14 +17,11 @@ */ #pragma once -#ifdef USE_SERIAL_4WAY_SK_BOOTLOADER - uint8_t Stk_SignOn(void); -uint8_t Stk_ConnectEx(escDeviceInfo_t *pDeviceInfo); +uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo); uint8_t Stk_ReadEEprom(ioMem_t *pMem); uint8_t Stk_WriteEEprom(ioMem_t *pMem); uint8_t Stk_ReadFlash(ioMem_t *pMem); uint8_t Stk_WriteFlash(ioMem_t *pMem); uint8_t Stk_Chip_Erase(void); -#endif From 669b8e72395b26ce28e87d4503eda50cc61c5e4f Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 16 Jul 2016 21:09:32 +1000 Subject: [PATCH 056/456] Small fix for VCP port buffer send incomplete --- src/main/drivers/serial_usb_vcp.c | 7 +++---- src/main/vcpf4/usbd_cdc_vcp.c | 13 ++++++------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index f225dd62bb..61b3979967 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -90,7 +90,6 @@ static void usbVcpWriteBuf(serialPort_t *instance, void *data, int count) { UNUSED(instance); - if (!(usbIsConnected() && usbIsConfigured())) { return; } @@ -120,12 +119,12 @@ static bool usbVcpFlush(vcpPort_t *port) return false; } - uint32_t txed; + uint8_t txed = 0; uint32_t start = millis(); do { - txed = CDC_Send_DATA(port->txBuf, count); - } while (txed != count && (millis() - start < USB_TIMEOUT)); + txed += CDC_Send_DATA(&port->txBuf[txed], count-txed); + } while (txed < count && (millis() - start < USB_TIMEOUT)); return txed == count; } diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index f312e4a34c..879b2edc66 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -166,7 +166,7 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) * this function. * @param Buf: Buffer of data to be sent * @param Len: Number of data to be sent (in bytes) - * @retval Result of the opeartion: USBD_OK if all operations are OK else VCP_FAIL + * @retval Result of the operation: USBD_OK if all operations are OK else VCP_FAIL */ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) { @@ -191,18 +191,17 @@ uint8_t usbAvailable(void) *******************************************************************************/ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) { - uint32_t ch = 0; + uint32_t count = 0; - while (usbAvailable() && ch < len) { - recvBuf[ch] = usbData.buffer[usbData.bufferOutPosition]; + while (usbAvailable() && count < len) { + recvBuf[count] = usbData.buffer[usbData.bufferOutPosition]; usbData.bufferOutPosition = (usbData.bufferOutPosition + 1) % USB_RX_BUFSIZE; - ch++; + count++; receiveLength--; } - return ch; + return count; } - /** * @brief VCP_DataRx * Data received over USB OUT endpoint are sent over CDC interface From 187c41cfd13c7c50dbff6ea3b881fee02e99c786 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 16 Jul 2016 21:12:07 +1000 Subject: [PATCH 057/456] Removed unnecessary include --- src/main/io/serial_4way.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index b065dfd91c..9dd777b462 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -32,7 +32,6 @@ #include "drivers/pwm_output.h" #include "drivers/light_led.h" #include "drivers/system.h" -#include "drivers/buf_writer.h" #include "flight/mixer.h" #include "io/beeper.h" #include "io/serial_msp.h" From e7eb534401bb021efaae366dc78abcc83a53418e Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 17 Jul 2016 07:56:55 +1000 Subject: [PATCH 058/456] minor cleanup and a little extra safety --- src/main/drivers/serial_usb_vcp.c | 32 +++++++++++++++++-------------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 61b3979967..8360b0068f 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -77,13 +77,10 @@ static uint8_t usbVcpRead(serialPort_t *instance) uint8_t buf[1]; - uint32_t rxed = 0; - - while (rxed < 1) { - rxed += CDC_Receive_DATA((uint8_t*)buf + rxed, 1 - rxed); + while (true) { + if (CDC_Receive_DATA(buf, 1)) + return buf[0]; } - - return buf[0]; } static void usbVcpWriteBuf(serialPort_t *instance, void *data, int count) @@ -95,8 +92,10 @@ static void usbVcpWriteBuf(serialPort_t *instance, void *data, int count) } uint32_t start = millis(); - for (uint8_t *p = data; count > 0; ) { - uint32_t txed = CDC_Send_DATA(p, count); + uint8_t *p = data; + uint32_t txed = 0; + while (count > 0) { + txed = CDC_Send_DATA(p, count); count -= txed; p += txed; @@ -119,14 +118,19 @@ static bool usbVcpFlush(vcpPort_t *port) return false; } - uint8_t txed = 0; uint32_t start = millis(); + uint8_t *p = port->txBuf; + uint32_t txed = 0; + while (count > 0) { + txed = CDC_Send_DATA(p, count); + count -= txed; + p += txed; - do { - txed += CDC_Send_DATA(&port->txBuf[txed], count-txed); - } while (txed < count && (millis() - start < USB_TIMEOUT)); - - return txed == count; + if (millis() - start > USB_TIMEOUT) { + break; + } + } + return count == 0; } static void usbVcpWrite(serialPort_t *instance, uint8_t c) From 0f4b3dbe7f094aab9f4e87ba4b48ab1b847ed608 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 17 Jul 2016 09:35:22 +0100 Subject: [PATCH 059/456] Further standardisation of target.h files --- src/main/target/AIR32/target.h | 3 +-- src/main/target/ALIENFLIGHTF4/target.h | 3 +-- src/main/target/CHEBUZZF3/target.h | 3 +-- src/main/target/COLIBRI_RACE/target.h | 20 ++++++++------------ src/main/target/EUSTM32F103RC/target.h | 3 ++- src/main/target/FURYF3/target.h | 12 ++++-------- src/main/target/LUX_RACE/target.h | 20 +++++++++----------- src/main/target/MOTOLAB/target.h | 3 +-- src/main/target/NAZE/target.h | 15 ++++++--------- src/main/target/REVO/target.h | 7 +++---- src/main/target/REVONANO/target.h | 7 +++---- src/main/target/RMDO/target.h | 6 +----- src/main/target/SPARKY/target.h | 3 +-- src/main/target/SPRACINGF3/target.h | 9 +++------ src/main/target/STM32F3DISCOVERY/target.h | 11 +++++------ 15 files changed, 49 insertions(+), 76 deletions(-) diff --git a/src/main/target/AIR32/target.h b/src/main/target/AIR32/target.h index 10bc3a49be..e4c97dfc55 100644 --- a/src/main/target/AIR32/target.h +++ b/src/main/target/AIR32/target.h @@ -26,8 +26,6 @@ #define BEEPER PA0 -#define USABLE_TIMER_CHANNEL_COUNT 9 - // MPU6050 interrupts #define USE_EXTI #define MPU_INT_EXTI PA15 @@ -142,5 +140,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 45a40fb0f1..5bd61457fc 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -93,8 +93,6 @@ //#define USE_FLASHFS //#define USE_FLASH_M25P16 -#define USABLE_TIMER_CHANNEL_COUNT 13 - #define USE_VCP #define USE_UART1 @@ -175,5 +173,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 13 #define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) ) diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index 84f7071c86..5623a8a87f 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -32,8 +32,6 @@ #define BEEPER PE9 // Red LEDs - PE9/PE13 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 18 - #define USE_SPI #define USE_SPI_DEVICE_1 #define USE_SPI_DEVICE_2 @@ -113,5 +111,6 @@ #define TARGET_IO_PORTE 0xffff #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)|BIT(9)|BIT(10)) +#define USABLE_TIMER_CHANNEL_COUNT 18 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index f98d39eb36..d4db45a6de 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -31,7 +31,14 @@ #define BEEPER PB13 #define BEEPER_INVERTED +// MPU6500 interrupt #define USE_EXTI +#define MPU_INT_EXTI PA5 +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW +//#define DEBUG_MPU_DATA_READY_INTERRUPT + #define USE_SPI #define USE_SPI_DEVICE_1 @@ -47,10 +54,6 @@ #define MPU6000_CS_PIN SPI1_NSS_PIN #define MPU6000_SPI_INSTANCE SPI1 -#define USABLE_TIMER_CHANNEL_COUNT 11 - -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready - #define GYRO #define USE_GYRO_SPI_MPU6000 #define GYRO_MPU6000_ALIGN CW270_DEG @@ -119,14 +122,6 @@ #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER - -// MPU6500 interrupt -#define USE_EXTI -#define MPU_INT_EXTI PA5 -//#define DEBUG_MPU_DATA_READY_INTERRUPT -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW - #define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -141,5 +136,6 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 11 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 87c531b109..12ec85f371 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -23,9 +23,10 @@ #define LED1 PB4 #define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO -#define INVERTER_USART USART2 +#define INVERTER_USART USART2 #define USE_EXTI + #define MPU6000_CS_GPIO GPIOB #define MPU6000_CS_PIN PB12 #define MPU6000_SPI_INSTANCE SPI2 diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 82323ad058..858b2d76e9 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -20,9 +20,6 @@ #define TARGET_BOARD_IDENTIFIER "FYF3" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - -#define MPU_INT_EXTI PC4 -#define USE_EXTI #define CONFIG_PREFER_ACC_ON #define LED0 PC14 @@ -30,18 +27,18 @@ #define BEEPER PC15 #define BEEPER_INVERTED +#define USE_EXTI +#define MPU_INT_EXTI PC4 #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect - #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define GYRO - #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 #define MPU6500_CS_PIN PA4 #define MPU6500_SPI_INSTANCE SPI1 +#define GYRO #define USE_GYRO_SPI_MPU6000 #define GYRO_MPU6000_ALIGN CW180_DEG #define USE_ACC_SPI_MPU6000 @@ -101,8 +98,6 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -#define USABLE_TIMER_CHANNEL_COUNT 8 - #define USB_IO #define USE_VCP @@ -167,5 +162,6 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTF (BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 8 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index ef5fc8547c..b0a2bf7520 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -29,6 +29,14 @@ #define BEEPER PB13 #define BEEPER_INVERTED +// MPU6500 interrupt +#define USE_EXTI +#define MPU_INT_EXTI PA5 +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready +//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + #define USE_SPI #define USE_SPI_DEVICE_1 @@ -40,10 +48,6 @@ #define MPU6500_CS_PIN SPI1_NSS_PIN #define MPU6500_SPI_INSTANCE SPI1 -#define USABLE_TIMER_CHANNEL_COUNT 11 - -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready - #define GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 @@ -90,13 +94,6 @@ #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER -// MPU6500 interrupt -#define USE_EXTI -#define MPU_INT_EXTI PA5 -//#define DEBUG_MPU_DATA_READY_INTERRUPT -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW - #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define SPEKTRUM_BIND @@ -112,5 +109,6 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 11 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15)) diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 1db54b9886..fe465da9c9 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -28,8 +28,6 @@ #define BEEPER PA0 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 9 - // MPU6050 interrupts #define USE_EXTI #define MPU_INT_EXTI PA15 @@ -147,5 +145,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 390dc39f2a..512746d003 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -37,6 +37,12 @@ #define INVERTER_USART USART2 #define USE_EXTI +#define MAG_INT_EXTI PC14 +#define EXTI_CALLBACK_HANDLER_COUNT 3 // MPU data ready, MAG data ready, BMP085 EOC +//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define USE_MPU_DATA_READY_SIGNAL +//#define DEBUG_MAG_DATA_READY_INTERRUPT +#define USE_MAG_DATA_READY_SIGNAL // SPI2 // PB15 28 SPI2_MOSI @@ -65,15 +71,6 @@ #define USE_FLASHFS #define USE_FLASH_M25P16 -#define EXTI_CALLBACK_HANDLER_COUNT 3 // MPU data ready, MAG data ready, BMP085 EOC - -//#define DEBUG_MPU_DATA_READY_INTERRUPT -#define USE_MPU_DATA_READY_SIGNAL - -//#define DEBUG_MAG_DATA_READY_INTERRUPT -#define USE_MAG_DATA_READY_SIGNAL -#define MAG_INT_EXTI PC14 - #define GYRO #define USE_GYRO_MPU3050 #define USE_GYRO_MPU6050 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index f566b07da7..9aab5b4b10 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -44,10 +44,10 @@ #define ACC_MPU6000_ALIGN CW270_DEG // MPU6000 interrupts +#define USE_EXTI +#define MPU_INT_EXTI PC4 #define USE_MPU_DATA_READY_SIGNAL #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PC4 -#define USE_EXTI #define MAG #define USE_MAG_HMC5883 @@ -69,8 +69,6 @@ #define USE_FLASHFS #define USE_FLASH_M25P16 -#define USABLE_TIMER_CHANNEL_COUNT 12 - #define USE_VCP #define VBUS_SENSING_PIN PC5 @@ -121,4 +119,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff +#define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) ) diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index a298c3ed9d..448d235b81 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -53,13 +53,11 @@ #define USE_BARO_MS5611 // MPU9250 interrupts +#define USE_EXTI +#define MPU_INT_EXTI PA15 #define USE_MPU_DATA_READY_SIGNAL //#define ENSURE_MPU_DATA_READY_IS_LOW #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PA15 -#define USE_EXTI - -#define USABLE_TIMER_CHANNEL_COUNT 12 #define USE_VCP #define VBUS_SENSING_PIN PA9 @@ -98,5 +96,6 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff +#define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) ) diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index fadbdb874b..67d2232e68 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -26,17 +26,12 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready - #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW - #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH - #define GYRO #define USE_GYRO_MPU6050 #define GYRO_MPU6050_ALIGN CW270_DEG @@ -119,5 +114,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index db0528e984..ab498ff5ac 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -27,8 +27,6 @@ #define BEEPER PA1 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 11 - // MPU6050 interrupts #define USE_EXTI #define MPU_INT_EXTI PA15 @@ -127,5 +125,6 @@ #define TARGET_IO_PORTB (BIT(0)|BIT(1)|BIT(6)|BIT(10)|BIT(11)|BIT(14)|BIT(15)|BIT(3)|BIT(4)|BIT(5)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(12)|BIT(13)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 11 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index c68e8b2b21..4b2ac2e943 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -26,12 +26,9 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 17 - -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready - #define USE_EXTI #define MPU_INT_EXTI PC13 +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -57,13 +54,12 @@ #define ENSURE_MAG_DATA_READY_IS_HIGH #define MAG_INT_EXTI PC14 - #define USE_FLASHFS #define USE_FLASH_M25P16 #define SONAR -#define SONAR_ECHO_PIN PB1 #define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 #define USE_UART1 #define USE_UART2 @@ -131,5 +127,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index b5e9ae0e0f..592833bc48 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -81,15 +81,15 @@ // Support the GY-91 MPU9250 dev board #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define MPU6500_CS_PIN PC14 -#define MPU6500_SPI_INSTANCE SPI2 -#define GYRO_MPU6500_ALIGN CW270_DEG_FLIP +#define MPU6500_CS_PIN PC14 +#define MPU6500_SPI_INSTANCE SPI2 +#define GYRO_MPU6500_ALIGN CW270_DEG_FLIP #define ACC #define USE_ACC_LSM303DLHC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG_FLIP +#define ACC_MPU6500_ALIGN CW270_DEG_FLIP //#define BARO //#define BMP280_CS_PIN PB12 @@ -136,7 +136,7 @@ //#define UART2_RX_PINSOURCE GPIO_PinSource6 #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) +#define I2C_DEVICE (I2CDEV_1) #define USE_ADC #define ADC_INSTANCE ADC1 @@ -165,6 +165,5 @@ #define TARGET_IO_PORTE 0xffff #define TARGET_IO_PORTF 0x00ff - #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) From 221f617799ba73841e56b37e99b662dbd7f9806c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 21:10:00 +0200 Subject: [PATCH 060/456] Fix for missing I gain (float) --- src/main/flight/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8de667012c..e4702b30d7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -185,7 +185,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; float antiWindupScaler = constrainf(1.0f - (1.5f * (ABS(angleRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * errorLimiter * RateError * getdT() * antiWindupScaler, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * RateError * getdT() * pidProfile->I8[axis] * antiWindupScaler, -250.0f, 250.0f); // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings @@ -320,7 +320,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin // is normalized to cycle time = 2048. // Prevent Accumulation uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - uint16_t dynamicFactor = (1 << 8) - constrain(((ABS((int32_t)angleRate) << 6) / resetRate), 0, 1 << 8); + uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; errorGyroI[axis] = errorGyroI[axis] + ((uint16_t)targetPidLooptime >> 11) * dynamicKi; From 2d6e0da773ee0a7bebecc600dcbe00e49dff643e Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 21:12:46 +0200 Subject: [PATCH 061/456] Minor cleanup Fix for integer PIDC --- src/main/flight/mixer.c | 4 ---- src/main/flight/pid.c | 2 +- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 5b0ae97aa0..a35e051e6a 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -836,10 +836,6 @@ void mixTable(void) throttleMax = throttleMax - (rollPitchYawMixRange / 2); } - // adjust feedback to scale PID error inputs to our limitations. - errorLimiter = constrainf(((float)throttleRange / rollPitchYawMixRange), 0.1f, 1.0f); - if (debugMode == DEBUG_AIRMODE) debug[1] = errorLimiter * 100; - // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (i = 0; i < motorCount; i++) { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e4702b30d7..0c08f64ce7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -323,7 +323,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - errorGyroI[axis] = errorGyroI[axis] + ((uint16_t)targetPidLooptime >> 11) * dynamicKi; + errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings From 9c50ed8769c84d1a9770e198c2ad3e0a142e96c1 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 21:35:04 +0200 Subject: [PATCH 062/456] anti desync depricated // Replaced by motor_accel_limit --- src/main/config/config.c | 2 +- src/main/flight/mixer.c | 38 ++++++++++++++++++++++++-------------- src/main/io/escservo.h | 2 +- src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 4 ++-- 5 files changed, 29 insertions(+), 19 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index a9723ecaa9..5479b5820e 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -278,7 +278,7 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) #endif escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; - escAndServoConfig->escDesyncProtection = 0; + escAndServoConfig->accelerationLimitPercent = 15; } void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a35e051e6a..e4043d4d04 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -830,17 +830,40 @@ void mixTable(void) rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]); } // Get the maximum correction by setting offset to center - if (!escAndServoConfig->escDesyncProtection) throttleMin = throttleMax = throttleMin + (throttleRange / 2); + throttleMin = throttleMax = throttleMin + (throttleRange / 2); } else { throttleMin = throttleMin + (rollPitchYawMixRange / 2); throttleMax = throttleMax - (rollPitchYawMixRange / 2); } + // Keep track for motor update timing + float motorDtms; + if (escAndServoConfig->accelerationLimitPercent) { + static uint32_t previousMotorTime; + uint32_t currentMotorTime = micros(); + motorDtms = (float) (currentMotorTime - previousMotorTime) / 1000.0f; + previousMotorTime = currentMotorTime; + } + // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (i = 0; i < motorCount; i++) { motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax); + // Accel limit. Prevent PID controller to output huge ramps to the motors. Only limiting acceleration. + if (escAndServoConfig->accelerationLimitPercent) { + static int16_t lastFilteredMotor[MAX_SUPPORTED_MOTORS]; + + // acceleration limit + float delta = motor[i] - lastFilteredMotor[i]; + const float maxDeltaPerMs = throttleRange * ((float)escAndServoConfig->accelerationLimitPercent / 100.0f); + float maxDelta = maxDeltaPerMs * motorDtms; + if (delta > maxDelta) { // accelerating too hard + motor[i] = lastFilteredMotor[i] + maxDelta; + } + lastFilteredMotor[i] = motor[i]; + } + if (isFailsafeActive) { motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); } else if (feature(FEATURE_3D)) { @@ -859,19 +882,6 @@ void mixTable(void) motor[i] = escAndServoConfig->mincommand; } } - - // Experimental Code. Anti Desync feature for ESC's - if (escAndServoConfig->escDesyncProtection) { - const int16_t maxThrottleStep = constrain(escAndServoConfig->escDesyncProtection / (1000 / targetPidLooptime), 5, 10000); - - // Only makes sense when it's within the range - if (maxThrottleStep < throttleRange) { - static int16_t motorPrevious[MAX_SUPPORTED_MOTORS]; - - motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep); - motorPrevious[i] = motor[i]; - } - } } // Disarmed mode diff --git a/src/main/io/escservo.h b/src/main/io/escservo.h index b73f9920f3..d97f41df49 100644 --- a/src/main/io/escservo.h +++ b/src/main/io/escservo.h @@ -24,5 +24,5 @@ typedef struct escAndServoConfig_s { uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500. - uint16_t escDesyncProtection; // Value that a motor is allowed to increase or decrease in a period of 1ms + uint16_t accelerationLimitPercent; // Percentage that motor is allowed to increase or decrease in a period of 1ms } escAndServoConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 4904764252..a888065fa1 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -601,7 +601,7 @@ const clivalue_t valueTable[] = { { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "anti_desync_power_step", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.escDesyncProtection, .config.minmax = { 0, 10000 } }, + { "motor_accel_limit_percent", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.accelerationLimitPercent, .config.minmax = { 0, 10000 } }, { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 9a5b96ff5b..952bc73b53 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1226,7 +1226,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentControlRateProfile->rcYawRate8); serialize16(masterConfig.rxConfig.airModeActivateThreshold); serialize8(masterConfig.rxConfig.rcSmoothInterval); - serialize16(masterConfig.escAndServoConfig.escDesyncProtection); + serialize16(masterConfig.escAndServoConfig.accelerationLimitPercent); break; case MSP_SENSOR_CONFIG: headSerialReply(3); @@ -1801,7 +1801,7 @@ static bool processInCommand(void) currentControlRateProfile->rcYawRate8 = read8(); masterConfig.rxConfig.airModeActivateThreshold = read16(); masterConfig.rxConfig.rcSmoothInterval = read8(); - masterConfig.escAndServoConfig.escDesyncProtection = read16(); + masterConfig.escAndServoConfig.accelerationLimitPercent = read16(); break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); From 37fd2e5adc4677b257d2a550ac8b3b59bc9cb543 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 17 Jul 2016 23:35:56 +0200 Subject: [PATCH 063/456] Initial PID controller separation // Betaflight pidc for future development --- src/main/blackbox/blackbox.c | 3 +- src/main/config/config.c | 22 +++++----- src/main/flight/mixer.c | 15 +++---- src/main/flight/mixer.h | 2 +- src/main/flight/pid.c | 79 ++++++++++++++++++++++++++--------- src/main/flight/pid.h | 20 ++++++--- src/main/io/escservo.h | 1 - src/main/io/serial_cli.c | 14 ++++--- src/main/io/serial_msp.c | 10 ++--- src/main/mw.c | 4 +- src/main/sensors/battery.c | 2 +- src/main/sensors/battery.h | 1 - src/main/target/CC3D/target.h | 1 - 13 files changed, 113 insertions(+), 61 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 9ab7e8a0b0..96281b05fe 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1211,7 +1211,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); - BLACKBOX_PRINT_HEADER_LINE("dynamic_pid:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pid); + BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.vbatPidCompensation); BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); @@ -1226,7 +1226,6 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware); BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.mag_hardware); BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); - BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation); BLACKBOX_PRINT_HEADER_LINE("rc_smooth_interval:%d", masterConfig.rxConfig.rcSmoothInterval); BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold); BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures); diff --git a/src/main/config/config.c b/src/main/config/config.c index 5479b5820e..be9d615ed3 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -182,12 +182,7 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) void resetPidProfile(pidProfile_t *pidProfile) { - -#if (defined(STM32F10X)) - pidProfile->pidController = PID_CONTROLLER_INTEGER; -#else - pidProfile->pidController = PID_CONTROLLER_FLOAT; -#endif + pidProfile->pidController = PID_CONTROLLER_BETAFLIGHT; pidProfile->P8[ROLL] = 45; pidProfile->I8[ROLL] = 40; @@ -224,7 +219,14 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yawItermIgnoreRate = 50; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; - pidProfile->dynamic_pid = 1; + pidProfile->vbatPidCompensation = 0; + + // Betaflight PID controller parameters + pidProfile->toleranceBand = 15; + pidProfile->toleranceBandReduction = 35; + pidProfile->zeroCrossAllowanceCount = 3; + pidProfile->accelerationLimitPercent = 15; + pidProfile->itermThrottleGain = 0; #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. @@ -278,7 +280,6 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) #endif escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; - escAndServoConfig->accelerationLimitPercent = 15; } void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) @@ -311,7 +312,6 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig) batteryConfig->vbatmincellvoltage = 33; batteryConfig->vbatwarningcellvoltage = 35; batteryConfig->vbathysteresis = 1; - batteryConfig->vbatPidCompensation = 0; batteryConfig->currentMeterOffset = 0; batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A) batteryConfig->batteryCapacity = 0; @@ -452,15 +452,15 @@ static void resetConf(void) masterConfig.gyro_lpf = 0; // 256HZ default #ifdef STM32F10X masterConfig.gyro_sync_denom = 8; + masterConfig.pid_process_denom = 1; #else masterConfig.gyro_sync_denom = 4; + masterConfig.pid_process_denom = 2; #endif masterConfig.gyro_soft_lpf_hz = 100; masterConfig.gyro_soft_notch_hz = 0; masterConfig.gyro_soft_notch_q = 5; - masterConfig.pid_process_denom = 2; - masterConfig.debug_mode = 0; resetAccelerometerTrims(&masterConfig.accZero); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e4043d4d04..aa9f0fc90a 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -755,13 +755,14 @@ STATIC_UNIT_TESTED void servoMixer(void) #endif -void mixTable(void) +void mixTable(void *pidProfilePtr) { uint32_t i = 0; fix12_t vbatCompensationFactor = 0; static fix12_t mixReduction; bool use_vbat_compensation = false; - if (batteryConfig && batteryConfig->vbatPidCompensation) { + pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr; + if (batteryConfig && pidProfile->vbatPidCompensation) { use_vbat_compensation = true; vbatCompensationFactor = calculateVbatPidCompensation(); } @@ -836,9 +837,9 @@ void mixTable(void) throttleMax = throttleMax - (rollPitchYawMixRange / 2); } - // Keep track for motor update timing + // Keep track for motor update timing // Only for Betaflight pid controller to keep legacy pidc basic and free from additional float math float motorDtms; - if (escAndServoConfig->accelerationLimitPercent) { + if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) { static uint32_t previousMotorTime; uint32_t currentMotorTime = micros(); motorDtms = (float) (currentMotorTime - previousMotorTime) / 1000.0f; @@ -850,13 +851,13 @@ void mixTable(void) for (i = 0; i < motorCount; i++) { motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax); - // Accel limit. Prevent PID controller to output huge ramps to the motors. Only limiting acceleration. - if (escAndServoConfig->accelerationLimitPercent) { + // Accel limit. Prevent PID controller to output huge ramps to the motors. Only limiting acceleration. Only for Betaflight pid controller to keep legacy pidc basic + if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) { static int16_t lastFilteredMotor[MAX_SUPPORTED_MOTORS]; // acceleration limit float delta = motor[i] - lastFilteredMotor[i]; - const float maxDeltaPerMs = throttleRange * ((float)escAndServoConfig->accelerationLimitPercent / 100.0f); + const float maxDeltaPerMs = throttleRange * ((float)pidProfile->accelerationLimitPercent / 100.0f); float maxDelta = maxDeltaPerMs * motorDtms; if (delta > maxDelta) { // accelerating too hard motor[i] = lastFilteredMotor[i] + maxDelta; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 4f4869b205..b76aafa1bd 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -211,7 +211,7 @@ void loadCustomServoMixer(void); int servoDirection(int servoIndex, int fromChannel); #endif void mixerResetDisarmedMotors(void); -void mixTable(void); +void mixTable(void *pidProfilePtr); void syncMotors(bool enabled); void writeMotors(void); void stopMotors(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0c08f64ce7..3907e3955d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -62,14 +62,12 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; uint8_t PIDweight[3]; static int32_t errorGyroI[3]; -#ifndef SKIP_PID_LUXFLOAT static float errorGyroIf[3]; -#endif -static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, +static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); -pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we using +pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using void setTargetPidLooptime(uint8_t pidProcessDenom) { @@ -82,9 +80,7 @@ void pidResetErrorGyroState(void) for (axis = 0; axis < 3; axis++) { errorGyroI[axis] = 0; -#ifndef SKIP_PID_LUXFLOAT errorGyroIf[axis] = 0.0f; -#endif } } @@ -101,8 +97,8 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static pt1Filter_t deltaFilter[3]; static pt1Filter_t yawFilter; -#ifndef SKIP_PID_LUXFLOAT -static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, +// Experimental betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage +static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) { float RateError = 0, gyroRate = 0, RateErrorSmooth = 0; @@ -133,6 +129,25 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat } } + // Yet Highly experimental and under test and development + // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) + static float kiThrottleGain = 1.0f; + + if (pidProfile->itermThrottleGain) { + const uint16_t maxLoopCount = 20000 / targetPidLooptime; + const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; + static int16_t previousThrottle; + static uint16_t loopIncrement; + + if (loopIncrement >= maxLoopCount) { + kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 + previousThrottle = rcCommand[THROTTLE]; + loopIncrement = 0; + } else { + loopIncrement++; + } + } + // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { @@ -164,6 +179,34 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = angleRate[axis] - gyroRate; + float dynReduction = tpaFactor; + // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount + if (pidProfile->toleranceBand) { + const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; + static uint8_t zeroCrossCount[3]; + static uint8_t currentErrorPolarity[3]; + if (ABS(RateError) < pidProfile->toleranceBand) { + if (zeroCrossCount[axis]) { + if (currentErrorPolarity[axis] == POSITIVE_ERROR) { + if (RateError < 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = NEGATIVE_ERROR; + } + } else { + if (RateError > 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = POSITIVE_ERROR; + } + } + } else { + dynReduction *= constrainf(ABS(RateError) / pidProfile->toleranceBand, minReduction, 1.0f); + } + } else { + zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; + currentErrorPolarity[axis] = (RateError > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; + } + } + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { // Smoothed Error for Derivative when delta from error selected if (flightModeFlags && axis != YAW) @@ -173,7 +216,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat } // -----calculate P component - PTerm = PTermScale * RateError * pidProfile->P8[axis] * tpaFactor; + PTerm = PTermScale * RateError * pidProfile->P8[axis] * dynReduction; // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { @@ -185,7 +228,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; float antiWindupScaler = constrainf(1.0f - (1.5f * (ABS(angleRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * RateError * getdT() * pidProfile->I8[axis] * antiWindupScaler, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * RateError * getdT() * pidProfile->I8[axis] * antiWindupScaler * kiThrottleGain, -250.0f, 250.0f); // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings @@ -220,7 +263,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat // Filter delta if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - DTerm = constrainf(DTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); + DTerm = constrainf(DTermScale * delta * (float)pidProfile->D8[axis] * dynReduction, -300.0f, 300.0f); // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); @@ -239,9 +282,9 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat #endif } } -#endif -static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, +// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future +static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) { int axis; @@ -385,13 +428,11 @@ void pidSetController(pidControllerType_e type) { switch (type) { default: - case PID_CONTROLLER_INTEGER: - pid_controller = pidInteger; + case PID_CONTROLLER_LEGACY: + pid_controller = pidLegacy; break; -#ifndef SKIP_PID_LUXFLOAT - case PID_CONTROLLER_FLOAT: - pid_controller = pidFloat; -#endif + case PID_CONTROLLER_BETAFLIGHT: + pid_controller = pidBetaflight; } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index a407aeb80a..56767f8719 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -41,8 +41,8 @@ typedef enum { } pidIndex_e; typedef enum { - PID_CONTROLLER_INTEGER = 1, // Integer math to gain some more performance from F1 targets - PID_CONTROLLER_FLOAT, + PID_CONTROLLER_LEGACY = 0, // Legacy PID controller. Old INT / Rewrite with 2.9 status. Fastest performance....least math. Will stay same in the future + PID_CONTROLLER_BETAFLIGHT, // Betaflight PID controller. Old luxfloat -> float evolution. More math added and maintained in the future PID_COUNT } pidControllerType_e; @@ -57,10 +57,13 @@ typedef enum { SUPEREXPO_YAW_ALWAYS } pidSuperExpoYaw_e; -#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2) +typedef enum { + NEGATIVE_ERROR = 0, + POSITIVE_ERROR +} pidErrorPolarity_e; typedef struct pidProfile_s { - uint8_t pidController; // 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Luggi09s new baseflight pid + uint8_t pidController; // 1 = rewrite betaflight evolved from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Betaflight PIDc (Evolved Luxfloat) uint8_t P8[PID_ITEM_COUNT]; uint8_t I8[PID_ITEM_COUNT]; @@ -73,7 +76,14 @@ typedef struct pidProfile_s { uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm - uint8_t dynamic_pid; // Dynamic PID implementation (currently only P and I) + uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage + + // Betaflight PID controller parameters + uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances + uint8_t toleranceBandReduction; // Lowest possible P and D reduction in percentage + uint8_t zeroCrossAllowanceCount; // Amount of bouncebacks within tolerance band allowed before reduction kicks in + uint16_t accelerationLimitPercent; // Percentage that motor is allowed to increase or decrease in a period of 1ms + uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/escservo.h b/src/main/io/escservo.h index d97f41df49..66cd7db59e 100644 --- a/src/main/io/escservo.h +++ b/src/main/io/escservo.h @@ -24,5 +24,4 @@ typedef struct escAndServoConfig_s { uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500. - uint16_t accelerationLimitPercent; // Percentage that motor is allowed to increase or decrease in a period of 1ms } escAndServoConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a888065fa1..ec06f6fcc2 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -389,7 +389,7 @@ static const char * const lookupTableBlackboxDevice[] = { static const char * const lookupTablePidController[] = { - "UNUSED", "INT", "FLOAT" + "LEGACY", "BETAFLIGHT" }; static const char * const lookupTableSerialRX[] = { @@ -601,7 +601,6 @@ const clivalue_t valueTable[] = { { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "motor_accel_limit_percent", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.accelerationLimitPercent, .config.minmax = { 0, 10000 } }, { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently @@ -680,7 +679,6 @@ const clivalue_t valueTable[] = { { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, .config.minmax = { 10, 50 } }, { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, .config.minmax = { 10, 50 } }, { "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbathysteresis, .config.minmax = { 0, 250 } }, - { "vbat_pid_compensation", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -10000, 10000 } }, { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0, 3300 } }, { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, @@ -767,8 +765,14 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, - { "dynamic_iterm", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pid, .config.lookup = { TABLE_OFF_ON } }, - { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, + { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, + { "motor_accel_limit_percent", VAR_UINT16 | MASTER_VALUE, &masterConfig.profile[0].pidProfile.accelerationLimitPercent, .config.minmax = { 0, 10000 } }, + { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, + { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, + { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, + { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, + + { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 952bc73b53..054eb49403 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1219,14 +1219,14 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentProfile->pidProfile.yawItermIgnoreRate); serialize16(currentProfile->pidProfile.yaw_p_limit); serialize8(currentProfile->pidProfile.deltaMethod); - serialize8(masterConfig.batteryConfig.vbatPidCompensation); + serialize8(currentProfile->pidProfile.vbatPidCompensation); break; case MSP_SPECIAL_PARAMETERS: headSerialReply(1 + 2 + 1 + 2); serialize8(currentControlRateProfile->rcYawRate8); serialize16(masterConfig.rxConfig.airModeActivateThreshold); serialize8(masterConfig.rxConfig.rcSmoothInterval); - serialize16(masterConfig.escAndServoConfig.accelerationLimitPercent); + serialize16(currentProfile->pidProfile.accelerationLimitPercent); break; case MSP_SENSOR_CONFIG: headSerialReply(3); @@ -1295,7 +1295,7 @@ static bool processInCommand(void) read16(); break; case MSP_SET_PID_CONTROLLER: - currentProfile->pidProfile.pidController = constrain(read8(), 1, 2); + currentProfile->pidProfile.pidController = constrain(read8(), 0, 1); pidSetController(currentProfile->pidProfile.pidController); break; case MSP_SET_PID: @@ -1795,13 +1795,13 @@ static bool processInCommand(void) currentProfile->pidProfile.yawItermIgnoreRate = read16(); currentProfile->pidProfile.yaw_p_limit = read16(); currentProfile->pidProfile.deltaMethod = read8(); - masterConfig.batteryConfig.vbatPidCompensation = read8(); + currentProfile->pidProfile.vbatPidCompensation = read8(); break; case MSP_SET_SPECIAL_PARAMETERS: currentControlRateProfile->rcYawRate8 = read8(); masterConfig.rxConfig.airModeActivateThreshold = read16(); masterConfig.rxConfig.rcSmoothInterval = read8(); - masterConfig.escAndServoConfig.accelerationLimitPercent = read16(); + currentProfile->pidProfile.accelerationLimitPercent = read16(); break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); diff --git a/src/main/mw.c b/src/main/mw.c index 05c8d1a08f..91165131db 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -184,7 +184,7 @@ float calculateRate(int axis, int16_t rc) { angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f; } - if (currentProfile->pidProfile.pidController == PID_CONTROLLER_INTEGER) + if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection else return constrainf(angleRate / 4.1f, -1997.0f, 1997.0f); // Rate limit protection (deg/sec) @@ -765,7 +765,7 @@ void subTaskMotorUpdate(void) previousMotorUpdateTime = startTime; } - mixTable(); + mixTable(¤tProfile->pidProfile); #ifdef USE_SERVOS filterServos(); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 53e7096480..70129156b0 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -212,7 +212,7 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea fix12_t calculateVbatPidCompensation(void) { fix12_t batteryScaler; - if (batteryConfig->vbatPidCompensation && feature(FEATURE_VBAT) && batteryCellCount > 1) { + if (feature(FEATURE_VBAT) && batteryCellCount > 1) { uint16_t maxCalculatedVoltage = batteryConfig->vbatmaxcellvoltage * batteryCellCount; batteryScaler = qConstruct(maxCalculatedVoltage, constrain(vbat, maxCalculatedVoltage - batteryConfig->vbatmaxcellvoltage, maxCalculatedVoltage)); } else { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 21197e07fc..e52541db70 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -43,7 +43,6 @@ typedef struct batteryConfig_s { uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V) uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V) uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V - uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A uint16_t currentMeterOffset; // offset of the current sensor in millivolt steps diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index cee25b2723..ab711c151a 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -121,7 +121,6 @@ #ifdef CC3D_OPBL #define SKIP_CLI_COMMAND_HELP -//#define SKIP_PID_LUXFLOAT #undef DISPLAY #undef SONAR #endif From 0ae5d9734e41bb26a459cd3e4eab30b04dc3dde8 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 18 Jul 2016 02:18:04 +0200 Subject: [PATCH 064/456] Add option to fully disable PID controller on zero throttle --- src/main/config/config.c | 1 + src/main/flight/mixer.c | 2 -- src/main/flight/pid.c | 15 +++++++++++---- src/main/flight/pid.h | 7 +++++++ src/main/io/serial_cli.c | 1 + src/main/mw.c | 6 ++++++ 6 files changed, 26 insertions(+), 6 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index be9d615ed3..1b7c714790 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -220,6 +220,7 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; + pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; // Betaflight PID controller parameters pidProfile->toleranceBand = 15; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index aa9f0fc90a..f8ac94b22b 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -70,8 +70,6 @@ static bool syncPwmWithPidLoop = false; static mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; -float errorLimiter = 1.0f; - #ifdef USE_SERVOS static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3907e3955d..d23db67a3d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -49,9 +49,10 @@ extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float errorLimiter; extern float angleRate[3], angleRateSmooth[3]; +static bool pidStabilisationEnabled; + int16_t axisPID[3]; #ifdef BLACKBOX @@ -76,14 +77,17 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) void pidResetErrorGyroState(void) { - int axis; - - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { errorGyroI[axis] = 0; errorGyroIf[axis] = 0.0f; } } +void pidStabilisationState(pidStabilisationState_e pidControllerState) +{ + pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false; +} + float getdT (void) { static float dT; @@ -269,6 +273,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); } + if (!pidStabilisationEnabled) axisPID[axis] = 0; + #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); @@ -409,6 +415,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina axisPID[axis] = PTerm + ITerm + DTerm; } + if (!pidStabilisationEnabled) axisPID[axis] = 0; #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 56767f8719..64182f8256 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -62,6 +62,11 @@ typedef enum { POSITIVE_ERROR } pidErrorPolarity_e; +typedef enum { + PID_STABILISATION_OFF = 0, + PID_STABILISATION_ON +} pidStabilisationState_e; + typedef struct pidProfile_s { uint8_t pidController; // 1 = rewrite betaflight evolved from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 2 = Betaflight PIDc (Evolved Luxfloat) @@ -77,6 +82,7 @@ typedef struct pidProfile_s { uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage + uint8_t zeroThrottleStabilisation; // Disable/Enable zero throttle stabilisation. Normally even without airmode P and D would be active. // Betaflight PID controller parameters uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances @@ -107,5 +113,6 @@ extern uint32_t targetPidLooptime; void pidSetController(pidControllerType_e type); void pidResetErrorGyroState(void); +void pidStabilisationState(pidStabilisationState_e pidControllerState); void setTargetPidLooptime(uint8_t pidProcessDenom); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ec06f6fcc2..768489a8de 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -766,6 +766,7 @@ const clivalue_t valueTable[] = { { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, + { "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } }, { "motor_accel_limit_percent", VAR_UINT16 | MASTER_VALUE, &masterConfig.profile[0].pidProfile.accelerationLimitPercent, .config.minmax = { 0, 10000 } }, { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, diff --git a/src/main/mw.c b/src/main/mw.c index 91165131db..2848abd40e 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -518,6 +518,12 @@ void processRx(void) This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ if (throttleStatus == THROTTLE_LOW && !wasAirmodeIsActivated) { pidResetErrorGyroState(); + if (currentProfile->pidProfile.zeroThrottleStabilisation) + pidStabilisationState(PID_STABILISATION_ON); + else + pidStabilisationState(PID_STABILISATION_OFF); + } else { + if (currentProfile->pidProfile.zeroThrottleStabilisation || wasAirmodeIsActivated) pidStabilisationState(PID_STABILISATION_ON); } // When armed and motors aren't spinning, do beeps and then disarm From e8924ec672dee6348eb4caa9cc57c902e6ee60ec Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 18 Jul 2016 18:52:08 +1200 Subject: [PATCH 065/456] Added number of profiles supported to MSP_STATUS_EX. --- src/main/io/serial_msp.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 054eb49403..56194367d2 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -672,7 +672,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_STATUS_EX: - headSerialReply(13); + headSerialReply(14); serialize16(cycleTime); #ifdef USE_I2C serialize16(i2cGetErrorCounter()); @@ -683,6 +683,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize32(packFlightModeFlags()); serialize8(masterConfig.current_profile_index); serialize16(constrain(averageSystemLoadPercent, 0, 100)); + serialize8(MAX_PROFILE_COUNT); break; case MSP_NAME: From 0f6bfd301fe4ee5552f0d3ba76aa760319a2d091 Mon Sep 17 00:00:00 2001 From: nathan Date: Sun, 17 Jul 2016 23:59:28 -0700 Subject: [PATCH 066/456] already defined in dma_stm32f4xx.c --- src/main/drivers/dma.c | 31 ------------------------------- 1 file changed, 31 deletions(-) diff --git a/src/main/drivers/dma.c b/src/main/drivers/dma.c index cc7540601f..0ad411312e 100644 --- a/src/main/drivers/dma.c +++ b/src/main/drivers/dma.c @@ -27,26 +27,6 @@ /* * DMA descriptors. */ -#ifdef STM32F4 -static dmaChannelDescriptor_t dmaDescriptors[] = { - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream0, 0, DMA1_Stream0_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream1, 4, DMA1_Stream1_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream2, 8, DMA1_Stream2_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream3, 12, DMA1_Stream3_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream4, 16, DMA1_Stream4_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream5, 20, DMA1_Stream5_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream6, 24, DMA1_Stream6_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA1, DMA1_Stream7, 28, DMA1_Stream7_IRQn, RCC_AHBPeriph_DMA1), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream0, 0, DMA2_Stream0_IRQn, RCC_AHBPeriph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream1, 4, DMA2_Stream1_IRQn, RCC_AHBPeriph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream2, 8, DMA2_Stream2_IRQn, RCC_AHBPeriph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream3, 12, DMA2_Stream3_IRQn, RCC_AHBPeriph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream4, 16, DMA2_Stream4_IRQn, RCC_AHBPeriph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream5, 20, DMA2_Stream5_IRQn, RCC_AHBPeriph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream6, 24, DMA2_Stream5_IRQn, RCC_AHBPeriph_DMA2), - DEFINE_DMA_CHANNEL(DMA2, DMA2_Stream7, 28, DMA2_Stream5_IRQn, RCC_AHBPeriph_DMA2), -}; -#else static dmaChannelDescriptor_t dmaDescriptors[] = { DEFINE_DMA_CHANNEL(DMA1, DMA1_Channel1, 0, DMA1_Channel1_IRQn, RCC_AHBPeriph_DMA1), DEFINE_DMA_CHANNEL(DMA1, DMA1_Channel2, 4, DMA1_Channel2_IRQn, RCC_AHBPeriph_DMA1), @@ -63,14 +43,10 @@ static dmaChannelDescriptor_t dmaDescriptors[] = { DEFINE_DMA_CHANNEL(DMA2, DMA2_Channel5, 16, DMA2_Channel5_IRQn, RCC_AHBPeriph_DMA2), #endif }; -#endif /* * DMA IRQ Handlers */ -#ifdef STM32F4 -DEFINE_DMA_IRQ_HANDLER(1, 0, DMA1_CH0_HANDLER) -#endif DEFINE_DMA_IRQ_HANDLER(1, 1, DMA1_CH1_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 2, DMA1_CH2_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 3, DMA1_CH3_HANDLER) @@ -79,9 +55,6 @@ DEFINE_DMA_IRQ_HANDLER(1, 5, DMA1_CH5_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 6, DMA1_CH6_HANDLER) DEFINE_DMA_IRQ_HANDLER(1, 7, DMA1_CH7_HANDLER) -#ifdef STM32F4 -DEFINE_DMA_IRQ_HANDLER(2, 0, DMA2_CH0_HANDLER) -#endif #if defined(STM32F3) || defined(STM32F10X_CL) DEFINE_DMA_IRQ_HANDLER(2, 1, DMA2_CH1_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 2, DMA2_CH2_HANDLER) @@ -89,10 +62,6 @@ DEFINE_DMA_IRQ_HANDLER(2, 3, DMA2_CH3_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 4, DMA2_CH4_HANDLER) DEFINE_DMA_IRQ_HANDLER(2, 5, DMA2_CH5_HANDLER) #endif -#ifdef STM32F4 -DEFINE_DMA_IRQ_HANDLER(2, 6, DMA2_CH6_HANDLER) -DEFINE_DMA_IRQ_HANDLER(2, 7, DMA2_CH7_HANDLER) -#endif void dmaInit(void) From ddf1f80b48218d5c015cdeb9f220ffc763b15e9a Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 18 Jul 2016 09:19:13 +0200 Subject: [PATCH 067/456] Revert "Revert "Betaflight led strip resubmit"" This reverts commit 63c7ae18ff8dc5d3a95f861956cbb00501b95f1d. --- src/main/common/utils.h | 11 + src/main/config/config.c | 4 +- src/main/config/config_master.h | 6 +- src/main/drivers/light_ws2811strip.h | 3 - .../drivers/light_ws2811strip_stm32f10x.c | 1 + .../drivers/light_ws2811strip_stm32f30x.c | 1 + .../drivers/light_ws2811strip_stm32f4xx.c | 1 + src/main/io/ledstrip.c | 1601 +++++++++-------- src/main/io/ledstrip.h | 206 ++- src/main/io/serial_cli.c | 58 +- src/main/io/serial_msp.c | 64 +- src/main/io/serial_msp.h | 4 +- src/main/main.c | 4 +- src/main/sensors/battery.c | 2 +- 14 files changed, 1136 insertions(+), 830 deletions(-) diff --git a/src/main/common/utils.h b/src/main/common/utils.h index a5c0591918..27a51f3984 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -71,4 +71,15 @@ http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html static inline int16_t cmp16(uint16_t a, uint16_t b) { return a-b; } static inline int32_t cmp32(uint32_t a, uint32_t b) { return a-b; } +// using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions +// than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions) +#ifdef UNIT_TEST +// Call memcpy when building unittest - this is easier that asm symbol name mangling (symbols start with _underscore on win32) +#include +static inline void memcpy_fn ( void * destination, const void * source, size_t num ) { memcpy(destination, source, num); }; +#else +void * memcpy_fn ( void * destination, const void * source, size_t num ) asm("memcpy"); +#endif + + #endif diff --git a/src/main/config/config.c b/src/main/config/config.c index 1b7c714790..bb6ff6ad66 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -616,8 +616,10 @@ static void resetConf(void) } #ifdef LED_STRIP - applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT); + applyDefaultColors(masterConfig.colors); applyDefaultLedStripConfig(masterConfig.ledConfigs); + applyDefaultModeColors(masterConfig.modeColors); + applyDefaultSpecialColors(&(masterConfig.specialColors)); masterConfig.ledstrip_visual_beeper = 0; #endif diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 5ce1d36833..29a236e565 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -119,8 +119,10 @@ typedef struct master_t { telemetryConfig_t telemetryConfig; #ifdef LED_STRIP - ledConfig_t ledConfigs[MAX_LED_STRIP_LENGTH]; - hsvColor_t colors[CONFIGURABLE_COLOR_COUNT]; + ledConfig_t ledConfigs[LED_MAX_STRIP_LENGTH]; + hsvColor_t colors[LED_CONFIGURABLE_COLOR_COUNT]; + modeColorIndexes_t modeColors[LED_MODE_COUNT]; + specialColorIndexes_t specialColors; uint8_t ledstrip_visual_beeper; // suppress LEDLOW mode if beeper is on #endif diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 974c19c655..853ac7acbc 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -53,6 +53,3 @@ bool isWS2811LedStripReady(void); extern uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; extern volatile uint8_t ws2811LedDataTransferInProgress; - -extern const hsvColor_t hsv_white; -extern const hsvColor_t hsv_black; diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 96bb85fd26..b4a9b54268 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -105,6 +105,7 @@ void ws2811LedStripHardwareInit(void) DMA_ITConfig(DMA1_Channel6, DMA_IT_TC, ENABLE); + const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index ff52a64836..722dd9a2e4 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -111,6 +111,7 @@ void ws2811LedStripHardwareInit(void) DMA_ITConfig(WS2811_DMA_CHANNEL, DMA_IT_TC, ENABLE); + const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index 377954ce99..e8676e2cdf 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -154,6 +154,7 @@ void ws2811LedStripHardwareInit(void) dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, 0); + const hsvColor_t hsv_white = { 0, 255, 255}; ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index e74ca001a0..4ab463bd1c 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -42,6 +42,7 @@ #include #include "common/axis.h" +#include "common/utils.h" #include "io/rc_controls.h" @@ -61,6 +62,8 @@ #include "io/osd.h" #include "io/vtx.h" +#include "rx/rx.h" + #include "flight/failsafe.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -74,6 +77,13 @@ #include "config/config_profile.h" #include "config/config_master.h" +/* +PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0); +PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0); +PG_REGISTER_ARR_WITH_RESET_FN(modeColorIndexes_t, LED_MODE_COUNT, modeColors, PG_MODE_COLOR_CONFIG, 0); +PG_REGISTER_WITH_RESET_FN(specialColorIndexes_t, specialColors, PG_SPECIAL_COLOR_CONFIG, 0); +*/ + static bool ledStripInitialised = false; static bool ledStripEnabled = true; @@ -82,73 +92,13 @@ static void ledStripDisable(void); //#define USE_LED_ANIMATION //#define USE_LED_RING_DEFAULT_CONFIG -// timers -#ifdef USE_LED_ANIMATION -static uint32_t nextAnimationUpdateAt = 0; +#define LED_STRIP_HZ(hz) ((int32_t)((1000 * 1000) / (hz))) +#define LED_STRIP_MS(ms) ((int32_t)(1000 * (ms))) + +#if LED_MAX_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH +# error "Led strip length must match driver" #endif -static uint32_t nextIndicatorFlashAt = 0; -static uint32_t nextWarningFlashAt = 0; -static uint32_t nextRotationUpdateAt = 0; - -#define LED_STRIP_20HZ ((1000 * 1000) / 20) -#define LED_STRIP_10HZ ((1000 * 1000) / 10) -#define LED_STRIP_5HZ ((1000 * 1000) / 5) - -#if MAX_LED_STRIP_LENGTH > WS2811_LED_STRIP_LENGTH -#error "Led strip length must match driver" -#endif - -// H S V -#define LED_BLACK { 0, 0, 0} -#define LED_WHITE { 0, 255, 255} -#define LED_RED { 0, 0, 255} -#define LED_ORANGE { 30, 0, 255} -#define LED_YELLOW { 60, 0, 255} -#define LED_LIME_GREEN { 90, 0, 255} -#define LED_GREEN {120, 0, 255} -#define LED_MINT_GREEN {150, 0, 255} -#define LED_CYAN {180, 0, 255} -#define LED_LIGHT_BLUE {210, 0, 255} -#define LED_BLUE {240, 0, 255} -#define LED_DARK_VIOLET {270, 0, 255} -#define LED_MAGENTA {300, 0, 255} -#define LED_DEEP_PINK {330, 0, 255} - -const hsvColor_t hsv_black = LED_BLACK; -const hsvColor_t hsv_white = LED_WHITE; -const hsvColor_t hsv_red = LED_RED; -const hsvColor_t hsv_orange = LED_ORANGE; -const hsvColor_t hsv_yellow = LED_YELLOW; -const hsvColor_t hsv_limeGreen = LED_LIME_GREEN; -const hsvColor_t hsv_green = LED_GREEN; -const hsvColor_t hsv_mintGreen = LED_MINT_GREEN; -const hsvColor_t hsv_cyan = LED_CYAN; -const hsvColor_t hsv_lightBlue = LED_LIGHT_BLUE; -const hsvColor_t hsv_blue = LED_BLUE; -const hsvColor_t hsv_darkViolet = LED_DARK_VIOLET; -const hsvColor_t hsv_magenta = LED_MAGENTA; -const hsvColor_t hsv_deepPink = LED_DEEP_PINK; - -#define LED_DIRECTION_COUNT 6 - -const hsvColor_t * const defaultColors[] = { - &hsv_black, - &hsv_white, - &hsv_red, - &hsv_orange, - &hsv_yellow, - &hsv_limeGreen, - &hsv_green, - &hsv_mintGreen, - &hsv_cyan, - &hsv_lightBlue, - &hsv_blue, - &hsv_darkViolet, - &hsv_magenta, - &hsv_deepPink -}; - typedef enum { COLOR_BLACK = 0, COLOR_WHITE, @@ -164,98 +114,134 @@ typedef enum { COLOR_DARK_VIOLET, COLOR_MAGENTA, COLOR_DEEP_PINK, -} colorIds; +} colorId_e; -typedef enum { - DIRECTION_NORTH = 0, - DIRECTION_EAST, - DIRECTION_SOUTH, - DIRECTION_WEST, - DIRECTION_UP, - DIRECTION_DOWN -} directionId_e; - -typedef struct modeColorIndexes_s { - uint8_t north; - uint8_t east; - uint8_t south; - uint8_t west; - uint8_t up; - uint8_t down; -} modeColorIndexes_t; - - -// Note, the color index used for the mode colors below refer to the default colors. -// if the colors are reconfigured the index is still valid but the displayed color might -// be different. -// See colors[] and defaultColors[] and applyDefaultColors[] - -static const modeColorIndexes_t orientationModeColors = { - COLOR_WHITE, - COLOR_DARK_VIOLET, - COLOR_RED, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE +const hsvColor_t hsv[] = { + // H S V + [COLOR_BLACK] = { 0, 0, 0}, + [COLOR_WHITE] = { 0, 255, 255}, + [COLOR_RED] = { 0, 0, 255}, + [COLOR_ORANGE] = { 30, 0, 255}, + [COLOR_YELLOW] = { 60, 0, 255}, + [COLOR_LIME_GREEN] = { 90, 0, 255}, + [COLOR_GREEN] = {120, 0, 255}, + [COLOR_MINT_GREEN] = {150, 0, 255}, + [COLOR_CYAN] = {180, 0, 255}, + [COLOR_LIGHT_BLUE] = {210, 0, 255}, + [COLOR_BLUE] = {240, 0, 255}, + [COLOR_DARK_VIOLET] = {270, 0, 255}, + [COLOR_MAGENTA] = {300, 0, 255}, + [COLOR_DEEP_PINK] = {330, 0, 255}, }; +// macro to save typing on default colors +#define HSV(color) (hsv[COLOR_ ## color]) -static const modeColorIndexes_t headfreeModeColors = { - COLOR_LIME_GREEN, - COLOR_DARK_VIOLET, - COLOR_ORANGE, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE +STATIC_UNIT_TESTED uint8_t ledGridWidth; +STATIC_UNIT_TESTED uint8_t ledGridHeight; +// grid offsets +STATIC_UNIT_TESTED uint8_t highestYValueForNorth; +STATIC_UNIT_TESTED uint8_t lowestYValueForSouth; +STATIC_UNIT_TESTED uint8_t highestXValueForWest; +STATIC_UNIT_TESTED uint8_t lowestXValueForEast; + +STATIC_UNIT_TESTED ledCounts_t ledCounts; + +// macro for initializer +#define LF(name) LED_FUNCTION_ ## name +#define LO(name) LED_FLAG_OVERLAY(LED_OVERLAY_ ## name) +#define LD(name) LED_FLAG_DIRECTION(LED_DIRECTION_ ## name) + +#ifdef USE_LED_RING_DEFAULT_CONFIG +static const ledConfig_t defaultLedStripConfig[] = { + DEFINE_LED( 2, 2, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 2, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 2, 0, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 0, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 0, 0, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 0, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 0, 2, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 2, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), }; +#else +static const ledConfig_t defaultLedStripConfig[] = { + DEFINE_LED(15, 15, 0, LD(SOUTH) | LD(EAST), LF(ARM_STATE), LO(INDICATOR), 0), -static const modeColorIndexes_t horizonModeColors = { - COLOR_BLUE, - COLOR_DARK_VIOLET, - COLOR_YELLOW, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE -}; + DEFINE_LED(15, 8, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED(15, 7, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0), -static const modeColorIndexes_t angleModeColors = { - COLOR_CYAN, - COLOR_DARK_VIOLET, - COLOR_YELLOW, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE -}; + DEFINE_LED(15, 0, 0, LD(NORTH) | LD(EAST), LF(ARM_STATE) , LO(INDICATOR), 0), + + DEFINE_LED( 8, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0), + DEFINE_LED( 7, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0), + + DEFINE_LED( 0, 0, 0, LD(NORTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0), + + DEFINE_LED( 0, 7, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 0, 8, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0), + + DEFINE_LED( 0, 15, 0, LD(SOUTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0), + + DEFINE_LED( 7, 15, 0, LD(SOUTH) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 8, 15, 0, LD(SOUTH) , LF(FLIGHT_MODE), LO(WARNING), 0), + + DEFINE_LED( 7, 7, 0, LD(UP) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 8, 7, 0, LD(UP) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 7, 8, 0, LD(DOWN) , LF(FLIGHT_MODE), LO(WARNING), 0), + DEFINE_LED( 8, 8, 0, LD(DOWN) , LF(FLIGHT_MODE), LO(WARNING), 0), + + DEFINE_LED( 8, 9, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 9, 10, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED(10, 11, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED(10, 12, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 9, 13, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 8, 14, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 7, 14, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 6, 13, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 5, 12, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 5, 11, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 6, 10, 3, 0, LF(THRUST_RING), 0, 0), + DEFINE_LED( 7, 9, 3, 0, LF(THRUST_RING), 0, 0), -#ifdef MAG -static const modeColorIndexes_t magModeColors = { - COLOR_MINT_GREEN, - COLOR_DARK_VIOLET, - COLOR_ORANGE, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE }; #endif -static const modeColorIndexes_t baroModeColors = { - COLOR_LIGHT_BLUE, - COLOR_DARK_VIOLET, - COLOR_RED, - COLOR_DEEP_PINK, - COLOR_BLUE, - COLOR_ORANGE +#undef LD +#undef LF +#undef LO + +static const modeColorIndexes_t defaultModeColors[] = { + // NORTH EAST SOUTH WEST UP DOWN + [LED_MODE_ORIENTATION] = {{ COLOR_WHITE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_HEADFREE] = {{ COLOR_LIME_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_HORIZON] = {{ COLOR_BLUE, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_ANGLE] = {{ COLOR_CYAN, COLOR_DARK_VIOLET, COLOR_YELLOW, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_MAG] = {{ COLOR_MINT_GREEN, COLOR_DARK_VIOLET, COLOR_ORANGE, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, + [LED_MODE_BARO] = {{ COLOR_LIGHT_BLUE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, +}; + +static const specialColorIndexes_t defaultSpecialColors[] = { + {{ [LED_SCOLOR_DISARMED] = COLOR_GREEN, + [LED_SCOLOR_ARMED] = COLOR_BLUE, + [LED_SCOLOR_ANIMATION] = COLOR_WHITE, + [LED_SCOLOR_BACKGROUND] = COLOR_BLACK, + [LED_SCOLOR_BLINKBACKGROUND] = COLOR_BLACK, + [LED_SCOLOR_GPSNOSATS] = COLOR_RED, + [LED_SCOLOR_GPSNOLOCK] = COLOR_ORANGE, + [LED_SCOLOR_GPSLOCKED] = COLOR_GREEN, + }} }; -uint8_t ledGridWidth; -uint8_t ledGridHeight; -uint8_t ledCount; -uint8_t ledsInRingCount; - -ledConfig_t *ledConfigs; -hsvColor_t *colors; +static int scaledThrottle; + + +/* #ifdef USE_LED_RING_DEFAULT_CONFIG const ledConfig_t defaultLedStripConfig[] = { { CALCULATE_LED_XY( 2, 2), 3, LED_FUNCTION_THRUST_RING}, @@ -273,16 +259,16 @@ const ledConfig_t defaultLedStripConfig[] = { }; #elif defined(USE_COLIBTI_RACE_LED_DEFAULT_CONFIG) const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, + { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, }; #else const ledConfig_t defaultLedStripConfig[] = { @@ -326,118 +312,58 @@ const ledConfig_t defaultLedStripConfig[] = { }; #endif +*/ -/* - * 6 coords @nn,nn - * 4 direction @## - * 6 modes @#### - * = 16 bytes per led - * 16 * 32 leds = 512 bytes storage needed worst case. - * = not efficient to store led configs as strings in flash. - * = becomes a problem to send all the data via cli due to serial/cli buffers - */ -typedef enum { - X_COORDINATE, - Y_COORDINATE, - DIRECTIONS, - FUNCTIONS, - RING_COLORS -} parseState_e; +static void updateLedRingCounts(void); -#define PARSE_STATE_COUNT 5 - -static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0' }; - -static const char directionCodes[] = { 'N', 'E', 'S', 'W', 'U', 'D' }; -#define DIRECTION_COUNT (sizeof(directionCodes) / sizeof(directionCodes[0])) -static const uint8_t directionMappings[DIRECTION_COUNT] = { - LED_DIRECTION_NORTH, - LED_DIRECTION_EAST, - LED_DIRECTION_SOUTH, - LED_DIRECTION_WEST, - LED_DIRECTION_UP, - LED_DIRECTION_DOWN -}; - -static const char functionCodes[] = { 'I', 'W', 'F', 'A', 'T', 'R', 'C' }; -#define FUNCTION_COUNT (sizeof(functionCodes) / sizeof(functionCodes[0])) -static const uint16_t functionMappings[FUNCTION_COUNT] = { - LED_FUNCTION_INDICATOR, - LED_FUNCTION_WARNING, - LED_FUNCTION_FLIGHT_MODE, - LED_FUNCTION_ARM_STATE, - LED_FUNCTION_THROTTLE, - LED_FUNCTION_THRUST_RING, - LED_FUNCTION_COLOR -}; - -// grid offsets -uint8_t highestYValueForNorth; -uint8_t lowestYValueForSouth; -uint8_t highestXValueForWest; -uint8_t lowestXValueForEast; - -void determineLedStripDimensions(void) +STATIC_UNIT_TESTED void determineLedStripDimensions(void) { - ledGridWidth = 0; - ledGridHeight = 0; + int maxX = 0; + int maxY = 0; - uint8_t ledIndex; - const ledConfig_t *ledConfig; + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - ledConfig = &ledConfigs[ledIndex]; - - if (GET_LED_X(ledConfig) >= ledGridWidth) { - ledGridWidth = GET_LED_X(ledConfig) + 1; - } - if (GET_LED_Y(ledConfig) >= ledGridHeight) { - ledGridHeight = GET_LED_Y(ledConfig) + 1; - } + maxX = MAX(ledGetX(ledConfig), maxX); + maxY = MAX(ledGetY(ledConfig), maxY); } + ledGridWidth = maxX + 1; + ledGridHeight = maxY + 1; } -void determineOrientationLimits(void) +STATIC_UNIT_TESTED void determineOrientationLimits(void) { - bool isOddHeight = (ledGridHeight & 1); - bool isOddWidth = (ledGridWidth & 1); - uint8_t heightModifier = isOddHeight ? 1 : 0; - uint8_t widthModifier = isOddWidth ? 1 : 0; - highestYValueForNorth = (ledGridHeight / 2) - 1; - lowestYValueForSouth = (ledGridHeight / 2) + heightModifier; + lowestYValueForSouth = ((ledGridHeight + 1) / 2); highestXValueForWest = (ledGridWidth / 2) - 1; - lowestXValueForEast = (ledGridWidth / 2) + widthModifier; + lowestXValueForEast = ((ledGridWidth + 1) / 2); } -void updateLedCount(void) +STATIC_UNIT_TESTED void updateLedCount(void) { - const ledConfig_t *ledConfig; - uint8_t ledIndex; - ledCount = 0; - ledsInRingCount = 0; + int count = 0, countRing = 0, countScanner= 0; - if( ledConfigs == 0 ){ - return; - } + for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - for (ledIndex = 0; ledIndex < MAX_LED_STRIP_LENGTH; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (ledConfig->flags == 0 && ledConfig->xy == 0) { + if (!(*ledConfig)) break; - } - ledCount++; + count++; - if ((ledConfig->flags & LED_FUNCTION_THRUST_RING)) { - ledsInRingCount++; - } + if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) + countRing++; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) + countScanner++; } + + ledCounts.count = count; + ledCounts.ring = countRing; + ledCounts.larson = countScanner; } void reevaluateLedConfig(void) @@ -445,499 +371,714 @@ void reevaluateLedConfig(void) updateLedCount(); determineLedStripDimensions(); determineOrientationLimits(); + updateLedRingCounts(); } -#define CHUNK_BUFFER_SIZE 10 - -#define NEXT_PARSE_STATE(parseState) ((parseState + 1) % PARSE_STATE_COUNT) - - -bool parseLedStripConfig(uint8_t ledIndex, const char *config) +// get specialColor by index +static hsvColor_t* getSC(ledSpecialColorIds_e index) { - char chunk[CHUNK_BUFFER_SIZE]; - uint8_t chunkIndex; - uint8_t val; + return &masterConfig.colors[masterConfig.specialColors.color[index]]; +} - uint8_t parseState = X_COORDINATE; - bool ok = true; +static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' }; +static const char baseFunctionCodes[LED_BASEFUNCTION_COUNT] = { 'C', 'F', 'A', 'L', 'S', 'G', 'R' }; +static const char overlayCodes[LED_OVERLAY_COUNT] = { 'T', 'O', 'B', 'N', 'I', 'W' }; - if (ledIndex >= MAX_LED_STRIP_LENGTH) { - return !ok; - } +#define CHUNK_BUFFER_SIZE 11 - ledConfig_t *ledConfig = &ledConfigs[ledIndex]; +bool parseLedStripConfig(int ledIndex, const char *config) +{ + if (ledIndex >= LED_MAX_STRIP_LENGTH) + return false; + + enum parseState_e { + X_COORDINATE, + Y_COORDINATE, + DIRECTIONS, + FUNCTIONS, + RING_COLORS, + PARSE_STATE_COUNT + }; + static const char chunkSeparators[PARSE_STATE_COUNT] = {',', ':', ':',':', '\0'}; + + ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; memset(ledConfig, 0, sizeof(ledConfig_t)); - while (ok) { + int x = 0, y = 0, color = 0; // initialize to prevent warnings + int baseFunction = 0; + int overlay_flags = 0; + int direction_flags = 0; - char chunkSeparator = chunkSeparators[parseState]; - - memset(&chunk, 0, sizeof(chunk)); - chunkIndex = 0; - - while (*config && chunkIndex < CHUNK_BUFFER_SIZE && *config != chunkSeparator) { - chunk[chunkIndex++] = *config++; + for (enum parseState_e parseState = 0; parseState < PARSE_STATE_COUNT; parseState++) { + char chunk[CHUNK_BUFFER_SIZE]; + { + char chunkSeparator = chunkSeparators[parseState]; + int chunkIndex = 0; + while (*config && *config != chunkSeparator && chunkIndex < (CHUNK_BUFFER_SIZE - 1)) { + chunk[chunkIndex++] = *config++; + } + chunk[chunkIndex++] = 0; // zero-terminate chunk + if (*config != chunkSeparator) { + return false; + } + config++; // skip separator } - - if (*config++ != chunkSeparator) { - ok = false; - break; - } - - switch((parseState_e)parseState) { + switch (parseState) { case X_COORDINATE: - val = atoi(chunk); - ledConfig->xy |= CALCULATE_LED_X(val); + x = atoi(chunk); break; case Y_COORDINATE: - val = atoi(chunk); - ledConfig->xy |= CALCULATE_LED_Y(val); + y = atoi(chunk); break; case DIRECTIONS: - for (chunkIndex = 0; chunk[chunkIndex] && chunkIndex < CHUNK_BUFFER_SIZE; chunkIndex++) { - for (uint8_t mappingIndex = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) { - if (directionCodes[mappingIndex] == chunk[chunkIndex]) { - ledConfig->flags |= directionMappings[mappingIndex]; + for (char *ch = chunk; *ch; ch++) { + for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) { + if (directionCodes[dir] == *ch) { + direction_flags |= LED_FLAG_DIRECTION(dir); break; } } } break; case FUNCTIONS: - for (chunkIndex = 0; chunk[chunkIndex] && chunkIndex < CHUNK_BUFFER_SIZE; chunkIndex++) { - for (uint8_t mappingIndex = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) { - if (functionCodes[mappingIndex] == chunk[chunkIndex]) { - ledConfig->flags |= functionMappings[mappingIndex]; + for (char *ch = chunk; *ch; ch++) { + for (ledBaseFunctionId_e fn = 0; fn < LED_BASEFUNCTION_COUNT; fn++) { + if (baseFunctionCodes[fn] == *ch) { + baseFunction = fn; + break; + } + } + + for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) { + if (overlayCodes[ol] == *ch) { + overlay_flags |= LED_FLAG_OVERLAY(ol); break; } } } break; case RING_COLORS: - if (atoi(chunk) < CONFIGURABLE_COLOR_COUNT) { - ledConfig->color = atoi(chunk); - } else { - ledConfig->color = 0; - } + color = atoi(chunk); + if (color >= LED_CONFIGURABLE_COLOR_COUNT) + color = 0; break; - default : - break; - } - - parseState++; - if (parseState >= PARSE_STATE_COUNT) { - break; + case PARSE_STATE_COUNT:; // prevent warning } } - if (!ok) { - memset(ledConfig, 0, sizeof(ledConfig_t)); - } + *ledConfig = DEFINE_LED(x, y, color, direction_flags, baseFunction, overlay_flags, 0); reevaluateLedConfig(); - return ok; + return true; } -void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize) +void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize) { - char functions[FUNCTION_COUNT]; - char directions[DIRECTION_COUNT]; - uint8_t index; - uint8_t mappingIndex; + char directions[LED_DIRECTION_COUNT + 1]; + char baseFunctionOverlays[LED_OVERLAY_COUNT + 2]; - ledConfig_t *ledConfig = &ledConfigs[ledIndex]; + ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; memset(ledConfigBuffer, 0, bufferSize); - memset(&functions, 0, sizeof(functions)); - memset(&directions, 0, sizeof(directions)); - for (mappingIndex = 0, index = 0; mappingIndex < FUNCTION_COUNT; mappingIndex++) { - if (ledConfig->flags & functionMappings[mappingIndex]) { - functions[index++] = functionCodes[mappingIndex]; + char *dptr = directions; + for (ledDirectionId_e dir = 0; dir < LED_DIRECTION_COUNT; dir++) { + if (ledGetDirectionBit(ledConfig, dir)) { + *dptr++ = directionCodes[dir]; } } + *dptr = 0; - for (mappingIndex = 0, index = 0; mappingIndex < DIRECTION_COUNT; mappingIndex++) { - if (ledConfig->flags & directionMappings[mappingIndex]) { - directions[index++] = directionCodes[mappingIndex]; + char *fptr = baseFunctionOverlays; + *fptr++ = baseFunctionCodes[ledGetFunction(ledConfig)]; + + for (ledOverlayId_e ol = 0; ol < LED_OVERLAY_COUNT; ol++) { + if (ledGetOverlayBit(ledConfig, ol)) { + *fptr++ = overlayCodes[ol]; } } + *fptr = 0; - sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", GET_LED_X(ledConfig), GET_LED_Y(ledConfig), directions, functions, ledConfig->color); + // TODO - check buffer length + sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig)); } -void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const modeColorIndexes_t *modeColors) -{ - // apply up/down colors regardless of quadrant. - if ((ledConfig->flags & LED_DIRECTION_UP)) { - setLedHsv(ledIndex, &colors[modeColors->up]); - } - - if ((ledConfig->flags & LED_DIRECTION_DOWN)) { - setLedHsv(ledIndex, &colors[modeColors->down]); - } - - // override with n/e/s/w colors to each n/s e/w half - bail at first match. - if ((ledConfig->flags & LED_DIRECTION_WEST) && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, &colors[modeColors->west]); - } - - if ((ledConfig->flags & LED_DIRECTION_EAST) && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, &colors[modeColors->east]); - } - - if ((ledConfig->flags & LED_DIRECTION_NORTH) && GET_LED_Y(ledConfig) <= highestYValueForNorth) { - setLedHsv(ledIndex, &colors[modeColors->north]); - } - - if ((ledConfig->flags & LED_DIRECTION_SOUTH) && GET_LED_Y(ledConfig) >= lowestYValueForSouth) { - setLedHsv(ledIndex, &colors[modeColors->south]); - } - -} typedef enum { - QUADRANT_NORTH_EAST = 1, - QUADRANT_SOUTH_EAST, - QUADRANT_SOUTH_WEST, - QUADRANT_NORTH_WEST + // the ordering is important, see below how NSEW is mapped to NE/SE/NW/SW + QUADRANT_NORTH = 1 << 0, + QUADRANT_SOUTH = 1 << 1, + QUADRANT_EAST = 1 << 2, + QUADRANT_WEST = 1 << 3, + QUADRANT_NORTH_EAST = 1 << 4, + QUADRANT_SOUTH_EAST = 1 << 5, + QUADRANT_NORTH_WEST = 1 << 6, + QUADRANT_SOUTH_WEST = 1 << 7, + QUADRANT_NONE = 1 << 8, + QUADRANT_NOTDIAG = 1 << 9, // not in NE/SE/NW/SW + // values for test + QUADRANT_ANY = QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST | QUADRANT_NONE, } quadrant_e; -void applyQuadrantColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const quadrant_e quadrant, const hsvColor_t *color) +static quadrant_e getLedQuadrant(const int ledIndex) { - switch (quadrant) { - case QUADRANT_NORTH_EAST: - if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, color); - } - return; + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - case QUADRANT_SOUTH_EAST: - if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) >= lowestXValueForEast) { - setLedHsv(ledIndex, color); - } - return; + int x = ledGetX(ledConfig); + int y = ledGetY(ledConfig); - case QUADRANT_SOUTH_WEST: - if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, color); - } - return; + int quad = 0; + if (y <= highestYValueForNorth) + quad |= QUADRANT_NORTH; + else if (y >= lowestYValueForSouth) + quad |= QUADRANT_SOUTH; + if (x >= lowestXValueForEast) + quad |= QUADRANT_EAST; + else if (x <= highestXValueForWest) + quad |= QUADRANT_WEST; + + if ((quad & (QUADRANT_NORTH | QUADRANT_SOUTH)) + && (quad & (QUADRANT_EAST | QUADRANT_WEST)) ) { // is led in one of NE/SE/NW/SW? + quad |= 1 << (4 + ((quad & QUADRANT_SOUTH) ? 1 : 0) + ((quad & QUADRANT_WEST) ? 2 : 0)); + } else { + quad |= QUADRANT_NOTDIAG; + } + + if ((quad & (QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST)) == 0) + quad |= QUADRANT_NONE; + + return quad; +} + +static const struct { + uint8_t dir; // ledDirectionId_e + uint16_t quadrantMask; // quadrant_e +} directionQuadrantMap[] = { + {LED_DIRECTION_SOUTH, QUADRANT_SOUTH}, + {LED_DIRECTION_NORTH, QUADRANT_NORTH}, + {LED_DIRECTION_EAST, QUADRANT_EAST}, + {LED_DIRECTION_WEST, QUADRANT_WEST}, + {LED_DIRECTION_DOWN, QUADRANT_ANY}, + {LED_DIRECTION_UP, QUADRANT_ANY}, +}; + +static hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIndexes_t *modeColors) +{ + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + + quadrant_e quad = getLedQuadrant(ledIndex); + for (unsigned i = 0; i < ARRAYLEN(directionQuadrantMap); i++) { + ledDirectionId_e dir = directionQuadrantMap[i].dir; + quadrant_e quadMask = directionQuadrantMap[i].quadrantMask; + + if (ledGetDirectionBit(ledConfig, dir) && (quad & quadMask)) + return &masterConfig.colors[modeColors->color[dir]]; + } + return NULL; +} + + +// map flight mode to led mode, in order of priority +// flightMode == 0 is always active +static const struct { + uint16_t flightMode; + uint8_t ledMode; +} flightModeToLed[] = { + {HEADFREE_MODE, LED_MODE_HEADFREE}, +#ifdef MAG + {MAG_MODE, LED_MODE_MAG}, +#endif +#ifdef BARO + {BARO_MODE, LED_MODE_BARO}, +#endif + {HORIZON_MODE, LED_MODE_HORIZON}, + {ANGLE_MODE, LED_MODE_ANGLE}, + {0, LED_MODE_ORIENTATION}, +}; + +static void applyLedFixedLayers() +{ + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + hsvColor_t color = *getSC(LED_SCOLOR_BACKGROUND); + + int fn = ledGetFunction(ledConfig); + int hOffset = HSV_HUE_MAX; + + switch (fn) { + case LED_FUNCTION_COLOR: + color = masterConfig.colors[ledGetColor(ledConfig)]; + break; + + case LED_FUNCTION_FLIGHT_MODE: + for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) + if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { + color = *getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]); + break; // stop on first match + } + break; + + case LED_FUNCTION_ARM_STATE: + color = ARMING_FLAG(ARMED) ? *getSC(LED_SCOLOR_ARMED) : *getSC(LED_SCOLOR_DISARMED); + break; + + case LED_FUNCTION_BATTERY: + color = HSV(RED); + hOffset += scaleRange(calculateBatteryCapacityRemainingPercentage(), 0, 100, -30, 120); + break; + + case LED_FUNCTION_RSSI: + color = HSV(RED); + hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120); + break; + + default: + break; + } + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_THROTTLE)) { + hOffset += ((scaledThrottle - 10) * 4) / 3; + } + + color.h = (color.h + hOffset) % (HSV_HUE_MAX + 1); + + setLedHsv(ledIndex, &color); - case QUADRANT_NORTH_WEST: - if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) <= highestXValueForWest) { - setLedHsv(ledIndex, color); - } - return; } } -void applyLedModeLayer(void) +static void applyLedHsv(uint32_t mask, const hsvColor_t *color) { - const ledConfig_t *ledConfig; - - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (!(ledConfig->flags & LED_FUNCTION_THRUST_RING)) { - if (ledConfig->flags & LED_FUNCTION_COLOR) { - setLedHsv(ledIndex, &colors[ledConfig->color]); - } else { - setLedHsv(ledIndex, &hsv_black); - } - } - - if (!(ledConfig->flags & LED_FUNCTION_FLIGHT_MODE)) { - if (ledConfig->flags & LED_FUNCTION_ARM_STATE) { - if (!ARMING_FLAG(ARMED)) { - setLedHsv(ledIndex, &hsv_green); - } else { - setLedHsv(ledIndex, &hsv_blue); - } - } - continue; - } - - applyDirectionalModeColor(ledIndex, ledConfig, &orientationModeColors); - - if (FLIGHT_MODE(HEADFREE_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &headfreeModeColors); -#ifdef MAG - } else if (FLIGHT_MODE(MAG_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &magModeColors); -#endif -#ifdef BARO - } else if (FLIGHT_MODE(BARO_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &baroModeColors); -#endif - } else if (FLIGHT_MODE(HORIZON_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &horizonModeColors); - } else if (FLIGHT_MODE(ANGLE_MODE)) { - applyDirectionalModeColor(ledIndex, ledConfig, &angleModeColors); - } + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if ((*ledConfig & mask) == mask) + setLedHsv(ledIndex, color); } } typedef enum { - WARNING_FLAG_NONE = 0, - WARNING_FLAG_LOW_BATTERY = (1 << 0), - WARNING_FLAG_FAILSAFE = (1 << 1), - WARNING_FLAG_ARMING_DISABLED = (1 << 2) + WARNING_ARMING_DISABLED, + WARNING_LOW_BATTERY, + WARNING_FAILSAFE, } warningFlags_e; -static uint8_t warningFlags = WARNING_FLAG_NONE; -void applyLedWarningLayer(uint8_t updateNow) +static void applyLedWarningLayer(bool updateNow, uint32_t *timer) { - const ledConfig_t *ledConfig; - uint8_t ledIndex; static uint8_t warningFlashCounter = 0; + static uint8_t warningFlags = 0; // non-zero during blinks - if (updateNow && warningFlashCounter == 0) { - warningFlags = WARNING_FLAG_NONE; - if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) { - warningFlags |= WARNING_FLAG_LOW_BATTERY; - } - if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) { - warningFlags |= WARNING_FLAG_FAILSAFE; - } - if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) { - warningFlags |= WARNING_FLAG_ARMING_DISABLED; - } - } - - if (warningFlags || warningFlashCounter > 0) { - const hsvColor_t *warningColor = &hsv_black; - - if ((warningFlashCounter & 1) == 0) { - if (warningFlashCounter < 4 && (warningFlags & WARNING_FLAG_ARMING_DISABLED)) { - warningColor = &hsv_green; - } - if (warningFlashCounter >= 4 && warningFlashCounter < 12 && (warningFlags & WARNING_FLAG_LOW_BATTERY)) { - warningColor = &hsv_red; - } - if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) { - warningColor = &hsv_yellow; - } - } else { - if (warningFlashCounter >= 12 && warningFlashCounter < 16 && (warningFlags & WARNING_FLAG_FAILSAFE)) { - warningColor = &hsv_blue; - } - } - - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (!(ledConfig->flags & LED_FUNCTION_WARNING)) { - continue; - } - setLedHsv(ledIndex, warningColor); - } - } - - if (updateNow && (warningFlags || warningFlashCounter)) { + if (updateNow) { + // keep counter running, so it stays in sync with blink warningFlashCounter++; - if (warningFlashCounter == 20) { - warningFlashCounter = 0; + warningFlashCounter &= 0xF; + + if (warningFlashCounter == 0) { // update when old flags was processed + warningFlags = 0; + if (feature(FEATURE_VBAT) && getBatteryState() != BATTERY_OK) + warningFlags |= 1 << WARNING_LOW_BATTERY; + if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) + warningFlags |= 1 << WARNING_FAILSAFE; + if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) + warningFlags |= 1 << WARNING_ARMING_DISABLED; } + *timer += LED_STRIP_HZ(10); + } + + if (warningFlags) { + const hsvColor_t *warningColor = NULL; + + bool colorOn = (warningFlashCounter % 2) == 0; // w_w_ + warningFlags_e warningId = warningFlashCounter / 4; + if (warningFlags & (1 << warningId)) { + switch (warningId) { + case WARNING_ARMING_DISABLED: + warningColor = colorOn ? &HSV(GREEN) : NULL; + break; + case WARNING_LOW_BATTERY: + warningColor = colorOn ? &HSV(RED) : NULL; + break; + case WARNING_FAILSAFE: + warningColor = colorOn ? &HSV(YELLOW) : &HSV(BLUE); + break; + default:; + } + } + if (warningColor) + applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor); } } +static void applyLedBatteryLayer(bool updateNow, uint32_t *timer) +{ + static bool flash = false; + + int state; + int timeOffset = 1; + + if (updateNow) { + state = getBatteryState(); + + switch (state) { + case BATTERY_OK: + flash = false; + timeOffset = 1; + break; + case BATTERY_WARNING: + timeOffset = 2; + break; + default: + timeOffset = 8; + break; + } + flash = !flash; + } + + *timer += LED_STRIP_HZ(timeOffset); + + if (!flash) { + hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); + applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_BATTERY), bgc); + } +} + +static void applyLedRssiLayer(bool updateNow, uint32_t *timer) +{ + static bool flash = false; + + int state; + int timeOffset = 0; + + if (updateNow) { + state = (rssi * 100) / 1023; + + if (state > 50) { + flash = false; + timeOffset = 1; + } else if (state > 20) { + timeOffset = 2; + } else { + timeOffset = 8; + } + flash = !flash; + } + + + *timer += LED_STRIP_HZ(timeOffset); + + if (!flash) { + hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); + applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_RSSI), bgc); + } +} + +#ifdef GPS +static void applyLedGpsLayer(bool updateNow, uint32_t *timer) +{ + static uint8_t gpsFlashCounter = 0; + static uint8_t gpsPauseCounter = 0; + const uint8_t blinkPauseLength = 4; + + if (updateNow) { + if (gpsPauseCounter > 0) { + gpsPauseCounter--; + } else if (gpsFlashCounter >= GPS_numSat) { + gpsFlashCounter = 0; + gpsPauseCounter = blinkPauseLength; + } else { + gpsFlashCounter++; + gpsPauseCounter = 1; + } + *timer += LED_STRIP_HZ(2.5); + } + + const hsvColor_t *gpsColor; + + if (GPS_numSat == 0 || !sensors(SENSOR_GPS)) { + gpsColor = getSC(LED_SCOLOR_GPSNOSATS); + } else { + bool colorOn = gpsPauseCounter == 0; // each interval starts with pause + if (STATE(GPS_FIX)) { + gpsColor = colorOn ? getSC(LED_SCOLOR_GPSLOCKED) : getSC(LED_SCOLOR_BACKGROUND); + } else { + gpsColor = colorOn ? getSC(LED_SCOLOR_GPSNOLOCK) : getSC(LED_SCOLOR_GPSNOSATS); + } + } + + applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_GPS), gpsColor); +} + +#endif + + #define INDICATOR_DEADBAND 25 -void applyLedIndicatorLayer(uint8_t indicatorFlashState) +static void applyLedIndicatorLayer(bool updateNow, uint32_t *timer) { - const ledConfig_t *ledConfig; - static const hsvColor_t *flashColor; + static bool flash = 0; - if (!rxIsReceivingSignal()) { + if (updateNow) { + if (rxIsReceivingSignal()) { + // calculate update frequency + int scale = MAX(ABS(rcCommand[ROLL]), ABS(rcCommand[PITCH])); // 0 - 500 + scale += (50 - INDICATOR_DEADBAND); // start increasing frequency right after deadband + *timer += LED_STRIP_HZ(5) * 50 / MAX(50, scale); // 5 - 50Hz update, 2.5 - 25Hz blink + + flash = !flash; + } else { + *timer += LED_STRIP_HZ(5); // try again soon + } + } + + if (!flash) return; + + const hsvColor_t *flashColor = &HSV(ORANGE); // TODO - use user color? + + quadrant_e quadrants = 0; + if (rcCommand[ROLL] > INDICATOR_DEADBAND) { + quadrants |= QUADRANT_NORTH_EAST | QUADRANT_SOUTH_EAST; + } else if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { + quadrants |= QUADRANT_NORTH_WEST | QUADRANT_SOUTH_WEST; + } + if (rcCommand[PITCH] > INDICATOR_DEADBAND) { + quadrants |= QUADRANT_NORTH_EAST | QUADRANT_NORTH_WEST; + } else if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { + quadrants |= QUADRANT_SOUTH_EAST | QUADRANT_SOUTH_WEST; } - if (indicatorFlashState == 0) { - flashColor = &hsv_orange; - } else { - flashColor = &hsv_black; - } - - - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if (!(ledConfig->flags & LED_FUNCTION_INDICATOR)) { - continue; + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_INDICATOR)) { + if (getLedQuadrant(ledIndex) & quadrants) + setLedHsv(ledIndex, flashColor); } - - if (rcCommand[ROLL] > INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); - } - - if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); - } - - if (rcCommand[PITCH] > INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); - } - - if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); - applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); - } - } -} - -void applyLedThrottleLayer() -{ - const ledConfig_t *ledConfig; - hsvColor_t color; - - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - ledConfig = &ledConfigs[ledIndex]; - if (!(ledConfig->flags & LED_FUNCTION_THROTTLE)) { - continue; - } - - getLedHsv(ledIndex, &color); - - int scaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, -60, +60); - scaled += HSV_HUE_MAX; - color.h = (color.h + scaled) % HSV_HUE_MAX; - setLedHsv(ledIndex, &color); } } #define ROTATION_SEQUENCE_LED_COUNT 6 // 2 on, 4 off -#define ROTATION_SEQUENCE_LED_WIDTH 2 +#define ROTATION_SEQUENCE_LED_WIDTH 2 // 2 on -#define RING_PATTERN_NOT_CALCULATED 255 - -void applyLedThrustRingLayer(void) +static void updateLedRingCounts(void) { - const ledConfig_t *ledConfig; - hsvColor_t ringColor; - uint8_t ledIndex; - - // initialised to special value instead of using more memory for a flag. - static uint8_t rotationSeqLedCount = RING_PATTERN_NOT_CALCULATED; - static uint8_t rotationPhase = ROTATION_SEQUENCE_LED_COUNT; - bool nextLedOn = false; - - uint8_t ledRingIndex = 0; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { - - ledConfig = &ledConfigs[ledIndex]; - - if ((ledConfig->flags & LED_FUNCTION_THRUST_RING) == 0) { - continue; + int seqLen; + // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds + if ((ledCounts.ring % ROTATION_SEQUENCE_LED_COUNT) == 0) { + seqLen = ROTATION_SEQUENCE_LED_COUNT; + } else { + seqLen = ledCounts.ring; + // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds + // TODO - improve partitioning (15 leds -> 3x5) + while ((seqLen > ROTATION_SEQUENCE_LED_COUNT) && ((seqLen % 2) == 0)) { + seqLen /= 2; } + } + ledCounts.ringSeqLen = seqLen; +} - bool applyColor = false; - if (ARMING_FLAG(ARMED)) { - if ((ledRingIndex + rotationPhase) % rotationSeqLedCount < ROTATION_SEQUENCE_LED_WIDTH) { - applyColor = true; - } - } else { - if (nextLedOn == false) { - applyColor = true; - } - nextLedOn = !nextLedOn; - } +static void applyLedThrustRingLayer(bool updateNow, uint32_t *timer) +{ + static uint8_t rotationPhase; + int ledRingIndex = 0; - if (applyColor) { - ringColor = colors[ledConfig->color]; - } else { - ringColor = hsv_black; - } + if (updateNow) { + rotationPhase = rotationPhase > 0 ? rotationPhase - 1 : ledCounts.ringSeqLen - 1; - setLedHsv(ledIndex, &ringColor); - - ledRingIndex++; + int scale = scaledThrottle; // ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + *timer += LED_STRIP_HZ(5) * 10 / scale; // 5 - 50Hz update rate } - uint8_t ledRingLedCount = ledRingIndex; - if (rotationSeqLedCount == RING_PATTERN_NOT_CALCULATED) { - // update ring pattern according to total number of ring leds found + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + if (ledGetFunction(ledConfig) == LED_FUNCTION_THRUST_RING) { - rotationSeqLedCount = ledRingLedCount; - - // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds - if ((ledRingLedCount % ROTATION_SEQUENCE_LED_COUNT) == 0) { - rotationSeqLedCount = ROTATION_SEQUENCE_LED_COUNT; - } else { - // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds - while ((rotationSeqLedCount > ROTATION_SEQUENCE_LED_COUNT) && ((rotationSeqLedCount % 2) == 0)) { - rotationSeqLedCount >>= 1; + bool applyColor; + if (ARMING_FLAG(ARMED)) { + applyColor = (ledRingIndex + rotationPhase) % ledCounts.ringSeqLen < ROTATION_SEQUENCE_LED_WIDTH; + } else { + applyColor = !(ledRingIndex % 2); // alternating pattern } + + if (applyColor) { + const hsvColor_t *ringColor = &masterConfig.colors[ledGetColor(ledConfig)]; + setLedHsv(ledIndex, ringColor); + } + + ledRingIndex++; } - - // trigger start over - rotationPhase = 1; - } - - rotationPhase--; - if (rotationPhase == 0) { - rotationPhase = rotationSeqLedCount; } } +typedef struct larsonParameters_s { + uint8_t currentBrightness; + int8_t currentIndex; + int8_t direction; +} larsonParameters_t; + +static int brightnessForLarsonIndex(larsonParameters_t *larsonParameters, uint8_t larsonIndex) +{ + int offset = larsonIndex - larsonParameters->currentIndex; + static const int larsonLowValue = 8; + + if (ABS(offset) > 1) + return (larsonLowValue); + + if (offset == 0) + return (larsonParameters->currentBrightness); + + if (larsonParameters->direction == offset) { + return (larsonParameters->currentBrightness - 127); + } + + return (255 - larsonParameters->currentBrightness); + +} + +static void larsonScannerNextStep(larsonParameters_t *larsonParameters, int delta) +{ + if (larsonParameters->currentBrightness > (255 - delta)) { + larsonParameters->currentBrightness = 127; + if (larsonParameters->currentIndex >= ledCounts.larson || larsonParameters->currentIndex < 0) { + larsonParameters->direction = -larsonParameters->direction; + } + larsonParameters->currentIndex += larsonParameters->direction; + } else { + larsonParameters->currentBrightness += delta; + } +} + +static void applyLarsonScannerLayer(bool updateNow, uint32_t *timer) +{ + static larsonParameters_t larsonParameters = { 0, 0, 1 }; + + if (updateNow) { + larsonScannerNextStep(&larsonParameters, 15); + *timer += LED_STRIP_HZ(60); + } + + int scannerLedIndex = 0; + for (unsigned i = 0; i < ledCounts.count; i++) { + + const ledConfig_t *ledConfig = &ledConfigs[i]; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_LARSON_SCANNER)) { + hsvColor_t ledColor; + getLedHsv(i, &ledColor); + ledColor.v = brightnessForLarsonIndex(&larsonParameters, scannerLedIndex); + setLedHsv(i, &ledColor); + scannerLedIndex++; + } + } +} + +// blink twice, then wait ; either always or just when landing +static void applyLedBlinkLayer(bool updateNow, uint32_t *timer) +{ + const uint16_t blinkPattern = 0x8005; // 0b1000000000000101; + static uint16_t blinkMask; + + if (updateNow) { + blinkMask = blinkMask >> 1; + if (blinkMask <= 1) + blinkMask = blinkPattern; + + *timer += LED_STRIP_HZ(10); + } + + bool ledOn = (blinkMask & 1); // b_b_____... + if (!ledOn) { + for (int i = 0; i < ledCounts.count; ++i) { + const ledConfig_t *ledConfig = &ledConfigs[i]; + + if (ledGetOverlayBit(ledConfig, LED_OVERLAY_BLINK) || + (ledGetOverlayBit(ledConfig, LED_OVERLAY_LANDING_FLASH) && scaledThrottle < 55 && scaledThrottle > 10)) { + setLedHsv(i, getSC(LED_SCOLOR_BLINKBACKGROUND)); + } + } + } +} + + #ifdef USE_LED_ANIMATION -static uint8_t previousRow; -static uint8_t currentRow; -static uint8_t nextRow; -void updateLedAnimationState(void) +static void applyLedAnimationLayer(bool updateNow, uint32_t *timer) { static uint8_t frameCounter = 0; - - uint8_t animationFrames = ledGridHeight; - - previousRow = (frameCounter + animationFrames - 1) % animationFrames; - currentRow = frameCounter; - nextRow = (frameCounter + 1) % animationFrames; - - frameCounter = (frameCounter + 1) % animationFrames; -} - -static void applyLedAnimationLayer(void) -{ - const ledConfig_t *ledConfig; - - if (ARMING_FLAG(ARMED)) { - return; + const int animationFrames = ledGridHeight; + if(updateNow) { + frameCounter = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; + *timer += LED_STRIP_HZ(20); } - uint8_t ledIndex; - for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { + if (ARMING_FLAG(ARMED)) + return; - ledConfig = &ledConfigs[ledIndex]; + int previousRow = frameCounter > 0 ? frameCounter - 1 : animationFrames - 1; + int currentRow = frameCounter; + int nextRow = (frameCounter + 1 < animationFrames) ? frameCounter + 1 : 0; - if (GET_LED_Y(ledConfig) == previousRow) { - setLedHsv(ledIndex, &hsv_white); + for (int ledIndex = 0; ledIndex < ledCounts.count; ledIndex++) { + const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; + + if (ledGetY(ledConfig) == previousRow) { + setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION)); scaleLedValue(ledIndex, 50); - - } else if (GET_LED_Y(ledConfig) == currentRow) { - setLedHsv(ledIndex, &hsv_white); - } else if (GET_LED_Y(ledConfig) == nextRow) { + } else if (ledGetY(ledConfig) == currentRow) { + setLedHsv(ledIndex, getSC(LED_SCOLOR_ANIMATION)); + } else if (ledGetY(ledConfig) == nextRow) { scaleLedValue(ledIndex, 50); } } } #endif +typedef enum { + timBlink, + timLarson, + timBattery, + timRssi, +#ifdef GPS + timGps, +#endif + timWarning, + timIndicator, +#ifdef USE_LED_ANIMATION + timAnimation, +#endif + timRing, + timTimerCount +} timId_e; + +static uint32_t timerVal[timTimerCount]; + + +// function to apply layer. +// function must replan self using timer pointer +// when updateNow is true (timer triggered), state must be updated first, +// before calculating led state. Otherwise update started by different trigger +// may modify LED state. +typedef void applyLayerFn_timed(bool updateNow, uint32_t *timer); + + +static applyLayerFn_timed* layerTable[] = { + [timBlink] = &applyLedBlinkLayer, + [timLarson] = &applyLarsonScannerLayer, + [timBattery] = &applyLedBatteryLayer, + [timRssi] = &applyLedRssiLayer, +#ifdef GPS + [timGps] = &applyLedGpsLayer, +#endif + [timWarning] = &applyLedWarningLayer, + [timIndicator] = &applyLedIndicatorLayer, +#ifdef USE_LED_ANIMATION + [timAnimation] = &applyLedAnimationLayer, +#endif + [timRing] = &applyLedThrustRingLayer +}; + void updateLedStrip(void) { - if (!(ledStripInitialised && isWS2811LedStripReady())) { return; } @@ -947,161 +1088,173 @@ void updateLedStrip(void) ledStripDisable(); ledStripEnabled = false; } - } else { - ledStripEnabled = true; - } - - if (!ledStripEnabled){ return; } - + ledStripEnabled = true; uint32_t now = micros(); - bool indicatorFlashNow = (int32_t)(now - nextIndicatorFlashAt) >= 0L; - bool warningFlashNow = (int32_t)(now - nextWarningFlashAt) >= 0L; - bool rotationUpdateNow = (int32_t)(now - nextRotationUpdateAt) >= 0L; -#ifdef USE_LED_ANIMATION - bool animationUpdateNow = (int32_t)(now - nextAnimationUpdateAt) >= 0L; -#endif - if (!( - indicatorFlashNow || - rotationUpdateNow || - warningFlashNow -#ifdef USE_LED_ANIMATION - || animationUpdateNow -#endif - )) { - return; - } - - static uint8_t indicatorFlashState = 0; - - // LAYER 1 - applyLedModeLayer(); - applyLedThrottleLayer(); - - // LAYER 2 - - if (warningFlashNow) { - nextWarningFlashAt = now + LED_STRIP_10HZ; - } - applyLedWarningLayer(warningFlashNow); - - // LAYER 3 - - if (indicatorFlashNow) { - - uint8_t rollScale = ABS(rcCommand[ROLL]) / 50; - uint8_t pitchScale = ABS(rcCommand[PITCH]) / 50; - uint8_t scale = MAX(rollScale, pitchScale); - nextIndicatorFlashAt = now + (LED_STRIP_5HZ / MAX(1, scale)); - - if (indicatorFlashState == 0) { - indicatorFlashState = 1; - } else { - indicatorFlashState = 0; + // test all led timers, setting corresponding bits + uint32_t timActive = 0; + for (timId_e timId = 0; timId < timTimerCount; timId++) { + // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value. + // max delay is limited to 5s + int32_t delta = cmp32(now, timerVal[timId]); + if (delta < 0 && delta > -LED_STRIP_MS(5000)) + continue; // not ready yet + timActive |= 1 << timId; + if (delta >= LED_STRIP_MS(100) || delta < 0) { + timerVal[timId] = now; } } - applyLedIndicatorLayer(indicatorFlashState); + if (!timActive) + return; // no change this update, keep old state -#ifdef USE_LED_ANIMATION - if (animationUpdateNow) { - nextAnimationUpdateAt = now + LED_STRIP_20HZ; - updateLedAnimationState(); + // apply all layers; triggered timed functions has to update timers + + scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10; + + applyLedFixedLayers(); + + for (timId_e timId = 0; timId < ARRAYLEN(layerTable); timId++) { + uint32_t *timer = &timerVal[timId]; + bool updateNow = timActive & (1 << timId); + (*layerTable[timId])(updateNow, timer); } - applyLedAnimationLayer(); -#endif - - if (rotationUpdateNow) { - - applyLedThrustRingLayer(); - - uint8_t animationSpeedScale = 1; - - if (ARMING_FLAG(ARMED)) { - animationSpeedScale = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 1, 10); - } - - nextRotationUpdateAt = now + LED_STRIP_5HZ/animationSpeedScale; - } - ws2811UpdateStrip(); } -bool parseColor(uint8_t index, const char *colorConfig) +bool parseColor(int index, const char *colorConfig) { const char *remainingCharacters = colorConfig; - hsvColor_t *color = &colors[index]; + hsvColor_t *color = &masterConfig.colors[index]; - bool ok = true; - - uint8_t componentIndex; - for (componentIndex = 0; ok && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) { - uint16_t val = atoi(remainingCharacters); + bool result = true; + static const uint16_t hsv_limit[HSV_COLOR_COMPONENT_COUNT] = { + [HSV_HUE] = HSV_HUE_MAX, + [HSV_SATURATION] = HSV_SATURATION_MAX, + [HSV_VALUE] = HSV_VALUE_MAX, + }; + for (int componentIndex = 0; result && componentIndex < HSV_COLOR_COMPONENT_COUNT; componentIndex++) { + int val = atoi(remainingCharacters); + if(val > hsv_limit[componentIndex]) { + result = false; + break; + } switch (componentIndex) { case HSV_HUE: - if (val > HSV_HUE_MAX) { - ok = false; - continue; - - } - colors[index].h = val; + color->h = val; break; case HSV_SATURATION: - if (val > HSV_SATURATION_MAX) { - ok = false; - continue; - } - colors[index].s = (uint8_t)val; + color->s = val; break; case HSV_VALUE: - if (val > HSV_VALUE_MAX) { - ok = false; - continue; - } - colors[index].v = (uint8_t)val; + color->v = val; break; } - remainingCharacters = strstr(remainingCharacters, ","); + remainingCharacters = strchr(remainingCharacters, ','); if (remainingCharacters) { - remainingCharacters++; + remainingCharacters++; // skip separator } else { - if (componentIndex < 2) { - ok = false; + if (componentIndex < HSV_COLOR_COMPONENT_COUNT - 1) { + result = false; } } } - if (!ok) { - memset(color, 0, sizeof(hsvColor_t)); + if (!result) { + memset(color, 0, sizeof(*color)); } - return ok; + return result; } -void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount) +/* + * Redefine a color in a mode. + * */ +bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) { - memset(colors, 0, colorCount * sizeof(hsvColor_t)); - for (uint8_t colorIndex = 0; colorIndex < colorCount && colorIndex < (sizeof(defaultColors) / sizeof(defaultColors[0])); colorIndex++) { - *colors++ = *defaultColors[colorIndex]; + // check color + if (colorIndex < 0 || colorIndex >= LED_CONFIGURABLE_COLOR_COUNT) + return false; + if (modeIndex < LED_MODE_COUNT) { // modeIndex_e is unsigned, so one-sided test is enough + if(modeColorIndex < 0 || modeColorIndex >= LED_DIRECTION_COUNT) + return false; + masterConfig.modeColors[modeIndex].color[modeColorIndex] = colorIndex; + } else if (modeIndex == LED_SPECIAL) { + if (modeColorIndex < 0 || modeColorIndex >= LED_SPECIAL_COLOR_COUNT) + return false; + masterConfig.specialColors.color[modeColorIndex] = colorIndex; + } else { + return false; + } + return true; +} + +/* +void pgResetFn_ledConfigs(ledConfig_t *instance) +{ + memcpy_fn(instance, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); +} + +void pgResetFn_colors(hsvColor_t *instance) +{ + // copy hsv colors as default + BUILD_BUG_ON(ARRAYLEN(*colors_arr()) < ARRAYLEN(hsv)); + + for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { + *instance++ = hsv[colorIndex]; } } +void pgResetFn_modeColors(modeColorIndexes_t *instance) +{ + memcpy_fn(instance, &defaultModeColors, sizeof(defaultModeColors)); +} + +void pgResetFn_specialColors(specialColorIndexes_t *instance) +{ + memcpy_fn(instance, &defaultSpecialColors, sizeof(defaultSpecialColors)); +} +*/ + void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) { - memset(ledConfigs, 0, MAX_LED_STRIP_LENGTH * sizeof(ledConfig_t)); + memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); reevaluateLedConfig(); } -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse) +void applyDefaultColors(hsvColor_t *colors) +{ + // copy hsv colors as default + memset(colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t)); + for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) { + *colors++ = hsv[colorIndex]; + } +} + +void applyDefaultModeColors(modeColorIndexes_t *modeColors) +{ + memcpy_fn(modeColors, &defaultModeColors, sizeof(defaultModeColors)); +} + +void applyDefaultSpecialColors(specialColorIndexes_t *specialColors) +{ + memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors)); +} + + + +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse) { ledConfigs = ledConfigsToUse; colors = colorsToUse; + modeColors = modeColorsToUse; + specialColors = *specialColorsToUse; ledStripInitialised = false; } @@ -1115,8 +1268,8 @@ void ledStripEnable(void) static void ledStripDisable(void) { - setStripColor(&hsv_black); - - ws2811UpdateStrip(); + setStripColor(&HSV(BLACK)); + + ws2811UpdateStrip(); } #endif diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 18915229b7..39b3cad6d2 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -17,81 +17,163 @@ #pragma once -#define MAX_LED_STRIP_LENGTH 32 -#define CONFIGURABLE_COLOR_COUNT 16 +#define LED_MAX_STRIP_LENGTH 32 +#define LED_CONFIGURABLE_COLOR_COUNT 16 +#define LED_MODE_COUNT 6 +#define LED_DIRECTION_COUNT 6 +#define LED_BASEFUNCTION_COUNT 7 +#define LED_OVERLAY_COUNT 6 +#define LED_SPECIAL_COLOR_COUNT 11 + +#define LED_POS_OFFSET 0 +#define LED_FUNCTION_OFFSET 8 +#define LED_OVERLAY_OFFSET 12 +#define LED_COLOR_OFFSET 18 +#define LED_DIRECTION_OFFSET 22 +#define LED_PARAMS_OFFSET 28 + +#define LED_POS_BITCNT 8 +#define LED_FUNCTION_BITCNT 4 +#define LED_OVERLAY_BITCNT 6 +#define LED_COLOR_BITCNT 4 +#define LED_DIRECTION_BITCNT 6 +#define LED_PARAMS_BITCNT 4 + +#define LED_FLAG_OVERLAY_MASK ((1 << LED_OVERLAY_BITCNT) - 1) +#define LED_FLAG_DIRECTION_MASK ((1 << LED_DIRECTION_BITCNT) - 1) + +#define LED_MOV_POS(pos) ((pos) << LED_POS_OFFSET) +#define LED_MOV_FUNCTION(func) ((func) << LED_FUNCTION_OFFSET) +#define LED_MOV_OVERLAY(overlay) ((overlay) << LED_OVERLAY_OFFSET) +#define LED_MOV_COLOR(colorId) ((colorId) << LED_COLOR_OFFSET) +#define LED_MOV_DIRECTION(direction) ((direction) << LED_DIRECTION_OFFSET) +#define LED_MOV_PARAMS(param) ((param) << LED_PARAMS_OFFSET) + +#define LED_BIT_MASK(len) ((1 << (len)) - 1) + +#define LED_POS_MASK LED_MOV_POS(((1 << LED_POS_BITCNT) - 1)) +#define LED_FUNCTION_MASK LED_MOV_FUNCTION(((1 << LED_FUNCTION_BITCNT) - 1)) +#define LED_OVERLAY_MASK LED_MOV_OVERLAY(LED_FLAG_OVERLAY_MASK) +#define LED_COLOR_MASK LED_MOV_COLOR(((1 << LED_COLOR_BITCNT) - 1)) +#define LED_DIRECTION_MASK LED_MOV_DIRECTION(LED_FLAG_DIRECTION_MASK) +#define LED_PARAMS_MASK LED_MOV_PARAMS(((1 << LED_PARAMS_BITCNT) - 1)) + +#define LED_FLAG_OVERLAY(id) (1 << (id)) +#define LED_FLAG_DIRECTION(id) (1 << (id)) #define LED_X_BIT_OFFSET 4 #define LED_Y_BIT_OFFSET 0 - -#define LED_XY_MASK (0x0F) - -#define GET_LED_X(ledConfig) ((ledConfig->xy >> LED_X_BIT_OFFSET) & LED_XY_MASK) -#define GET_LED_Y(ledConfig) ((ledConfig->xy >> LED_Y_BIT_OFFSET) & LED_XY_MASK) - -#define CALCULATE_LED_X(x) ((x & LED_XY_MASK) << LED_X_BIT_OFFSET) -#define CALCULATE_LED_Y(y) ((y & LED_XY_MASK) << LED_Y_BIT_OFFSET) - - -#define CALCULATE_LED_XY(x,y) (CALCULATE_LED_X(x) | CALCULATE_LED_Y(y)) +#define LED_XY_MASK 0x0F +#define CALCULATE_LED_XY(x, y) ((((x) & LED_XY_MASK) << LED_X_BIT_OFFSET) | (((y) & LED_XY_MASK) << LED_Y_BIT_OFFSET)) typedef enum { - LED_DISABLED = 0, - LED_DIRECTION_NORTH = (1 << 0), - LED_DIRECTION_EAST = (1 << 1), - LED_DIRECTION_SOUTH = (1 << 2), - LED_DIRECTION_WEST = (1 << 3), - LED_DIRECTION_UP = (1 << 4), - LED_DIRECTION_DOWN = (1 << 5), - LED_FUNCTION_INDICATOR = (1 << 6), - LED_FUNCTION_WARNING = (1 << 7), - LED_FUNCTION_FLIGHT_MODE = (1 << 8), - LED_FUNCTION_ARM_STATE = (1 << 9), - LED_FUNCTION_THROTTLE = (1 << 10), - LED_FUNCTION_THRUST_RING = (1 << 11), - LED_FUNCTION_COLOR = (1 << 12), -} ledFlag_e; + LED_MODE_ORIENTATION = 0, + LED_MODE_HEADFREE, + LED_MODE_HORIZON, + LED_MODE_ANGLE, + LED_MODE_MAG, + LED_MODE_BARO, + LED_SPECIAL +} ledModeIndex_e; -#define LED_DIRECTION_BIT_OFFSET 0 -#define LED_DIRECTION_MASK ( \ - LED_DIRECTION_NORTH | \ - LED_DIRECTION_EAST | \ - LED_DIRECTION_SOUTH | \ - LED_DIRECTION_WEST | \ - LED_DIRECTION_UP | \ - LED_DIRECTION_DOWN \ -) -#define LED_FUNCTION_BIT_OFFSET 6 -#define LED_FUNCTION_MASK ( \ - LED_FUNCTION_INDICATOR | \ - LED_FUNCTION_WARNING | \ - LED_FUNCTION_FLIGHT_MODE | \ - LED_FUNCTION_ARM_STATE | \ - LED_FUNCTION_THROTTLE | \ - LED_FUNCTION_THRUST_RING | \ - LED_FUNCTION_COLOR \ -) +typedef enum { + LED_SCOLOR_DISARMED = 0, + LED_SCOLOR_ARMED, + LED_SCOLOR_ANIMATION, + LED_SCOLOR_BACKGROUND, + LED_SCOLOR_BLINKBACKGROUND, + LED_SCOLOR_GPSNOSATS, + LED_SCOLOR_GPSNOLOCK, + LED_SCOLOR_GPSLOCKED +} ledSpecialColorIds_e; + +typedef enum { + LED_DIRECTION_NORTH = 0, + LED_DIRECTION_EAST, + LED_DIRECTION_SOUTH, + LED_DIRECTION_WEST, + LED_DIRECTION_UP, + LED_DIRECTION_DOWN +} ledDirectionId_e; + +typedef enum { + LED_FUNCTION_COLOR, + LED_FUNCTION_FLIGHT_MODE, + LED_FUNCTION_ARM_STATE, + LED_FUNCTION_BATTERY, + LED_FUNCTION_RSSI, + LED_FUNCTION_GPS, + LED_FUNCTION_THRUST_RING, +} ledBaseFunctionId_e; + +typedef enum { + LED_OVERLAY_THROTTLE, + LED_OVERLAY_LARSON_SCANNER, + LED_OVERLAY_BLINK, + LED_OVERLAY_LANDING_FLASH, + LED_OVERLAY_INDICATOR, + LED_OVERLAY_WARNING, +} ledOverlayId_e; + +typedef struct modeColorIndexes_s { + uint8_t color[LED_DIRECTION_COUNT]; +} modeColorIndexes_t; + +typedef struct specialColorIndexes_s { + uint8_t color[LED_SPECIAL_COLOR_COUNT]; +} specialColorIndexes_t; + +typedef uint32_t ledConfig_t; + +typedef struct ledCounts_s { + uint8_t count; + uint8_t ring; + uint8_t larson; + uint8_t ringSeqLen; +} ledCounts_t; -typedef struct ledConfig_s { - uint8_t xy; // see LED_X/Y_MASK defines - uint8_t color; // see colors (config_master) - uint16_t flags; // see ledFlag_e -} ledConfig_t; +ledConfig_t *ledConfigs; +hsvColor_t *colors; +modeColorIndexes_t *modeColors; +specialColorIndexes_t specialColors; -extern uint8_t ledCount; -extern uint8_t ledsInRingCount; +#define DEFINE_LED(x, y, col, dir, func, ol, params) (LED_MOV_POS(CALCULATE_LED_XY(x, y)) | LED_MOV_COLOR(col) | LED_MOV_DIRECTION(dir) | LED_MOV_FUNCTION(func) | LED_MOV_OVERLAY(ol) | LED_MOV_PARAMS(params)) +static inline uint8_t ledGetXY(const ledConfig_t *lcfg) { return ((*lcfg >> LED_POS_OFFSET) & LED_BIT_MASK(LED_POS_BITCNT)); } +static inline uint8_t ledGetX(const ledConfig_t *lcfg) { return ((*lcfg >> (LED_POS_OFFSET + LED_X_BIT_OFFSET)) & LED_XY_MASK); } +static inline uint8_t ledGetY(const ledConfig_t *lcfg) { return ((*lcfg >> (LED_POS_OFFSET + LED_Y_BIT_OFFSET)) & LED_XY_MASK); } +static inline uint8_t ledGetFunction(const ledConfig_t *lcfg) { return ((*lcfg >> LED_FUNCTION_OFFSET) & LED_BIT_MASK(LED_FUNCTION_BITCNT)); } +static inline uint8_t ledGetOverlay(const ledConfig_t *lcfg) { return ((*lcfg >> LED_OVERLAY_OFFSET) & LED_BIT_MASK(LED_OVERLAY_BITCNT)); } +static inline uint8_t ledGetColor(const ledConfig_t *lcfg) { return ((*lcfg >> LED_COLOR_OFFSET) & LED_BIT_MASK(LED_COLOR_BITCNT)); } +static inline uint8_t ledGetDirection(const ledConfig_t *lcfg) { return ((*lcfg >> LED_DIRECTION_OFFSET) & LED_BIT_MASK(LED_DIRECTION_BITCNT)); } +static inline uint8_t ledGetParams(const ledConfig_t *lcfg) { return ((*lcfg >> LED_PARAMS_OFFSET) & LED_BIT_MASK(LED_PARAMS_BITCNT)); } +static inline bool ledGetOverlayBit(const ledConfig_t *lcfg, int id) { return ((ledGetOverlay(lcfg) >> id) & 1); } +static inline bool ledGetDirectionBit(const ledConfig_t *lcfg, int id) { return ((ledGetDirection(lcfg) >> id) & 1); } +/* +PG_DECLARE_ARR(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs); +PG_DECLARE_ARR(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors); +PG_DECLARE_ARR(modeColorIndexes_t, LED_MODE_COUNT, modeColors); +PG_DECLARE(specialColorIndexes_t, specialColors); +*/ +bool parseColor(int index, const char *colorConfig); -bool parseLedStripConfig(uint8_t ledIndex, const char *config); +bool parseLedStripConfig(int ledIndex, const char *config); +void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize); +void reevaluateLedConfig(void); + +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); +void ledStripEnable(void); void updateLedStrip(void); -void updateLedRing(void); + +bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex); + +extern uint16_t rssi; // FIXME dependency on mw.c + void applyDefaultLedStripConfig(ledConfig_t *ledConfig); -void generateLedConfig(uint8_t ledIndex, char *ledConfigBuffer, size_t bufferSize); +void applyDefaultColors(hsvColor_t *colors); +void applyDefaultModeColors(modeColorIndexes_t *modeColors); +void applyDefaultSpecialColors(specialColorIndexes_t *specialColors); -bool parseColor(uint8_t index, const char *colorConfig); -void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount); - -void ledStripEnable(void); -void reevaluateLedConfig(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 768489a8de..1ca39c4699 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -156,6 +156,7 @@ static void cliMap(char *cmdline); #ifdef LED_STRIP static void cliLed(char *cmdline); static void cliColor(char *cmdline); +static void cliModeColor(char *cmdline); #endif #ifndef USE_QUAD_MIXER_ONLY @@ -264,6 +265,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("aux", "configure modes", NULL, cliAux), #ifdef LED_STRIP CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), + CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor), #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), @@ -1409,20 +1411,20 @@ static void cliLed(char *cmdline) char ledConfigBuffer[20]; if (isEmpty(cmdline)) { - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { + for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); cliPrintf("led %u %s\r\n", i, ledConfigBuffer); } } else { ptr = cmdline; i = atoi(ptr); - if (i < MAX_LED_STRIP_LENGTH) { + if (i < LED_MAX_STRIP_LENGTH) { ptr = strchr(cmdline, ' '); if (!parseLedStripConfig(i, ++ptr)) { cliShowParseError(); } } else { - cliShowArgumentRangeError("index", 0, MAX_LED_STRIP_LENGTH - 1); + cliShowArgumentRangeError("index", 0, LED_MAX_STRIP_LENGTH - 1); } } } @@ -1433,7 +1435,7 @@ static void cliColor(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { cliPrintf("color %u %d,%u,%u\r\n", i, masterConfig.colors[i].h, @@ -1444,16 +1446,57 @@ static void cliColor(char *cmdline) } else { ptr = cmdline; i = atoi(ptr); - if (i < CONFIGURABLE_COLOR_COUNT) { + if (i < LED_CONFIGURABLE_COLOR_COUNT) { ptr = strchr(cmdline, ' '); if (!parseColor(i, ++ptr)) { cliShowParseError(); } } else { - cliShowArgumentRangeError("index", 0, CONFIGURABLE_COLOR_COUNT - 1); + cliShowArgumentRangeError("index", 0, LED_CONFIGURABLE_COLOR_COUNT - 1); } } } + +static void cliModeColor(char *cmdline) +{ + if (isEmpty(cmdline)) { + for (int i = 0; i < LED_MODE_COUNT; i++) { + for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + int colorIndex = modeColors[i].color[j]; + cliPrintf("mode_color %u %u %u\r\n", i, j, colorIndex); + } + } + + for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + int colorIndex = specialColors.color[j]; + cliPrintf("mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex); + } + } else { + enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; + int args[ARGS_COUNT]; + int argNo = 0; + char* ptr = strtok(cmdline, " "); + while (ptr && argNo < ARGS_COUNT) { + args[argNo++] = atoi(ptr); + ptr = strtok(NULL, " "); + } + + if (ptr != NULL || argNo != ARGS_COUNT) { + cliShowParseError(); + return; + } + + int modeIdx = args[MODE]; + int funIdx = args[FUNCTION]; + int color = args[COLOR]; + if(!setModeColor(modeIdx, funIdx, color)) { + cliShowParseError(); + return; + } + // values are validated + cliPrintf("mode_color %u %u %u\r\n", modeIdx, funIdx, color); + } +} #endif #ifdef USE_SERVOS @@ -2078,6 +2121,9 @@ static void cliDump(char *cmdline) cliPrint("\r\n\r\n# color\r\n"); cliColor(""); + + cliPrint("\r\n\r\n# mode_color\r\n"); + cliModeColor(""); #endif cliPrint("\r\n# aux\r\n"); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 054eb49403..a29542e216 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1085,8 +1085,8 @@ static bool processOutCommand(uint8_t cmdMSP) #ifdef LED_STRIP case MSP_LED_COLORS: - headSerialReply(CONFIGURABLE_COLOR_COUNT * 4); - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + headSerialReply(LED_CONFIGURABLE_COLOR_COUNT * 4); + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; serialize16(color->h); serialize8(color->s); @@ -1095,14 +1095,27 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_LED_STRIP_CONFIG: - headSerialReply(MAX_LED_STRIP_LENGTH * 7); - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { + headSerialReply(LED_MAX_STRIP_LENGTH * 4); + for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - serialize16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET); - serialize16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET); - serialize8(GET_LED_X(ledConfig)); - serialize8(GET_LED_Y(ledConfig)); - serialize8(ledConfig->color); + serialize32(*ledConfig); + } + break; + + case MSP_LED_STRIP_MODECOLOR: + headSerialReply(((LED_MODE_COUNT * LED_DIRECTION_COUNT) + LED_SPECIAL_COLOR_COUNT) * 3); + for (int i = 0; i < LED_MODE_COUNT; i++) { + for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + serialize8(i); + serialize8(j); + serialize8(masterConfig.modeColors[i].color[j]); + } + } + + for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + serialize8(LED_MODE_COUNT); + serialize8(j); + serialize8(masterConfig.specialColors.color[j]); } break; #endif @@ -1714,7 +1727,7 @@ static bool processInCommand(void) #ifdef LED_STRIP case MSP_SET_LED_COLORS: - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; color->h = read16(); color->s = read8(); @@ -1725,31 +1738,26 @@ static bool processInCommand(void) case MSP_SET_LED_STRIP_CONFIG: { i = read8(); - if (i >= MAX_LED_STRIP_LENGTH || currentPort->dataSize != (1 + 7)) { + if (i >= LED_MAX_STRIP_LENGTH || currentPort->dataSize != (1 + 4)) { headSerialError(0); break; } ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - uint16_t mask; - // currently we're storing directions and functions in a uint16 (flags) - // the msp uses 2 x uint16_t to cater for future expansion - mask = read16(); - ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; - - mask = read16(); - ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; - - mask = read8(); - ledConfig->xy = CALCULATE_LED_X(mask); - - mask = read8(); - ledConfig->xy |= CALCULATE_LED_Y(mask); - - ledConfig->color = read8(); - + *ledConfig = read32(); reevaluateLedConfig(); } break; + + case MSP_SET_LED_STRIP_MODECOLOR: + { + ledModeIndex_e modeIdx = read8(); + int funIdx = read8(); + int color = read8(); + + if (!setModeColor(modeIdx, funIdx, color)) + return false; + } + break; #endif case MSP_REBOOT: isRebootScheduled = true; diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 60d5a80d05..67d06032a5 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -59,7 +59,7 @@ #define MSP_PROTOCOL_VERSION 0 #define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 17 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR +#define API_VERSION_MINOR 20 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR #define API_VERSION_LENGTH 2 @@ -247,6 +247,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_3D 124 //out message Settings needed for reversible ESCs #define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll #define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag +#define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed @@ -267,6 +268,7 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll #define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults #define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag +#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings // #define MSP_BIND 240 //in message no param // #define MSP_ALARMS 242 diff --git a/src/main/main.c b/src/main/main.c index 944faf0fac..c0e99552f4 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -134,7 +134,7 @@ void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); void imuInit(void); void displayInit(rxConfig_t *intialRxConfig); -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); void spektrumBind(rxConfig_t *rxConfig); const sonarHardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor); void osdInit(void); @@ -544,7 +544,7 @@ void init(void) #endif #ifdef LED_STRIP - ledStripInit(masterConfig.ledConfigs, masterConfig.colors); + ledStripInit(masterConfig.ledConfigs, masterConfig.colors, masterConfig.modeColors, &masterConfig.specialColors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 70129156b0..2bb1f973f0 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -224,7 +224,7 @@ fix12_t calculateVbatPidCompensation(void) { uint8_t calculateBatteryPercentage(void) { - return (((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount); + return constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100); } uint8_t calculateBatteryCapacityRemainingPercentage(void) From 26f26ab9595a0bbb0c07e339cb41fd32940afea6 Mon Sep 17 00:00:00 2001 From: nathan Date: Mon, 18 Jul 2016 00:27:14 -0700 Subject: [PATCH 068/456] cc3d flash is overflowing w/ new bf pidc, undef led_strip for now? --- src/main/target/CC3D/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index ab711c151a..c06f397138 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -121,6 +121,7 @@ #ifdef CC3D_OPBL #define SKIP_CLI_COMMAND_HELP +#undef LED_STRIP #undef DISPLAY #undef SONAR #endif From eae86926d0c94494aba3efd4f255bea24688b47b Mon Sep 17 00:00:00 2001 From: gaelj Date: Mon, 18 Jul 2016 12:07:56 +0200 Subject: [PATCH 069/456] Fix broken compiling on colibri target due to new LED strip behaviour --- src/main/target/COLIBRI_RACE/i2c_bst.c | 30 +++++--------------------- 1 file changed, 5 insertions(+), 25 deletions(-) diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index be0764a03b..c2cf9377dc 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -933,7 +933,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) #ifdef LED_STRIP case BST_LED_COLORS: - for (i = 0; i < CONFIGURABLE_COLOR_COUNT; i++) { + for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { hsvColor_t *color = &masterConfig.colors[i]; bstWrite16(color->h); bstWrite8(color->s); @@ -942,13 +942,9 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest) break; case BST_LED_STRIP_CONFIG: - for (i = 0; i < MAX_LED_STRIP_LENGTH; i++) { + for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - bstWrite16((ledConfig->flags & LED_DIRECTION_MASK) >> LED_DIRECTION_BIT_OFFSET); - bstWrite16((ledConfig->flags & LED_FUNCTION_MASK) >> LED_FUNCTION_BIT_OFFSET); - bstWrite8(GET_LED_X(ledConfig)); - bstWrite8(GET_LED_Y(ledConfig)); - bstWrite8(ledConfig->color); + bstWrite32(*ledConfig); } break; #endif @@ -1381,28 +1377,12 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) case BST_SET_LED_STRIP_CONFIG: { i = bstRead8(); - if (i >= MAX_LED_STRIP_LENGTH || bstReadDataSize() != (1 + 7)) { + if (i >= LED_MAX_STRIP_LENGTH || bstReadDataSize() != (1 + 4)) { ret = BST_FAILED; break; } ledConfig_t *ledConfig = &masterConfig.ledConfigs[i]; - uint16_t mask; - // currently we're storing directions and functions in a uint16 (flags) - // the msp uses 2 x uint16_t to cater for future expansion - mask = bstRead16(); - ledConfig->flags = (mask << LED_DIRECTION_BIT_OFFSET) & LED_DIRECTION_MASK; - - mask = bstRead16(); - ledConfig->flags |= (mask << LED_FUNCTION_BIT_OFFSET) & LED_FUNCTION_MASK; - - mask = bstRead8(); - ledConfig->xy = CALCULATE_LED_X(mask); - - mask = bstRead8(); - ledConfig->xy |= CALCULATE_LED_Y(mask); - - ledConfig->color = bstRead8(); - + *ledConfig = bstRead32(); reevaluateLedConfig(); } break; From 58d310b20810d06378b24d7b86e210250b3a1fac Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 18 Jul 2016 17:27:50 +0200 Subject: [PATCH 070/456] Fix late stabilisation with zerothrtottle stabilisation feature --- src/main/mw.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index 2848abd40e..371961e013 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -485,7 +485,7 @@ void updateMagHold(void) void processRx(void) { static bool armedBeeperOn = false; - static bool wasAirmodeIsActivated; + static bool airmodeIsActivated; calculateRxChannelsAndUpdateFailsafe(currentTime); @@ -509,21 +509,21 @@ void processRx(void) throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { - if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) wasAirmodeIsActivated = true; // Prevent Iterm from being reset + if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset } else { - wasAirmodeIsActivated = false; + airmodeIsActivated = false; } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ - if (throttleStatus == THROTTLE_LOW && !wasAirmodeIsActivated) { + if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { pidResetErrorGyroState(); if (currentProfile->pidProfile.zeroThrottleStabilisation) pidStabilisationState(PID_STABILISATION_ON); else pidStabilisationState(PID_STABILISATION_OFF); } else { - if (currentProfile->pidProfile.zeroThrottleStabilisation || wasAirmodeIsActivated) pidStabilisationState(PID_STABILISATION_ON); + pidStabilisationState(PID_STABILISATION_ON); } // When armed and motors aren't spinning, do beeps and then disarm From 37daed3054949c10ee289eb8e53c75166b7f6888 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 19 Jul 2016 01:57:04 +1000 Subject: [PATCH 071/456] REVO target.c corrections. --- src/main/target/REVO/target.c | 78 +++++++++++++++++------------------ 1 file changed, 37 insertions(+), 41 deletions(-) diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 4e5aa61a12..67f7327285 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -22,28 +22,28 @@ #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { - PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; const uint16_t multiPWM[] = { - PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 - PWM2 | (MAP_TO_PWM_INPUT << 8), - PWM3 | (MAP_TO_PWM_INPUT << 8), - PWM4 | (MAP_TO_PWM_INPUT << 8), - PWM5 | (MAP_TO_PWM_INPUT << 8), - PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 @@ -54,7 +54,7 @@ const uint16_t multiPWM[] = { }; const uint16_t airPPM[] = { - PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), PWM9 | (MAP_TO_SERVO_OUTPUT << 8), @@ -70,12 +70,12 @@ const uint16_t airPPM[] = { }; const uint16_t airPWM[] = { - PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 - PWM2 | (MAP_TO_PWM_INPUT << 8), - PWM3 | (MAP_TO_PWM_INPUT << 8), - PWM4 | (MAP_TO_PWM_INPUT << 8), - PWM5 | (MAP_TO_PWM_INPUT << 8), - PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 @@ -85,21 +85,17 @@ const uint16_t airPWM[] = { 0xFFFF }; - const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S6_IN - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5 }, // S6_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S6_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5 }, // S6_OUT }; - - From 8f3c14c6491b41d05203cd471ed81c9e3635cc3f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 18 Jul 2016 18:31:52 +0200 Subject: [PATCH 072/456] Update defaults --- src/main/config/config.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index bb6ff6ad66..dbf2bca956 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -226,8 +226,8 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->toleranceBand = 15; pidProfile->toleranceBandReduction = 35; pidProfile->zeroCrossAllowanceCount = 3; - pidProfile->accelerationLimitPercent = 15; - pidProfile->itermThrottleGain = 0; + pidProfile->accelerationLimitPercent = 20; + pidProfile->itermThrottleGain = 10; #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. From 2e1766fbb9df666c37f0660aca6b850dfdca6084 Mon Sep 17 00:00:00 2001 From: gaelj Date: Mon, 18 Jul 2016 19:16:15 +0200 Subject: [PATCH 073/456] Set back LED strip update speed to 100Hz --- src/main/scheduler/scheduler_tasks.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/scheduler/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c index 18299e4904..d7852c55bf 100644 --- a/src/main/scheduler/scheduler_tasks.c +++ b/src/main/scheduler/scheduler_tasks.c @@ -168,7 +168,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_LEDSTRIP] = { .taskName = "LEDSTRIP", .taskFunc = taskLedStrip, - .desiredPeriod = 1000000 / 10, // 10 Hz + .desiredPeriod = 1000000 / 100, // 100 Hz .staticPriority = TASK_PRIORITY_LOW, }, #endif From 5ff2ed825b2a70642ce9e1af3aeadb1d64c07054 Mon Sep 17 00:00:00 2001 From: gaelj Date: Tue, 19 Jul 2016 01:34:11 +0200 Subject: [PATCH 074/456] extract msp_protocol.h from serial_msp.h --- src/main/io/msp_protocol.h | 289 +++++++++++++++++++++++++++++++++++++ src/main/io/serial_msp.c | 1 + src/main/io/serial_msp.h | 268 ---------------------------------- 3 files changed, 290 insertions(+), 268 deletions(-) create mode 100644 src/main/io/msp_protocol.h diff --git a/src/main/io/msp_protocol.h b/src/main/io/msp_protocol.h new file mode 100644 index 0000000000..a1c95c8b5e --- /dev/null +++ b/src/main/io/msp_protocol.h @@ -0,0 +1,289 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +/** + * MSP Guidelines, emphasis is used to clarify. + * + * Each FlightController (FC, Server) MUST change the API version when any MSP command is added, deleted, or changed. + * + * If you fork the FC source code and release your own version, you MUST change the Flight Controller Identifier. + * + * NEVER release a modified copy of this code that shares the same Flight controller IDENT and API version + * if the API doesn't match EXACTLY. + * + * Consumers of the API (API clients) SHOULD first attempt to get a response from the MSP_API_VERSION command. + * If no response is obtained then client MAY try the legacy MSP_IDENT command. + * + * API consumers should ALWAYS handle communication failures gracefully and attempt to continue + * without the information if possible. Clients MAY log/display a suitable message. + * + * API clients should NOT attempt any communication if they can't handle the returned API MAJOR VERSION. + * + * API clients SHOULD attempt communication if the API MINOR VERSION has increased from the time + * the API client was written and handle command failures gracefully. Clients MAY disable + * functionality that depends on the commands while still leaving other functionality intact. + * that the newer API version may cause problems before using API commands that change FC state. + * + * It is for this reason that each MSP command should be specific as possible, such that changes + * to commands break as little functionality as possible. + * + * API client authors MAY use a compatibility matrix/table when determining if they can support + * a given command from a given flight controller at a given api version level. + * + * Developers MUST NOT create new MSP commands that do more than one thing. + * + * Failure to follow these guidelines will likely invoke the wrath of developers trying to write tools + * that use the API and the users of those tools. + */ + +#pragma once + +/* Protocol numbers used both by the wire format, config system, and + field setters. +*/ + +#define MSP_PROTOCOL_VERSION 0 + +#define API_VERSION_MAJOR 1 // increment when major changes are made +#define API_VERSION_MINOR 20 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR + +#define API_VERSION_LENGTH 2 + +#define MULTIWII_IDENTIFIER "MWII"; +#define CLEANFLIGHT_IDENTIFIER "CLFL" +#define BETAFLIGHT_IDENTIFIER "BTFL" +#define BASEFLIGHT_IDENTIFIER "BAFL"; + +#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4 +extern const char * const flightControllerIdentifier; + +#define FLIGHT_CONTROLLER_VERSION_LENGTH 3 +#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF + +static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; +#define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used. +#define BOARD_HARDWARE_REVISION_LENGTH 2 + +// These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version. +#define CAP_PLATFORM_32BIT ((uint32_t)1 << 31) +#define CAP_BASEFLIGHT_CONFIG ((uint32_t)1 << 30) + +// MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask. +#define CAP_NAVI_VERSION_BIT_4_MSB ((uint32_t)1 << 31) +#define CAP_NAVI_VERSION_BIT_3 ((uint32_t)1 << 30) +#define CAP_NAVI_VERSION_BIT_2 ((uint32_t)1 << 29) +#define CAP_NAVI_VERSION_BIT_1_LSB ((uint32_t)1 << 28) + +#define CAP_DYNBALANCE ((uint32_t)1 << 2) +#define CAP_FLAPS ((uint32_t)1 << 3) +#define CAP_NAVCAP ((uint32_t)1 << 4) +#define CAP_EXTAUX ((uint32_t)1 << 5) + +#define MSP_API_VERSION 1 //out message +#define MSP_FC_VARIANT 2 //out message +#define MSP_FC_VERSION 3 //out message +#define MSP_BOARD_INFO 4 //out message +#define MSP_BUILD_INFO 5 //out message + +#define MSP_NAME 10 +#define MSP_SET_NAME 11 + + +// +// MSP commands for Cleanflight original features +// +#define MSP_MODE_RANGES 34 //out message Returns all mode ranges +#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range + +#define MSP_FEATURE 36 +#define MSP_SET_FEATURE 37 + +#define MSP_BOARD_ALIGNMENT 38 +#define MSP_SET_BOARD_ALIGNMENT 39 + +#define MSP_CURRENT_METER_CONFIG 40 +#define MSP_SET_CURRENT_METER_CONFIG 41 + +#define MSP_MIXER 42 +#define MSP_SET_MIXER 43 + +#define MSP_RX_CONFIG 44 +#define MSP_SET_RX_CONFIG 45 + +#define MSP_LED_COLORS 46 +#define MSP_SET_LED_COLORS 47 + +#define MSP_LED_STRIP_CONFIG 48 +#define MSP_SET_LED_STRIP_CONFIG 49 + +#define MSP_RSSI_CONFIG 50 +#define MSP_SET_RSSI_CONFIG 51 + +#define MSP_ADJUSTMENT_RANGES 52 +#define MSP_SET_ADJUSTMENT_RANGE 53 + +// private - only to be used by the configurator, the commands are likely to change +#define MSP_CF_SERIAL_CONFIG 54 +#define MSP_SET_CF_SERIAL_CONFIG 55 + +#define MSP_VOLTAGE_METER_CONFIG 56 +#define MSP_SET_VOLTAGE_METER_CONFIG 57 + +#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm] + +#define MSP_PID_CONTROLLER 59 +#define MSP_SET_PID_CONTROLLER 60 + +#define MSP_ARMING_CONFIG 61 //out message Returns auto_disarm_delay and disarm_kill_switch parameters +#define MSP_SET_ARMING_CONFIG 62 //in message Sets auto_disarm_delay and disarm_kill_switch parameters + +// +// Baseflight MSP commands (if enabled they exist in Cleanflight) +// +#define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total) +#define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP + +// FIXME - Provided for backwards compatibility with configurator code until configurator is updated. +// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. +#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere +#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save + +#define MSP_REBOOT 68 //in message reboot settings + +// DEPRECATED - Use MSP_BUILD_INFO instead +#define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion + + +#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip +#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip +#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip + +#define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter +#define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter + +#define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings +#define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings + +#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings +#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings + +#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card + +#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings +#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings + +#define MSP_TRANSPONDER_CONFIG 82 //in message Get transponder settings +#define MSP_SET_TRANSPONDER_CONFIG 83 //out message Set transponder settings + +#define MSP_OSD_CONFIG 84 //in message Get osd settings +#define MSP_SET_OSD_CONFIG 85 //out message Set osd settings + +#define MSP_OSD_CHAR_READ 86 //in message Get osd settings +#define MSP_OSD_CHAR_WRITE 87 //out message Set osd settings + +#define MSP_VTX_CONFIG 88 //in message Get vtx settings +#define MSP_SET_VTX_CONFIG 89 //out message Set vtx settings + +// Betaflight Additional Commands +#define MSP_PID_ADVANCED_CONFIG 90 +#define MSP_SET_PID_ADVANCED_CONFIG 91 + +#define MSP_FILTER_CONFIG 92 +#define MSP_SET_FILTER_CONFIG 93 + +#define MSP_ADVANCED_TUNING 94 +#define MSP_SET_ADVANCED_TUNING 95 + +#define MSP_SENSOR_CONFIG 96 +#define MSP_SET_SENSOR_CONFIG 97 + +#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility +#define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility + +// +// Multwii original MSP commands +// + +// DEPRECATED - See MSP_API_VERSION and MSP_MIXER +#define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable + + +#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number +#define MSP_RAW_IMU 102 //out message 9 DOF +#define MSP_SERVO 103 //out message servos +#define MSP_MOTOR 104 //out message motors +#define MSP_RC 105 //out message rc channels and more +#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course +#define MSP_COMP_GPS 107 //out message distance home, direction home +#define MSP_ATTITUDE 108 //out message 2 angles 1 heading +#define MSP_ALTITUDE 109 //out message altitude, variometer +#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX +#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID +#define MSP_PID 112 //out message P I D coeff (9 are used currently) +#define MSP_BOX 113 //out message BOX setup (number is dependant of your setup) +#define MSP_MISC 114 //out message powermeter trig +#define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI +#define MSP_BOXNAMES 116 //out message the aux switch names +#define MSP_PIDNAMES 117 //out message the PID names +#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold +#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes +#define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations. +#define MSP_NAV_STATUS 121 //out message Returns navigation status +#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters +#define MSP_3D 124 //out message Settings needed for reversible ESCs +#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll +#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag +#define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings + +#define MSP_SET_RAW_RC 200 //in message 8 rc chan +#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed +#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently) +#define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup) +#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo +#define MSP_ACC_CALIBRATION 205 //in message no param +#define MSP_MAG_CALIBRATION 206 //in message no param +#define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use +#define MSP_RESET_CONF 208 //in message no param +#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags) +#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2) +#define MSP_SET_HEAD 211 //in message define a new heading hold direction +#define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings +#define MSP_SET_MOTOR 214 //in message PropBalance function +#define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom +#define MSP_SET_3D 217 //in message Settings needed for reversible ESCs +#define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll +#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults +#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag +#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings + +// #define MSP_BIND 240 //in message no param +// #define MSP_ALARMS 242 + +#define MSP_EEPROM_WRITE 250 //in message no param + +#define MSP_DEBUGMSG 253 //out message debug string buffer +#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 + +// Additional commands that are not compatible with MultiWii +#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc +#define MSP_UID 160 //out message Unique device ID +#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) +#define MSP_ACC_TRIM 240 //out message get acc angle trim values +#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values +#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration +#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration +#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index ecd9273c8d..f0022e57e2 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -59,6 +59,7 @@ #include "io/asyncfatfs/asyncfatfs.h" #include "io/osd.h" #include "io/vtx.h" +#include "io/msp_protocol.h" #include "telemetry/telemetry.h" diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 67d06032a5..2778da7e22 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -20,274 +20,6 @@ #include "io/serial.h" #include "drivers/serial.h" -/** - * MSP Guidelines, emphasis is used to clarify. - * - * Each FlightController (FC, Server) MUST change the API version when any MSP command is added, deleted, or changed. - * - * If you fork the FC source code and release your own version, you MUST change the Flight Controller Identifier. - * - * NEVER release a modified copy of this code that shares the same Flight controller IDENT and API version - * if the API doesn't match EXACTLY. - * - * Consumers of the API (API clients) SHOULD first attempt to get a response from the MSP_API_VERSION command. - * If no response is obtained then client MAY try the legacy MSP_IDENT command. - * - * API consumers should ALWAYS handle communication failures gracefully and attempt to continue - * without the information if possible. Clients MAY log/display a suitable message. - * - * API clients should NOT attempt any communication if they can't handle the returned API MAJOR VERSION. - * - * API clients SHOULD attempt communication if the API MINOR VERSION has increased from the time - * the API client was written and handle command failures gracefully. Clients MAY disable - * functionality that depends on the commands while still leaving other functionality intact. - * Clients SHOULD operate in READ-ONLY mode and SHOULD present a warning to the user to state - * that the newer API version may cause problems before using API commands that change FC state. - * - * It is for this reason that each MSP command should be specific as possible, such that changes - * to commands break as little client functionality as possible. - * - * API client authors MAY use a compatibility matrix/table when determining if they can support - * a given command from a given flight controller at a given api version level. - * - * Developers MUST NOT create new MSP commands that do more than one thing. - * - * Failure to follow these guidelines will likely invoke the wrath of developers trying to write tools - * that use the API and the users of those tools. - */ - -#define MSP_PROTOCOL_VERSION 0 - -#define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 20 // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR - -#define API_VERSION_LENGTH 2 - -#define MULTIWII_IDENTIFIER "MWII"; -#define CLEANFLIGHT_IDENTIFIER "CLFL" -#define BETAFLIGHT_IDENTIFIER "BTFL" -#define BASEFLIGHT_IDENTIFIER "BAFL"; - -#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4 -extern const char * const flightControllerIdentifier; - -#define FLIGHT_CONTROLLER_VERSION_LENGTH 3 -#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF - -static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; -#define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used. -#define BOARD_HARDWARE_REVISION_LENGTH 2 - -// These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version. -#define CAP_PLATFORM_32BIT ((uint32_t)1 << 31) -#define CAP_BASEFLIGHT_CONFIG ((uint32_t)1 << 30) - -// MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask. -#define CAP_NAVI_VERSION_BIT_4_MSB ((uint32_t)1 << 31) -#define CAP_NAVI_VERSION_BIT_3 ((uint32_t)1 << 30) -#define CAP_NAVI_VERSION_BIT_2 ((uint32_t)1 << 29) -#define CAP_NAVI_VERSION_BIT_1_LSB ((uint32_t)1 << 28) - -#define CAP_DYNBALANCE ((uint32_t)1 << 2) -#define CAP_FLAPS ((uint32_t)1 << 3) -#define CAP_NAVCAP ((uint32_t)1 << 4) -#define CAP_EXTAUX ((uint32_t)1 << 5) - -#define MSP_API_VERSION 1 //out message -#define MSP_FC_VARIANT 2 //out message -#define MSP_FC_VERSION 3 //out message -#define MSP_BOARD_INFO 4 //out message -#define MSP_BUILD_INFO 5 //out message - -#define MSP_NAME 10 -#define MSP_SET_NAME 11 - - -// -// MSP commands for Cleanflight original features -// -#define MSP_MODE_RANGES 34 //out message Returns all mode ranges -#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range - -#define MSP_FEATURE 36 -#define MSP_SET_FEATURE 37 - -#define MSP_BOARD_ALIGNMENT 38 -#define MSP_SET_BOARD_ALIGNMENT 39 - -#define MSP_CURRENT_METER_CONFIG 40 -#define MSP_SET_CURRENT_METER_CONFIG 41 - -#define MSP_MIXER 42 -#define MSP_SET_MIXER 43 - -#define MSP_RX_CONFIG 44 -#define MSP_SET_RX_CONFIG 45 - -#define MSP_LED_COLORS 46 -#define MSP_SET_LED_COLORS 47 - -#define MSP_LED_STRIP_CONFIG 48 -#define MSP_SET_LED_STRIP_CONFIG 49 - -#define MSP_RSSI_CONFIG 50 -#define MSP_SET_RSSI_CONFIG 51 - -#define MSP_ADJUSTMENT_RANGES 52 -#define MSP_SET_ADJUSTMENT_RANGE 53 - -// private - only to be used by the configurator, the commands are likely to change -#define MSP_CF_SERIAL_CONFIG 54 -#define MSP_SET_CF_SERIAL_CONFIG 55 - -#define MSP_VOLTAGE_METER_CONFIG 56 -#define MSP_SET_VOLTAGE_METER_CONFIG 57 - -#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm] - -#define MSP_PID_CONTROLLER 59 -#define MSP_SET_PID_CONTROLLER 60 - -#define MSP_ARMING_CONFIG 61 //out message Returns auto_disarm_delay and disarm_kill_switch parameters -#define MSP_SET_ARMING_CONFIG 62 //in message Sets auto_disarm_delay and disarm_kill_switch parameters - -// -// Baseflight MSP commands (if enabled they exist in Cleanflight) -// -#define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total) -#define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP - -// FIXME - Provided for backwards compatibility with configurator code until configurator is updated. -// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. -#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere -#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save - -#define MSP_REBOOT 68 //in message reboot settings - -// DEPRECATED - Use MSP_BUILD_INFO instead -#define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion - - -#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip -#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip -#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip - -#define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter -#define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter - -#define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings -#define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings - -#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings -#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings - -#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card - -#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings -#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings - -#define MSP_TRANSPONDER_CONFIG 82 //in message Get transponder settings -#define MSP_SET_TRANSPONDER_CONFIG 83 //out message Set transponder settings - -#define MSP_OSD_CONFIG 84 //in message Get osd settings -#define MSP_SET_OSD_CONFIG 85 //out message Set osd settings - -#define MSP_OSD_CHAR_READ 86 //in message Get osd settings -#define MSP_OSD_CHAR_WRITE 87 //out message Set osd settings - -#define MSP_VTX_CONFIG 88 //in message Get vtx settings -#define MSP_SET_VTX_CONFIG 89 //out message Set vtx settings - -// Betaflight Additional Commands -#define MSP_PID_ADVANCED_CONFIG 90 -#define MSP_SET_PID_ADVANCED_CONFIG 91 - -#define MSP_FILTER_CONFIG 92 -#define MSP_SET_FILTER_CONFIG 93 - -#define MSP_ADVANCED_TUNING 94 -#define MSP_SET_ADVANCED_TUNING 95 - -#define MSP_SENSOR_CONFIG 96 -#define MSP_SET_SENSOR_CONFIG 97 - -#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility -#define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility - -// -// Multwii original MSP commands -// - -// DEPRECATED - See MSP_API_VERSION and MSP_MIXER -#define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable - - -#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number -#define MSP_RAW_IMU 102 //out message 9 DOF -#define MSP_SERVO 103 //out message servos -#define MSP_MOTOR 104 //out message motors -#define MSP_RC 105 //out message rc channels and more -#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course -#define MSP_COMP_GPS 107 //out message distance home, direction home -#define MSP_ATTITUDE 108 //out message 2 angles 1 heading -#define MSP_ALTITUDE 109 //out message altitude, variometer -#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX -#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID -#define MSP_PID 112 //out message P I D coeff (9 are used currently) -#define MSP_BOX 113 //out message BOX setup (number is dependant of your setup) -#define MSP_MISC 114 //out message powermeter trig -#define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI -#define MSP_BOXNAMES 116 //out message the aux switch names -#define MSP_PIDNAMES 117 //out message the PID names -#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold -#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes -#define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations. -#define MSP_NAV_STATUS 121 //out message Returns navigation status -#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters -#define MSP_3D 124 //out message Settings needed for reversible ESCs -#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll -#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag -#define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings - -#define MSP_SET_RAW_RC 200 //in message 8 rc chan -#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed -#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently) -#define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup) -#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo -#define MSP_ACC_CALIBRATION 205 //in message no param -#define MSP_MAG_CALIBRATION 206 //in message no param -#define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use -#define MSP_RESET_CONF 208 //in message no param -#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags) -#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2) -#define MSP_SET_HEAD 211 //in message define a new heading hold direction -#define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings -#define MSP_SET_MOTOR 214 //in message PropBalance function -#define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom -#define MSP_SET_3D 217 //in message Settings needed for reversible ESCs -#define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll -#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults -#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag -#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings - -// #define MSP_BIND 240 //in message no param -// #define MSP_ALARMS 242 - -#define MSP_EEPROM_WRITE 250 //in message no param - -#define MSP_DEBUGMSG 253 //out message debug string buffer -#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 - -// Additional commands that are not compatible with MultiWii -#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc -#define MSP_UID 160 //out message Unique device ID -#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) -#define MSP_ACC_TRIM 240 //out message get acc angle trim values -#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values -#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration -#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration -#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface - // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports. #define MAX_MSP_PORT_COUNT 2 From 2f52d4b7318d1db6310e788a3528f454b1d499b6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 19 Jul 2016 09:02:33 +0100 Subject: [PATCH 075/456] Updated MSP protocol with Cleanflight and iNav commands to help avoid future conflicts --- src/main/io/msp_protocol.h | 40 +++++++++++++++++++++++--------------- src/main/io/serial_msp.c | 1 + 2 files changed, 25 insertions(+), 16 deletions(-) diff --git a/src/main/io/msp_protocol.h b/src/main/io/msp_protocol.h index a1c95c8b5e..66ab4b9c58 100644 --- a/src/main/io/msp_protocol.h +++ b/src/main/io/msp_protocol.h @@ -64,17 +64,16 @@ #define API_VERSION_LENGTH 2 #define MULTIWII_IDENTIFIER "MWII"; -#define CLEANFLIGHT_IDENTIFIER "CLFL" -#define BETAFLIGHT_IDENTIFIER "BTFL" #define BASEFLIGHT_IDENTIFIER "BAFL"; +#define BETAFLIGHT_IDENTIFIER "BTFL" +#define CLEANFLIGHT_IDENTIFIER "CLFL" +#define INAV_IDENTIFIER "INAV" +#define RACEFLIGHT_IDENTIFIER "RCFL" #define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4 -extern const char * const flightControllerIdentifier; - #define FLIGHT_CONTROLLER_VERSION_LENGTH 3 #define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF -static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used. #define BOARD_HARDWARE_REVISION_LENGTH 2 @@ -99,8 +98,8 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_BOARD_INFO 4 //out message #define MSP_BUILD_INFO 5 //out message -#define MSP_NAME 10 -#define MSP_SET_NAME 11 +#define MSP_NAME 10 //out message Returns user set board name - betaflight +#define MSP_SET_NAME 11 //in message Sets board name - betaflight // @@ -186,17 +185,17 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings #define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings -#define MSP_TRANSPONDER_CONFIG 82 //in message Get transponder settings -#define MSP_SET_TRANSPONDER_CONFIG 83 //out message Set transponder settings +#define MSP_TRANSPONDER_CONFIG 82 //out message Get transponder settings +#define MSP_SET_TRANSPONDER_CONFIG 83 //in message Set transponder settings -#define MSP_OSD_CONFIG 84 //in message Get osd settings -#define MSP_SET_OSD_CONFIG 85 //out message Set osd settings +#define MSP_OSD_CONFIG 84 //out message Get osd settings - betaflight +#define MSP_SET_OSD_CONFIG 85 //in message Set osd settings - betaflight -#define MSP_OSD_CHAR_READ 86 //in message Get osd settings -#define MSP_OSD_CHAR_WRITE 87 //out message Set osd settings +#define MSP_OSD_CHAR_READ 86 //out message Get osd settings - betaflight +#define MSP_OSD_CHAR_WRITE 87 //in message Set osd settings - betaflight -#define MSP_VTX_CONFIG 88 //in message Get vtx settings -#define MSP_SET_VTX_CONFIG 89 //out message Set vtx settings +#define MSP_VTX_CONFIG 88 //out message Get vtx settings - betaflight +#define MSP_SET_VTX_CONFIG 89 //in message Set vtx settings - betaflight // Betaflight Additional Commands #define MSP_PID_ADVANCED_CONFIG 90 @@ -214,6 +213,12 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility #define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility +// +// OSD specific +// +#define MSP_OSD_VIDEO_CONFIG 180 +#define MSP_SET_OSD_VIDEO_CONFIG 181 + // // Multwii original MSP commands // @@ -274,14 +279,17 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; // #define MSP_ALARMS 242 #define MSP_EEPROM_WRITE 250 //in message no param - +#define MSP_RESERVE_1 251 //reserved for system usage +#define MSP_RESERVE_2 252 //reserved for system usage #define MSP_DEBUGMSG 253 //out message debug string buffer #define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 +#define MSP_RESERVE_3 255 //reserved for system usage // Additional commands that are not compatible with MultiWii #define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc #define MSP_UID 160 //out message Unique device ID #define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) +#define MSP_GPSSTATISTICS 166 //out message get GPS debugging data #define MSP_ACC_TRIM 240 //out message get acc angle trim values #define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values #define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index f0022e57e2..ecba5090de 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -108,6 +108,7 @@ extern void resetPidProfile(pidProfile_t *pidProfile); void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. +static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; typedef struct box_e { const uint8_t boxId; // see boxId_e From 52e302e51e07935a5384891e3e23151c1445bdc8 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 20 Jul 2016 01:11:25 +0200 Subject: [PATCH 076/456] Fix Horizon / Angle in betaflight pidc --- src/main/flight/pid.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d23db67a3d..b77ea8f039 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -159,19 +159,19 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { // calculate error angle and limit the angle to the max inclination #ifdef GPS - const float errorAngle = (constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here + const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #else - const float errorAngle = (constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here + const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #endif if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed - angleRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f; + angleRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; } else { // HORIZON mode - direct sticks control is applied to rate PID // mix up angle error to desired AngleRate to add a little auto-level feel - angleRate[axis] = angleRateSmooth[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f); + angleRate[axis] = angleRateSmooth[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); } } From f61ec9af0a77a10e2d829b5bea8540680aec901c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 20 Jul 2016 01:31:32 +0200 Subject: [PATCH 077/456] rcCommandSmooth for level mode --- src/main/flight/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b77ea8f039..41c8ef838b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -159,10 +159,10 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { // calculate error angle and limit the angle to the max inclination #ifdef GPS - const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + const float errorAngle = (constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #else - const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + const float errorAngle = (constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #endif if (FLIGHT_MODE(ANGLE_MODE)) { From 8ecd5e10c8c96d5d98233a6a304426bb806cfe98 Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Wed, 20 Jul 2016 15:42:40 +0200 Subject: [PATCH 078/456] fixed warning blink with better visualisation used a second black color for better blink effect --- src/main/io/ledstrip.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 4ab463bd1c..1c123cf9a5 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -687,17 +687,17 @@ static void applyLedWarningLayer(bool updateNow, uint32_t *timer) } if (warningFlags) { - const hsvColor_t *warningColor = NULL; + const hsvColor_t *warningColor = &HSV(BLACK); bool colorOn = (warningFlashCounter % 2) == 0; // w_w_ warningFlags_e warningId = warningFlashCounter / 4; if (warningFlags & (1 << warningId)) { switch (warningId) { case WARNING_ARMING_DISABLED: - warningColor = colorOn ? &HSV(GREEN) : NULL; + warningColor = colorOn ? &HSV(GREEN) : &HSV(BLACK); break; case WARNING_LOW_BATTERY: - warningColor = colorOn ? &HSV(RED) : NULL; + warningColor = colorOn ? &HSV(RED) : &HSV(BLACK); break; case WARNING_FAILSAFE: warningColor = colorOn ? &HSV(YELLOW) : &HSV(BLUE); From 6a82c669706c86ca5a9415f85bedd7bad44da03c Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Wed, 20 Jul 2016 09:35:50 -0700 Subject: [PATCH 079/456] Add CPPM support to Sirin target, fix vtx_channel size in cli --- src/main/io/serial_cli.c | 2 +- src/main/target/SIRINFPV/target.c | 21 +++++++++++---------- src/main/target/SIRINFPV/target.h | 4 ++-- src/main/target/SIRINFPV/target.mk | 1 - 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 1ca39c4699..fccab2747c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -824,7 +824,7 @@ const clivalue_t valueTable[] = { { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, #endif #ifdef USE_RTC6705 - { "vtx_channel", VAR_INT16 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } }, + { "vtx_channel", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_channel, .config.minmax = { 0, 39 } }, { "vtx_power", VAR_UINT8 | MASTER_VALUE, &masterConfig.vtx_power, .config.minmax = { 0, 1 } }, #endif #ifdef OSD diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index 0630b527db..b52fbd00e4 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -22,7 +22,7 @@ #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { - // No PPM + PWM7 | (MAP_TO_PPM_INPUT << 8), PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), @@ -43,21 +43,21 @@ const uint16_t multiPWM[] = { }; const uint16_t airPPM[] = { - // No PPM - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_PPM_INPUT << 8), + PWM1 | (MAP_TO_SERVO_OUTPUT << 8), + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; const uint16_t airPWM[] = { - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM1 | (MAP_TO_SERVO_OUTPUT << 8), + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF @@ -73,6 +73,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB0 - *TIM3_CH3 { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB1 - *TIM3_CH4 + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)y }; diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 0b2ca5f969..1632d2046d 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -149,8 +149,8 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define USABLE_TIMER_CHANNEL_COUNT 6 -#define USED_TIMERS (TIM_N(3) | TIM_N(4)) +#define USABLE_TIMER_CHANNEL_COUNT 7 +#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(4)) #define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4) #define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOB) diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index 945fb1fe9d..19cc56e950 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -15,4 +15,3 @@ TARGET_SRC = \ drivers/vtx_soft_spi_rtc6705.c \ drivers/max7456.c \ io/osd.c - From cfc38960f2e41ae274b880075d8d84394322122b Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Wed, 20 Jul 2016 20:46:18 +0200 Subject: [PATCH 080/456] fixed ledstrip warning overlay long dark pause between blinks --- src/main/io/ledstrip.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 1c123cf9a5..c92a45fdc2 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -687,7 +687,7 @@ static void applyLedWarningLayer(bool updateNow, uint32_t *timer) } if (warningFlags) { - const hsvColor_t *warningColor = &HSV(BLACK); + const hsvColor_t *warningColor = NULL; bool colorOn = (warningFlashCounter % 2) == 0; // w_w_ warningFlags_e warningId = warningFlashCounter / 4; @@ -705,7 +705,6 @@ static void applyLedWarningLayer(bool updateNow, uint32_t *timer) default:; } } - if (warningColor) applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor); } } From 25374161429544ec730b3c5aec1b4a5b3f9179ae Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Wed, 20 Jul 2016 21:22:30 +0200 Subject: [PATCH 081/456] fixed ledstrip warning overlay - line 708 back again --- src/main/io/ledstrip.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index c92a45fdc2..573d562374 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -705,6 +705,7 @@ static void applyLedWarningLayer(bool updateNow, uint32_t *timer) default:; } } + if (warningColor) applyLedHsv(LED_MOV_OVERLAY(LED_FLAG_OVERLAY(LED_OVERLAY_WARNING)), warningColor); } } From ab02e8d956f30283c1cceb3e05dc81ff2eca74d3 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 22 Jul 2016 11:53:53 +0200 Subject: [PATCH 082/456] Brushed motor fix --- src/main/config/config.c | 1 + src/main/main.c | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index dbf2bca956..74399f1bd8 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -537,6 +537,7 @@ static void resetConf(void) #ifdef BRUSHED_MOTORS masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; masterConfig.motor_pwm_protocol = PWM_TYPE_BRUSHED; + masterConfig.use_unsyncedPwm = true; #else masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; masterConfig.motor_pwm_protocol = PWM_TYPE_ONESHOT125; diff --git a/src/main/main.c b/src/main/main.c index c0e99552f4..63869ab52c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -323,7 +323,6 @@ void init(void) if (masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED) { featureClear(FEATURE_3D); pwm_params.idlePulse = 0; // brushed motors - use_unsyncedPwm = false; } #ifdef CC3D pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false; From df0f32344ff956bd94ea0a6d3bf3b10ae9a083bb Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 22 Jul 2016 13:36:34 +0200 Subject: [PATCH 083/456] AlienFlight target sepcific defaults update --- src/main/target/ALIENFLIGHTF3/config.c | 11 ++++++++--- src/main/target/ALIENFLIGHTF4/config.c | 11 ++++++++--- src/main/target/NAZE/config.c | 3 --- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 8c693a98cf..271c9292c4 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -84,9 +84,14 @@ void targetConfiguration(void) { masterConfig.motor_pwm_rate = 32000; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; - currentControlRateProfile->rates[FD_PITCH] = 40; - currentControlRateProfile->rates[FD_ROLL] = 40; - currentControlRateProfile->rates[FD_YAW] = 40; + masterConfig.gyro_sync_denom = 2; + masterConfig.pid_process_denom = 1; + currentProfile->pidProfile.P8[ROLL] = 90; + currentProfile->pidProfile.I8[ROLL] = 44; + currentProfile->pidProfile.D8[ROLL] = 60; + currentProfile->pidProfile.P8[PITCH] = 90; + currentProfile->pidProfile.I8[PITCH] = 44; + currentProfile->pidProfile.D8[PITCH] = 60; parseRcChannels("TAER1234", &masterConfig.rxConfig); masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 8c693a98cf..56086358c1 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -84,9 +84,14 @@ void targetConfiguration(void) { masterConfig.motor_pwm_rate = 32000; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; - currentControlRateProfile->rates[FD_PITCH] = 40; - currentControlRateProfile->rates[FD_ROLL] = 40; - currentControlRateProfile->rates[FD_YAW] = 40; + masterConfig.gyro_sync_denom = 1; + masterConfig.pid_process_denom = 1; + currentProfile->pidProfile.P8[ROLL] = 90; + currentProfile->pidProfile.I8[ROLL] = 44; + currentProfile->pidProfile.D8[ROLL] = 60; + currentProfile->pidProfile.P8[PITCH] = 90; + currentProfile->pidProfile.I8[PITCH] = 44; + currentProfile->pidProfile.D8[PITCH] = 60; parseRcChannels("TAER1234", &masterConfig.rxConfig); masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R diff --git a/src/main/target/NAZE/config.c b/src/main/target/NAZE/config.c index f9fde08608..411f4e4154 100644 --- a/src/main/target/NAZE/config.c +++ b/src/main/target/NAZE/config.c @@ -83,9 +83,6 @@ void targetConfiguration(void) { masterConfig.motor_pwm_rate = 32000; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; - currentControlRateProfile->rates[FD_PITCH] = 40; - currentControlRateProfile->rates[FD_ROLL] = 40; - currentControlRateProfile->rates[FD_YAW] = 40; parseRcChannels("TAER1234", &masterConfig.rxConfig); masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R From 1b3ae14b8d395aec8608889ec4eef23cac1efdc0 Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 23 Jul 2016 00:23:15 -0700 Subject: [PATCH 084/456] allow pwm mapping to xN (complementary) channels --- src/main/drivers/pwm_output.c | 16 +++++++++++----- src/main/drivers/timer.h | 3 ++- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index ecd0447a9a..bcee6c70b8 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -64,16 +64,22 @@ static uint8_t allocatedOutputPortCount = 0; static bool pwmMotorsEnabled = true; -static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t ouputPolarity) +static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) { TIM_OCInitTypeDef TIM_OCInitStructure; TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + if (output & TIMER_OUTPUT_N_CHANNEL) { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable; + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + } + else { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + } TIM_OCInitStructure.TIM_Pulse = value; - TIM_OCInitStructure.TIM_OCPolarity = ouputPolarity ? TIM_OCPolarity_High : TIM_OCPolarity_Low; + TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_High : TIM_OCPolarity_Low; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; switch (channel) { @@ -106,7 +112,7 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount); IOConfigGPIO(io, IOCFG_AF_PP); - pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output & TIMER_OUTPUT_INVERTED); + pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output); if (timerHardware->output & TIMER_OUTPUT_ENABLED) { TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE); diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index e11f5a8974..7050bbabda 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -88,7 +88,8 @@ typedef struct timerHardware_s { enum { TIMER_OUTPUT_ENABLED = 0x01, - TIMER_OUTPUT_INVERTED = 0x02 + TIMER_OUTPUT_INVERTED = 0x02, + TIMER_OUTPUT_N_CHANNEL= 0x04 }; #ifdef STM32F1 From 6fe75fcf34f64d4af49d1d4b2b452f26a7439d8b Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 23 Jul 2016 00:29:18 -0700 Subject: [PATCH 085/456] try some different timers instead of TIM15 for motor output --- src/main/target/KISSFC/target.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 5045599da5..f0b36fd605 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -65,8 +65,10 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_6}, { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_2}, - { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, + { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_6}, // TIM1_CH2N + { TIM1, IO_TAG(PB15), TIM_Channel_3, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_4}, // TIM1_CH3N + //{ TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, + //{ TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, From 61770a76ce67163e2a8d2baffe03a28a386b42b7 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 23 Jul 2016 18:09:39 +1000 Subject: [PATCH 086/456] Built test harness (Fake FVT 30A LittleBee) --- src/main/io/serial_4way.c | 25 +- src/main/io/serial_4way_avrootloader.c | 606 ++++++++++++++++++++++++- src/main/vcpf4/usbd_cdc_vcp.c | 35 +- src/main/vcpf4/usbd_cdc_vcp.h | 8 +- src/main/vcpf4/usbd_conf.h | 4 +- 5 files changed, 623 insertions(+), 55 deletions(-) diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 9dd777b462..80a150b4e5 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -443,20 +443,21 @@ void esc4wayProcess(serialPort_t *mspPort) CRC_check.bytes[1] = ReadByte(); CRC_check.bytes[0] = ReadByte(); - RX_LED_OFF; - if(CRC_check.word == CRC_in.word) { ACK_OUT = ACK_OK; } else { ACK_OUT = ACK_I_INVALID_CRC; } - + + TX_LED_ON; + if (ACK_OUT == ACK_OK) { // wtf.D_FLASH_ADDR_H=Adress_H; // wtf.D_FLASH_ADDR_L=Adress_L; ioMem.D_PTR_I = ParamBuf; + switch(CMD) { // ******* Interface related stuff ******* case cmd_InterfaceTestAlive: @@ -522,13 +523,13 @@ void esc4wayProcess(serialPort_t *mspPort) } case cmd_InterfaceSetMode: { - #if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) +#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) if ((ParamBuf[0] <= imSK) && (ParamBuf[0] >= imSIL_BLB)) { - #elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) +#elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) if ((ParamBuf[0] <= imATM_BLB) && (ParamBuf[0] >= imSIL_BLB)) { - #elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) +#elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) if (ParamBuf[0] == imSK) { - #endif +#endif CurrentInterfaceMode = ParamBuf[0]; } else { ACK_OUT = ACK_I_INVALID_PARAM; @@ -616,7 +617,7 @@ void esc4wayProcess(serialPort_t *mspPort) //Address = Page * 512 ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 1); ioMem.D_FLASH_ADDR_L = 0; - if (!BL_PageErase(&ioMem)) ACK_OUT = ACK_D_GENERAL_ERROR; + if (!BL_PageErase(&ioMem)) ACK_OUT = ACK_D_GENERAL_ERROR; break; } default: @@ -793,7 +794,8 @@ void esc4wayProcess(serialPort_t *mspPort) CRCout.word = 0; - TX_LED_ON; + RX_LED_OFF; + serialBeginWrite(port); WriteByteCrc(cmd_Remote_Escape); WriteByteCrc(CMD); @@ -812,6 +814,11 @@ void esc4wayProcess(serialPort_t *mspPort) WriteByte(CRCout.bytes[1]); WriteByte(CRCout.bytes[0]); serialEndWrite(port); + +#if defined(STM32F4) && defined(USE_VCP) + if (port->identifier == SERIAL_PORT_USB_VCP) + delay(75); /* we need some processing cycles to ensure VCP4 transmission completes and it doesn't lock up */ +#endif TX_LED_OFF; if (isExitScheduled) { esc4wayRelease(); diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 2a10dcdb4a..761efd2504 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -33,36 +33,36 @@ #include "io/serial_4way.h" #include "io/serial_4way_impl.h" #include "io/serial_4way_avrootloader.h" -#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && 1 // Bootloader commands // RunCmd -#define RestartBootloader 0 -#define ExitBootloader 1 +#define RestartBootloader 0 +#define ExitBootloader 1 -#define CMD_RUN 0x00 -#define CMD_PROG_FLASH 0x01 -#define CMD_ERASE_FLASH 0x02 -#define CMD_READ_FLASH_SIL 0x03 -#define CMD_VERIFY_FLASH 0x03 -#define CMD_READ_EEPROM 0x04 -#define CMD_PROG_EEPROM 0x05 -#define CMD_READ_SRAM 0x06 -#define CMD_READ_FLASH_ATM 0x07 -#define CMD_KEEP_ALIVE 0xFD -#define CMD_SET_ADDRESS 0xFF -#define CMD_SET_BUFFER 0xFE +#define CMD_RUN 0x00 +#define CMD_PROG_FLASH 0x01 +#define CMD_ERASE_FLASH 0x02 +#define CMD_READ_FLASH_SIL 0x03 +#define CMD_VERIFY_FLASH 0x03 +#define CMD_READ_EEPROM 0x04 +#define CMD_PROG_EEPROM 0x05 +#define CMD_READ_SRAM 0x06 +#define CMD_READ_FLASH_ATM 0x07 +#define CMD_KEEP_ALIVE 0xFD +#define CMD_SET_ADDRESS 0xFF +#define CMD_SET_BUFFER 0xFE -#define CMD_BOOTINIT 0x07 -#define CMD_BOOTSIGN 0x08 +#define CMD_BOOTINIT 0x07 +#define CMD_BOOTSIGN 0x08 // Bootloader result codes -#define brSUCCESS 0x30 -#define brERRORCOMMAND 0xC1 -#define brERRORCRC 0xC2 -#define brNONE 0xFF +#define brSUCCESS 0x30 +#define brERRORCOMMAND 0xC1 +#define brERRORCRC 0xC2 +#define brNONE 0xFF #define START_BIT_TIMEOUT_MS 2 @@ -296,7 +296,6 @@ static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout) return 0; } - uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem) { if(interface_mode == imATM_BLB) { @@ -304,8 +303,7 @@ uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem) } else { return BL_ReadA(CMD_READ_FLASH_SIL, pMem); } -} - +} uint8_t BL_ReadEEprom(ioMem_t *pMem) { @@ -332,5 +330,565 @@ uint8_t BL_WriteFlash(ioMem_t *pMem) return BL_WriteA(CMD_PROG_FLASH, pMem, (40 / START_BIT_TIMEOUT_MS)); } +#else + +#define FAKE_PAGE_SIZE 512 +#define FAKE_FLASH_SIZE 16385 + +static uint8_t fakeFlash[FAKE_FLASH_SIZE] = +{ + 0x02, 0x19, 0xFD, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0xAA, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x01, 0x87, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x44, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x02, 0x03, 0x31, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x02, 0x03, 0x04, 0x06, 0x08, 0x0C, 0x10, 0x18, 0x20, 0x30, 0x40, 0x60, 0x80, 0x04, 0x06, 0x08, + 0x0C, 0x10, 0x18, 0x20, 0x30, 0x40, 0x60, 0x80, 0xA0, 0xC0, 0x00, 0x03, 0x07, 0x0F, 0x1F, 0x0D, + 0x0D, 0x04, 0x05, 0x0D, 0x05, 0x03, 0x05, 0x03, 0x03, 0x02, 0xC2, 0xAF, 0x30, 0x63, 0x08, 0xC2, + 0x63, 0x85, 0x79, 0x8A, 0xD2, 0xAF, 0x32, 0xC0, 0xD0, 0xC0, 0xE0, 0x20, 0x62, 0x21, 0xE5, 0x25, + 0x60, 0x16, 0xE4, 0x73, 0xE5, 0x25, 0xF4, 0xC3, 0x33, 0x40, 0x09, 0x75, 0x8A, 0x00, 0xD2, 0x63, + 0xF5, 0x79, 0x01, 0xD6, 0xF5, 0x8A, 0xD2, 0x62, 0xD0, 0xE0, 0xD0, 0xD0, 0xD2, 0xAF, 0x32, 0xC3, + 0xE5, 0x26, 0x33, 0x40, 0x09, 0x75, 0x8A, 0x00, 0xD2, 0x63, 0xF5, 0x79, 0x01, 0xF0, 0xF5, 0x8A, + 0xC2, 0x62, 0xE5, 0x26, 0xF4, 0x60, 0x2F, 0x30, 0x68, 0x25, 0x20, 0x72, 0x0D, 0xD0, 0xE0, 0xD0, + 0xD0, 0xC2, 0x93, 0xC2, 0x97, 0xC2, 0x94, 0xD2, 0xAF, 0x32, 0xC2, 0x93, 0xC2, 0x97, 0xC2, 0x94, + 0x20, 0x6F, 0x0C, 0x20, 0x66, 0x09, 0x74, 0x06, 0xD5, 0xE0, 0xFD, 0xE5, 0x7B, 0x42, 0x90, 0xD0, + 0xE0, 0xD0, 0xD0, 0xD2, 0xAF, 0x32, 0x75, 0x8A, 0x00, 0xD2, 0x63, 0x75, 0x79, 0x00, 0xC2, 0x8D, + 0xD2, 0x62, 0x21, 0x1F, 0x01, 0xC4, 0x30, 0x68, 0x05, 0x20, 0x66, 0x02, 0xD2, 0x93, 0x01, 0xC4, + 0x30, 0x68, 0x05, 0x20, 0x66, 0x02, 0xD2, 0x94, 0x01, 0xC4, 0x30, 0x68, 0x05, 0x20, 0x66, 0x02, + 0xD2, 0x97, 0x01, 0xC4, 0xC2, 0x92, 0x30, 0x68, 0x0A, 0x20, 0x66, 0x07, 0x74, 0x06, 0xD5, 0xE0, + 0xFD, 0xD2, 0x93, 0x01, 0xC4, 0xC2, 0x95, 0x30, 0x68, 0x0A, 0x20, 0x66, 0x07, 0x74, 0x06, 0xD5, + 0xE0, 0xFD, 0xD2, 0x94, 0x01, 0xC4, 0xC2, 0x96, 0x30, 0x68, 0x0A, 0x20, 0x66, 0x07, 0x74, 0x06, + 0xD5, 0xE0, 0xFD, 0xD2, 0x97, 0x01, 0xC4, 0xC2, 0xAF, 0xC2, 0xAD, 0x53, 0xE6, 0xEF, 0xC0, 0xD0, + 0xC0, 0xE0, 0xD2, 0xD3, 0xD2, 0xAF, 0xE5, 0x7A, 0x60, 0x09, 0xE5, 0x77, 0x60, 0x05, 0x75, 0x77, + 0x00, 0x41, 0xDE, 0x75, 0x77, 0x01, 0xC2, 0xCE, 0xE5, 0x2A, 0x60, 0x07, 0x20, 0x74, 0x49, 0x15, + 0x2A, 0x21, 0xF8, 0x78, 0x00, 0x79, 0x00, 0x20, 0x74, 0x2E, 0xE5, 0x80, 0x30, 0x7E, 0x01, 0xF4, + 0x30, 0xE5, 0x02, 0x78, 0xFF, 0x53, 0xDA, 0xCF, 0x20, 0x7E, 0x03, 0x43, 0xDA, 0x20, 0x30, 0x7E, + 0x03, 0x43, 0xDA, 0x10, 0xC2, 0xD8, 0xC2, 0x71, 0xE5, 0x80, 0x30, 0x7E, 0x01, 0xF4, 0x30, 0xE5, + 0x02, 0x79, 0xFF, 0xC3, 0xE8, 0x99, 0x70, 0xCB, 0x30, 0x61, 0x03, 0x75, 0x2A, 0x18, 0x20, 0x74, + 0x03, 0x75, 0x2A, 0x18, 0x88, 0x5C, 0xD2, 0x70, 0x20, 0x74, 0x0D, 0xE5, 0x2B, 0x60, 0x04, 0x15, + 0x2B, 0x41, 0x08, 0x43, 0xDA, 0x01, 0xC2, 0xD8, 0x20, 0x70, 0x02, 0x41, 0x77, 0xA8, 0x5C, 0x20, + 0x61, 0x02, 0xC2, 0x70, 0x20, 0x74, 0x36, 0x79, 0x82, 0xB7, 0x04, 0x31, 0xC3, 0xE8, 0x94, 0xF0, + 0x40, 0x03, 0x74, 0xF0, 0xF8, 0xE8, 0xC4, 0x54, 0x0F, 0x28, 0xF8, 0x79, 0x84, 0xB7, 0x03, 0x02, + 0x41, 0x4D, 0xC3, 0x13, 0xC3, 0x13, 0x87, 0x21, 0x20, 0x08, 0x02, 0xC3, 0x13, 0x20, 0x0A, 0x06, + 0xC3, 0xC8, 0x98, 0xF8, 0x41, 0x4D, 0x28, 0xF8, 0x50, 0x03, 0x74, 0xFF, 0xF8, 0x88, 0x22, 0xE5, + 0x2D, 0x54, 0x06, 0x60, 0x22, 0x20, 0x6B, 0x1F, 0xE5, 0x36, 0xC3, 0x33, 0xC3, 0x33, 0xF8, 0xE5, + 0x64, 0xC3, 0x13, 0xC3, 0x13, 0xF9, 0xC3, 0x95, 0x22, 0x40, 0x02, 0x89, 0x22, 0xE8, 0x25, 0x22, + 0xF5, 0x22, 0x50, 0x03, 0x75, 0x22, 0xFF, 0x78, 0x82, 0xB6, 0x04, 0x59, 0x85, 0x22, 0x24, 0xA8, + 0x24, 0xC3, 0xE5, 0x24, 0x95, 0x61, 0x40, 0x02, 0xA8, 0x61, 0xC3, 0xE8, 0x95, 0x63, 0x40, 0x02, + 0xA8, 0x63, 0x88, 0x25, 0xE5, 0x66, 0x70, 0x02, 0x41, 0xC8, 0xC3, 0xE8, 0xAA, 0x66, 0x9A, 0x50, + 0x03, 0xE8, 0xFA, 0xE4, 0xF9, 0xEA, 0xC3, 0x33, 0xFB, 0xE5, 0x68, 0xF4, 0x5B, 0x29, 0x40, 0x0D, + 0x25, 0x67, 0xF8, 0xE5, 0x67, 0x60, 0x02, 0x15, 0x67, 0x40, 0x0B, 0x41, 0xC8, 0x0A, 0xC3, 0xE5, + 0x67, 0x9A, 0x50, 0x02, 0x05, 0x67, 0x78, 0xFF, 0x88, 0x26, 0xC2, 0x6F, 0xC3, 0xE5, 0x26, 0x94, + 0xF8, 0x40, 0x02, 0xD2, 0x6F, 0xC3, 0xE5, 0x25, 0x94, 0x40, 0x40, 0x02, 0xD2, 0x64, 0x20, 0xCF, + 0x0A, 0xD0, 0xE0, 0xD0, 0xD0, 0x43, 0xE6, 0x10, 0xD2, 0xAD, 0x32, 0xC2, 0xCF, 0x05, 0x3A, 0xE5, + 0x7A, 0x60, 0x09, 0xE5, 0x78, 0x60, 0x05, 0x75, 0x78, 0x00, 0x61, 0x27, 0x75, 0x78, 0x01, 0x78, + 0x01, 0xE5, 0x2A, 0x60, 0x05, 0x30, 0x74, 0x02, 0x15, 0x2A, 0xC3, 0xE5, 0x5C, 0x94, 0x01, 0x40, + 0x05, 0x75, 0x5F, 0x00, 0x61, 0x27, 0x75, 0x60, 0x00, 0x75, 0x69, 0x00, 0xE5, 0x5F, 0x24, 0x01, + 0xF5, 0x5F, 0x50, 0x03, 0x75, 0x5F, 0xFF, 0xD0, 0xE0, 0xD0, 0xD0, 0x43, 0xE6, 0x10, 0xD2, 0xAD, + 0x32, 0xC2, 0xAF, 0x53, 0xE6, 0x7F, 0x75, 0x92, 0xFA, 0x75, 0x93, 0xFF, 0xC2, 0x60, 0x75, 0x91, + 0x04, 0xD2, 0xAF, 0x32, 0xC2, 0xAF, 0x53, 0xE6, 0xEF, 0xC2, 0xAD, 0xC0, 0xD0, 0xC0, 0xE0, 0xC0, + 0xF0, 0xD2, 0xD3, 0xD2, 0xAF, 0xA8, 0xFB, 0xA9, 0xFC, 0xE5, 0x7A, 0x60, 0x07, 0xC3, 0xE9, 0x13, + 0xF9, 0xE8, 0x13, 0xF8, 0xC2, 0xD8, 0x30, 0x71, 0x02, 0x61, 0xB6, 0x53, 0xDA, 0xCF, 0x20, 0x7E, + 0x03, 0x43, 0xDA, 0x10, 0x30, 0x7E, 0x03, 0x43, 0xDA, 0x20, 0xD2, 0x71, 0xE5, 0x80, 0x30, 0x7E, + 0x01, 0xF4, 0x20, 0xE5, 0x02, 0x61, 0x8D, 0x88, 0x27, 0x89, 0x28, 0xA1, 0xE3, 0x53, 0xDA, 0xCF, + 0x20, 0x7E, 0x03, 0x43, 0xDA, 0x20, 0x30, 0x7E, 0x03, 0x43, 0xDA, 0x10, 0xC2, 0xD8, 0xC2, 0x71, + 0x30, 0x74, 0x02, 0xA1, 0xCF, 0x78, 0x00, 0xE5, 0x80, 0x30, 0x7E, 0x01, 0xF4, 0x30, 0xE5, 0x02, + 0xA1, 0xCF, 0x88, 0x5C, 0xA1, 0xB7, 0x53, 0xDA, 0xCF, 0x20, 0x7E, 0x03, 0x43, 0xDA, 0x20, 0x30, + 0x7E, 0x03, 0x43, 0xDA, 0x10, 0xC2, 0x71, 0x20, 0x61, 0x02, 0x81, 0x8F, 0x53, 0xDA, 0xCF, 0x20, + 0x7E, 0x03, 0x43, 0xDA, 0x10, 0x30, 0x7E, 0x03, 0x43, 0xDA, 0x20, 0xC2, 0xD8, 0xD2, 0x71, 0x88, + 0x57, 0x89, 0x58, 0xC3, 0xE8, 0x95, 0x55, 0xF8, 0xE9, 0x95, 0x56, 0xF9, 0x7B, 0x00, 0x7E, 0x08, + 0x7A, 0x00, 0xC3, 0xE8, 0x94, 0x8C, 0xE9, 0x94, 0x00, 0x50, 0x05, 0x75, 0x5B, 0x00, 0x81, 0x81, + 0x88, 0x21, 0x78, 0xA2, 0xE6, 0xA8, 0x21, 0x60, 0x55, 0xC3, 0xE8, 0x94, 0xC8, 0xE9, 0x94, 0x00, + 0x50, 0x08, 0xE4, 0xD2, 0xE4, 0xFB, 0x7A, 0x0A, 0x81, 0x5C, 0xC3, 0xE8, 0x94, 0x68, 0xE9, 0x94, + 0x01, 0x50, 0x08, 0xE4, 0xD2, 0xE3, 0xFB, 0x7A, 0x0F, 0x81, 0x5C, 0xC3, 0xE8, 0x94, 0xD0, 0xE9, + 0x94, 0x02, 0x50, 0x08, 0xE4, 0xD2, 0xE2, 0xFB, 0x7A, 0x1E, 0x81, 0x5C, 0xC3, 0xE8, 0x94, 0xA0, + 0xE9, 0x94, 0x05, 0x50, 0x08, 0xE4, 0xD2, 0xE1, 0xFB, 0x7A, 0x3C, 0x81, 0x5C, 0xC3, 0xE8, 0x94, + 0x98, 0xE9, 0x94, 0x08, 0x50, 0x08, 0xE4, 0xD2, 0xE0, 0xFB, 0x7A, 0x78, 0x7E, 0x00, 0xC3, 0xE8, + 0x95, 0x59, 0xFC, 0xE9, 0x95, 0x5A, 0xFD, 0x30, 0xE7, 0x0A, 0xEC, 0xF4, 0x24, 0x01, 0xFC, 0xED, + 0xF4, 0x34, 0x00, 0xFD, 0x75, 0x5B, 0x00, 0xC3, 0xEC, 0x9A, 0xED, 0x9E, 0x50, 0x03, 0x75, 0x5B, + 0x01, 0x88, 0x59, 0x89, 0x5A, 0x85, 0x57, 0x55, 0x85, 0x58, 0x56, 0x78, 0x02, 0xA1, 0xB7, 0xC3, + 0xE8, 0x95, 0x27, 0xF8, 0xE9, 0x95, 0x28, 0xF9, 0x30, 0x7C, 0x02, 0xA1, 0x9B, 0x30, 0x7B, 0x02, + 0xA1, 0x9B, 0x30, 0x7A, 0x02, 0xA1, 0x94, 0x20, 0x75, 0x02, 0x81, 0xB2, 0xE9, 0xFD, 0xE8, 0xFC, + 0x81, 0xD1, 0xE9, 0xC3, 0x13, 0xF9, 0xE8, 0x13, 0xF8, 0x30, 0x79, 0x02, 0xA1, 0x94, 0xE9, 0xC3, + 0x13, 0xF9, 0xE8, 0x13, 0xF8, 0x30, 0x78, 0x02, 0xA1, 0x94, 0xE9, 0xC3, 0x13, 0xFD, 0xE8, 0x13, + 0xFC, 0x20, 0x61, 0x2C, 0xC3, 0xEC, 0x94, 0x1C, 0xED, 0x94, 0x02, 0x40, 0x02, 0x81, 0xE8, 0xED, + 0x70, 0x1E, 0xC3, 0xEC, 0x94, 0xC8, 0x50, 0x18, 0x05, 0x29, 0xE5, 0x29, 0x70, 0x02, 0x15, 0x29, + 0xC3, 0xE5, 0x29, 0x94, 0x0A, 0x50, 0x02, 0xA1, 0xCF, 0x75, 0x5C, 0x00, 0xD2, 0x70, 0xA1, 0xCF, + 0xE5, 0x29, 0x60, 0x02, 0x15, 0x29, 0x78, 0x88, 0xE6, 0xF9, 0x74, 0x00, 0x20, 0x7F, 0x08, 0x78, + 0x96, 0xB9, 0x03, 0x02, 0x78, 0x9E, 0xE6, 0x24, 0xFA, 0xFE, 0xE4, 0x34, 0x00, 0xFF, 0xC3, 0xEC, + 0x9E, 0xFC, 0xED, 0x9F, 0xFD, 0x92, 0x08, 0xB9, 0x03, 0x10, 0xA2, 0x08, 0x50, 0x07, 0x20, 0x76, + 0x09, 0xD2, 0x76, 0xA1, 0x3A, 0x30, 0x76, 0x02, 0xC2, 0x76, 0xA2, 0x08, 0x50, 0x16, 0xB9, 0x03, + 0x0D, 0xEC, 0xF4, 0x24, 0x01, 0xFC, 0xED, 0xF4, 0x34, 0x00, 0xFD, 0x02, 0x05, 0x54, 0x78, 0x00, + 0x79, 0x00, 0xA1, 0x9B, 0xB9, 0x03, 0x15, 0xEC, 0x33, 0xFC, 0xED, 0x33, 0xFD, 0xC3, 0xEC, 0x94, + 0x0A, 0xFC, 0xED, 0x94, 0x00, 0xFD, 0x50, 0x04, 0x7C, 0x00, 0x7D, 0x00, 0xC3, 0xEC, 0x94, 0xFF, + 0xED, 0x94, 0x00, 0x40, 0x06, 0x78, 0xFF, 0x79, 0x00, 0xA1, 0x9B, 0xEC, 0x85, 0x72, 0xF0, 0xA4, + 0xC5, 0xF0, 0xA2, 0xF7, 0x33, 0xF8, 0x79, 0x00, 0x40, 0x03, 0x02, 0x05, 0xB7, 0x78, 0xFF, 0x79, + 0x00, 0x02, 0x05, 0xB7, 0xE9, 0xC3, 0x13, 0xF9, 0xE8, 0x13, 0xF8, 0x30, 0x7C, 0x0E, 0xE9, 0x60, + 0x02, 0x78, 0xFF, 0xC3, 0xE8, 0x13, 0x38, 0xF8, 0xE4, 0x34, 0x00, 0xF9, 0xC3, 0xE8, 0x94, 0xFF, + 0xE9, 0x94, 0x00, 0x40, 0x02, 0x78, 0xFF, 0x88, 0x5C, 0xD2, 0x70, 0x20, 0x61, 0x02, 0xA1, 0xCF, + 0x74, 0x1F, 0xF4, 0x55, 0x2F, 0x4B, 0xF5, 0x2F, 0xC2, 0x74, 0xEB, 0x70, 0x02, 0xD2, 0x74, 0x75, + 0x2A, 0x18, 0x30, 0x74, 0x03, 0x75, 0x2A, 0x0A, 0x30, 0x61, 0x02, 0xA1, 0xE3, 0x20, 0x74, 0x03, + 0x53, 0xDA, 0xFE, 0x20, 0x74, 0x03, 0x75, 0x2B, 0x06, 0xD0, 0xF0, 0xD0, 0xE0, 0xD0, 0xD0, 0xD2, + 0xAD, 0x43, 0xE6, 0x10, 0x32, 0x79, 0x01, 0x02, 0x06, 0x13, 0x79, 0x03, 0x02, 0x06, 0x13, 0x79, + 0x0A, 0x02, 0x06, 0x13, 0x79, 0x1E, 0x02, 0x06, 0x13, 0x79, 0x64, 0x02, 0x06, 0x13, 0x79, 0xC8, + 0x02, 0x06, 0x13, 0x78, 0x17, 0xE4, 0xD5, 0xE0, 0xFD, 0xD8, 0xFA, 0xD9, 0xF6, 0x22, 0x7A, 0x14, + 0x7B, 0x78, 0x02, 0x06, 0x3A, 0x7A, 0x10, 0x7B, 0x8C, 0x02, 0x06, 0x3A, 0x7A, 0x0D, 0x7B, 0xB4, + 0x02, 0x06, 0x3A, 0x7A, 0x0B, 0x7B, 0xC8, 0x02, 0x06, 0x3A, 0x79, 0x02, 0xE4, 0xC2, 0x95, 0xD5, + 0xE0, 0xFD, 0xD2, 0x94, 0xD5, 0xE0, 0xFD, 0xC2, 0x94, 0xD5, 0xE0, 0xFD, 0xD2, 0x95, 0xD5, 0xE0, + 0xFD, 0xE9, 0x20, 0xE0, 0x02, 0xD2, 0x93, 0x30, 0xE0, 0x02, 0xD2, 0x97, 0xE5, 0x73, 0xD5, 0xE0, + 0xFD, 0xE9, 0x20, 0xE0, 0x02, 0xC2, 0x93, 0x30, 0xE0, 0x02, 0xC2, 0x97, 0x74, 0x96, 0xD5, 0xE0, + 0xFD, 0xD9, 0xC9, 0xEA, 0xF8, 0xD5, 0xE0, 0xFD, 0xD8, 0xFB, 0xDB, 0xBE, 0xC2, 0x95, 0x22, 0xC3, + 0x7C, 0x00, 0x7D, 0x00, 0x75, 0xF0, 0x00, 0x05, 0xF0, 0xEA, 0x33, 0xFA, 0xEB, 0x33, 0xFB, 0x50, + 0xF6, 0xEB, 0x13, 0xFB, 0xEA, 0x13, 0xFA, 0xC3, 0xE9, 0xFF, 0xE8, 0xFE, 0xE8, 0x9A, 0xF8, 0xE9, + 0x9B, 0xF9, 0x50, 0x04, 0xEF, 0xF9, 0xEE, 0xF8, 0xB3, 0xEC, 0x33, 0xFC, 0xED, 0x33, 0xFD, 0xD5, + 0xF0, 0xDF, 0xED, 0xF9, 0xEC, 0xF8, 0x22, 0xE8, 0x89, 0xF0, 0x8A, 0x20, 0xD2, 0xD4, 0xF8, 0xA9, + 0xF0, 0x7B, 0x00, 0x30, 0xF7, 0x0B, 0x7B, 0xFF, 0xF4, 0x24, 0x01, 0xF8, 0xE9, 0xF4, 0x34, 0x00, + 0xF9, 0xE8, 0x85, 0x20, 0xF0, 0xA4, 0xAD, 0xF0, 0xF8, 0xE9, 0x85, 0x20, 0xF0, 0xA4, 0xAF, 0xF0, + 0xFE, 0xED, 0x2E, 0xF9, 0x74, 0x00, 0x3F, 0xFA, 0x7C, 0x04, 0xC3, 0xEA, 0x13, 0xFA, 0xE9, 0x13, + 0xF9, 0xE8, 0x13, 0xF8, 0xDC, 0xF4, 0x8B, 0xF0, 0x30, 0xF7, 0x0A, 0xE8, 0xF4, 0x24, 0x01, 0xF8, + 0xE9, 0xF4, 0x34, 0x00, 0xF9, 0xE8, 0x89, 0xF0, 0xC2, 0xD4, 0xF8, 0xA9, 0xF0, 0x22, 0x78, 0x82, + 0xB6, 0x04, 0x03, 0x02, 0x07, 0x69, 0xC3, 0xE5, 0x5C, 0x94, 0x01, 0x40, 0x03, 0x02, 0x07, 0x33, + 0x85, 0x22, 0x24, 0xE4, 0xF5, 0x44, 0xF5, 0x45, 0xF5, 0x46, 0xF5, 0x47, 0xF5, 0x48, 0xC2, 0x6E, + 0x02, 0x07, 0x69, 0x78, 0x82, 0xE6, 0xFC, 0xD2, 0x6E, 0xE5, 0x22, 0xF5, 0x23, 0x78, 0x38, 0x79, + 0xC7, 0xAA, 0x40, 0xAB, 0x41, 0xC3, 0xEB, 0x13, 0xFB, 0xEA, 0x13, 0xFA, 0xEC, 0x14, 0x60, 0x13, + 0xC3, 0xEB, 0x13, 0xFB, 0xEA, 0x13, 0xFA, 0xEC, 0x14, 0x14, 0x60, 0x07, 0xC3, 0xEB, 0x13, 0xFB, + 0xEA, 0x13, 0xFA, 0xD1, 0x7F, 0x88, 0x44, 0x89, 0x45, 0x22, 0xC3, 0xE5, 0x44, 0x95, 0x23, 0xF8, + 0xE5, 0x45, 0x94, 0x00, 0xF9, 0x50, 0x0C, 0xC3, 0xE8, 0x94, 0x80, 0xE9, 0x94, 0xFF, 0x40, 0x16, + 0x02, 0x07, 0x9A, 0xC3, 0xE8, 0x94, 0x7F, 0xE9, 0x94, 0x00, 0x50, 0x03, 0x02, 0x07, 0x9A, 0x78, + 0x7F, 0x79, 0x00, 0x02, 0x07, 0x9A, 0x78, 0x80, 0x79, 0xFF, 0x88, 0x49, 0x89, 0x4A, 0x22, 0xE5, + 0x49, 0x25, 0x46, 0xF8, 0xE5, 0x4A, 0x35, 0x47, 0xF9, 0x85, 0x4A, 0x20, 0xE4, 0x30, 0x07, 0x01, + 0xF4, 0x35, 0x48, 0xFA, 0x30, 0xE7, 0x09, 0xC3, 0xEA, 0x94, 0xF0, 0x40, 0x15, 0x02, 0x07, 0xD8, + 0xC3, 0xEA, 0x94, 0x0F, 0x50, 0x03, 0x02, 0x07, 0xD8, 0x78, 0xFF, 0x79, 0xFF, 0x7A, 0x0F, 0x02, + 0x07, 0xD8, 0x78, 0x00, 0x79, 0x00, 0x7A, 0xF0, 0xC3, 0xE5, 0x24, 0x95, 0x61, 0x50, 0x0A, 0xC3, + 0x74, 0x01, 0x95, 0x24, 0x50, 0x0B, 0x02, 0x07, 0xF6, 0xE5, 0x4A, 0x20, 0xE7, 0x0E, 0x02, 0x07, + 0xF6, 0xE5, 0x4A, 0x30, 0xE7, 0x06, 0x88, 0x46, 0x89, 0x47, 0x8A, 0x48, 0x22, 0x78, 0xA5, 0xE6, + 0xFA, 0xC3, 0xE5, 0x49, 0x33, 0xF8, 0xE5, 0x4A, 0x33, 0xF9, 0x12, 0x06, 0xB7, 0xE9, 0x30, 0xE7, + 0x0B, 0xC3, 0xE8, 0x94, 0x80, 0xE9, 0x94, 0xFF, 0x40, 0x13, 0x01, 0x31, 0xC3, 0xE8, 0x94, 0x7F, + 0xE9, 0x94, 0x00, 0x50, 0x02, 0x01, 0x31, 0x78, 0x7F, 0x79, 0x00, 0x01, 0x31, 0x78, 0x80, 0x79, + 0xFF, 0xE8, 0x20, 0xE7, 0x15, 0xC3, 0xE5, 0x23, 0x98, 0xF8, 0x40, 0x09, 0xC3, 0xE8, 0x94, 0x01, + 0x40, 0x03, 0x02, 0x08, 0x58, 0x78, 0x01, 0x02, 0x08, 0x58, 0xE8, 0xF4, 0x24, 0x01, 0x25, 0x23, + 0xF8, 0x40, 0x03, 0x02, 0x08, 0x58, 0x78, 0xFF, 0x88, 0x4B, 0x22, 0x78, 0xA6, 0xE6, 0xFA, 0xA8, + 0x47, 0xA9, 0x48, 0x12, 0x06, 0xB7, 0xE9, 0x30, 0xE7, 0x0C, 0xC3, 0xE8, 0x94, 0x01, 0xE9, 0x94, + 0xFF, 0x40, 0x16, 0x02, 0x08, 0x8D, 0xC3, 0xE8, 0x94, 0xFF, 0xE9, 0x94, 0x00, 0x50, 0x03, 0x02, + 0x08, 0x8D, 0x78, 0xFF, 0x79, 0x00, 0x02, 0x08, 0x8D, 0x78, 0x01, 0x79, 0xFF, 0xE9, 0x20, 0xE7, + 0x15, 0xC3, 0xE5, 0x4B, 0x98, 0xF8, 0x40, 0x09, 0xC3, 0xE8, 0x94, 0x01, 0x40, 0x03, 0x02, 0x08, + 0xB4, 0x78, 0x01, 0x02, 0x08, 0xB4, 0xE8, 0xF4, 0x24, 0x01, 0x25, 0x4B, 0xF8, 0x40, 0x03, 0x02, + 0x08, 0xB4, 0x78, 0xFF, 0x88, 0x24, 0x22, 0x78, 0xFF, 0xC2, 0x64, 0x20, 0x69, 0x3D, 0x20, 0x6A, + 0x12, 0xD2, 0x64, 0xC3, 0xE5, 0x41, 0x94, 0x0A, 0x40, 0x09, 0xC3, 0xE5, 0x25, 0x94, 0x40, 0x50, + 0x02, 0xC2, 0x64, 0x79, 0xA1, 0xE7, 0x60, 0x23, 0xE5, 0x41, 0x60, 0x1F, 0x74, 0xFF, 0x85, 0x41, + 0xF0, 0x84, 0x85, 0x39, 0xF0, 0x30, 0x6A, 0x03, 0x75, 0xF0, 0x05, 0xA4, 0xF8, 0xC5, 0xF0, 0x60, + 0x02, 0x78, 0xFF, 0xC3, 0xE8, 0x95, 0x64, 0x50, 0x02, 0xA8, 0x64, 0x88, 0x63, 0x22, 0xC3, 0xE5, + 0x40, 0x94, 0xC8, 0xE5, 0x41, 0x94, 0x00, 0xE5, 0x63, 0x50, 0x03, 0x14, 0x21, 0x0F, 0x04, 0x60, + 0x02, 0xF5, 0x63, 0x22, 0x02, 0x09, 0x17, 0x22, 0x75, 0xE8, 0x90, 0x22, 0x78, 0x83, 0xE6, 0xFF, + 0xE5, 0xE8, 0x20, 0xEC, 0xF7, 0xA8, 0xBD, 0xA9, 0xBE, 0x05, 0x70, 0xC3, 0xE5, 0x70, 0x94, 0x08, + 0x40, 0x52, 0x75, 0x70, 0x00, 0xE9, 0xFA, 0x79, 0xA0, 0xE7, 0x60, 0x44, 0xEA, 0x70, 0x07, 0xE5, + 0x71, 0x60, 0x1B, 0x02, 0x09, 0x52, 0xC3, 0xE8, 0x95, 0x71, 0x60, 0x10, 0xE5, 0x71, 0x50, 0x06, + 0x60, 0x0C, 0x14, 0x02, 0x09, 0x5E, 0x04, 0x60, 0xF9, 0x02, 0x09, 0x5E, 0xE5, 0x71, 0xF5, 0x71, + 0xC3, 0x94, 0x72, 0x40, 0x1B, 0x75, 0x61, 0xC0, 0xC3, 0x94, 0x04, 0x40, 0x13, 0x75, 0x61, 0x80, + 0xC3, 0x94, 0x04, 0x40, 0x0B, 0x75, 0x61, 0x40, 0xC3, 0x94, 0x04, 0x40, 0x03, 0x75, 0x61, 0x00, + 0x75, 0xBB, 0x09, 0x22, 0xE5, 0x61, 0x24, 0x10, 0x50, 0x02, 0x74, 0xFF, 0xF5, 0x61, 0x79, 0x82, + 0xB7, 0x04, 0x02, 0x21, 0xAB, 0xC3, 0xA8, 0x61, 0xE5, 0x24, 0x98, 0x50, 0x02, 0xA8, 0x24, 0xC3, + 0xE8, 0x95, 0x63, 0x40, 0x02, 0xA8, 0x63, 0x88, 0x25, 0x88, 0x26, 0xE5, 0x70, 0xB4, 0x07, 0x03, + 0x75, 0xBB, 0x10, 0x22, 0x74, 0x32, 0x79, 0xA7, 0x87, 0xF0, 0xA4, 0xC5, 0xF0, 0xA2, 0xF7, 0x33, + 0xF8, 0xC3, 0xE8, 0x95, 0x61, 0x40, 0x02, 0xA8, 0x61, 0x88, 0x22, 0x88, 0x24, 0x88, 0x25, 0x88, + 0x26, 0x88, 0x64, 0x22, 0x75, 0x40, 0x00, 0x75, 0x41, 0xF0, 0x22, 0xC2, 0xAF, 0x75, 0xC8, 0x20, + 0xA8, 0xCC, 0xA9, 0xCD, 0xAA, 0x3A, 0x30, 0xCF, 0x01, 0x0A, 0x75, 0xC8, 0x24, 0xD2, 0xAF, 0xC3, + 0xEA, 0x13, 0xFA, 0xE9, 0x13, 0xF9, 0xE8, 0x13, 0xF8, 0xAB, 0x3B, 0xAC, 0x3C, 0x88, 0x3B, 0x89, + 0x3C, 0xC3, 0xE8, 0x9B, 0xF8, 0xE9, 0x9C, 0x20, 0x69, 0x0A, 0x54, 0x7F, 0xF9, 0x30, 0x67, 0x02, + 0x41, 0xF5, 0x41, 0x52, 0xAD, 0x3D, 0x8A, 0x3D, 0xF9, 0xEA, 0x9D, 0x54, 0x7F, 0xFA, 0x60, 0x04, + 0x78, 0xFF, 0x79, 0xFF, 0xAE, 0x3E, 0xAF, 0x3F, 0x8B, 0x3E, 0x8C, 0x3F, 0xA8, 0x3B, 0xA9, 0x3C, + 0xC3, 0xE8, 0x9E, 0xF8, 0xE9, 0x9F, 0xF9, 0xC3, 0xE5, 0x41, 0x13, 0xFB, 0xE5, 0x40, 0x13, 0xFA, + 0xE8, 0x2A, 0xF5, 0x40, 0xE9, 0x3B, 0xF5, 0x41, 0x50, 0x06, 0x75, 0x40, 0xFF, 0x75, 0x41, 0xFF, + 0x41, 0x9C, 0xAA, 0x40, 0xAB, 0x41, 0xAC, 0x40, 0xAD, 0x41, 0x7E, 0x04, 0x7F, 0x02, 0xC3, 0xEB, + 0x94, 0x04, 0x40, 0x02, 0x1E, 0x1F, 0xC3, 0xEB, 0x94, 0x08, 0x40, 0x02, 0x1E, 0x1F, 0xC3, 0xED, + 0x13, 0xFD, 0xEC, 0x13, 0xFC, 0xDE, 0xF7, 0xC3, 0xEA, 0x9C, 0xFA, 0xEB, 0x9D, 0xFB, 0xEF, 0x60, + 0x09, 0xC3, 0xE9, 0x13, 0xF9, 0xE8, 0x13, 0xF8, 0xDF, 0xF7, 0xEA, 0x28, 0xFA, 0xEB, 0x39, 0xFB, + 0x8A, 0x40, 0x8B, 0x41, 0x50, 0x06, 0x7B, 0xFF, 0x8B, 0x40, 0x8B, 0x41, 0xC3, 0xEB, 0x94, 0x02, + 0x50, 0x02, 0xD2, 0x67, 0x30, 0x69, 0x04, 0x7F, 0x03, 0x41, 0xC7, 0x78, 0x92, 0xE6, 0xFF, 0xC3, + 0xE5, 0x37, 0x94, 0x82, 0x40, 0x11, 0x0F, 0xC3, 0xE5, 0x37, 0x94, 0xA0, 0x40, 0x01, 0x0F, 0xC3, + 0xEF, 0x94, 0x06, 0x40, 0x02, 0x7F, 0x05, 0x7E, 0x02, 0xE5, 0x41, 0xC4, 0x54, 0x0F, 0xF9, 0xE5, + 0x41, 0xC4, 0x54, 0xF0, 0xF8, 0xE5, 0x40, 0xC4, 0x54, 0x0F, 0x28, 0xF8, 0xC3, 0xE8, 0x9E, 0xFA, + 0xE9, 0x94, 0x00, 0xFB, 0x40, 0x09, 0xC3, 0xEA, 0x94, 0x02, 0xEB, 0x94, 0x00, 0x50, 0x04, 0x7A, + 0x02, 0xE4, 0xFB, 0x61, 0x41, 0xAA, 0x40, 0xAB, 0x41, 0xEB, 0xC4, 0xFE, 0xEA, 0xC4, 0x54, 0x0F, + 0x4E, 0xFC, 0xC3, 0xEA, 0x9C, 0xFA, 0xEB, 0x94, 0x00, 0xFB, 0xC3, 0xE8, 0x13, 0xC3, 0x13, 0xF8, + 0xEA, 0x28, 0xFA, 0xEB, 0x34, 0x00, 0xFB, 0x8A, 0x40, 0x8B, 0x41, 0xC3, 0xEB, 0x94, 0x02, 0x40, + 0x02, 0xC2, 0x67, 0x78, 0x02, 0xEB, 0xC4, 0xFE, 0x7B, 0x00, 0xEA, 0xC4, 0x54, 0x0F, 0x4E, 0xFA, + 0xC3, 0xEA, 0x98, 0xFA, 0x40, 0x05, 0xC3, 0x94, 0x02, 0x50, 0x02, 0x7A, 0x02, 0x78, 0x92, 0xE6, + 0xFF, 0x30, 0x60, 0x02, 0x61, 0x41, 0x85, 0x51, 0x92, 0x85, 0x52, 0x93, 0xD2, 0x60, 0x43, 0xE6, + 0x80, 0xC3, 0xE4, 0x9A, 0xF8, 0xE4, 0x9B, 0xF9, 0xC3, 0xE8, 0x33, 0xF8, 0xE9, 0x33, 0xF9, 0x30, + 0x67, 0x02, 0x61, 0xD4, 0xE8, 0xFA, 0xE9, 0xFB, 0xD3, 0xE9, 0x13, 0xFD, 0xE8, 0x13, 0xFC, 0x88, + 0x51, 0x89, 0x52, 0xC3, 0xEF, 0x94, 0x03, 0x60, 0x38, 0xEF, 0x20, 0xE0, 0x0D, 0xE8, 0x2C, 0xF8, + 0xE9, 0x3D, 0xF9, 0xEC, 0xFA, 0xED, 0xFB, 0x02, 0x0B, 0x9D, 0xE8, 0x28, 0xF8, 0xE9, 0x39, 0xF9, + 0xC3, 0xE8, 0x24, 0x02, 0xF8, 0xE9, 0x34, 0x00, 0xF9, 0x7A, 0xFE, 0x7B, 0xFF, 0xC3, 0xEF, 0x94, + 0x03, 0x40, 0x0E, 0x8A, 0x53, 0x8B, 0x54, 0x88, 0x4D, 0x89, 0x4E, 0x8C, 0x4F, 0x8D, 0x50, 0x81, + 0x08, 0x88, 0x53, 0x89, 0x54, 0x8A, 0x4D, 0x8B, 0x4E, 0x8C, 0x4F, 0x8D, 0x50, 0x30, 0x69, 0x12, + 0x75, 0x53, 0xF0, 0x75, 0x54, 0xFF, 0x75, 0x4F, 0xF0, 0x75, 0x50, 0xFF, 0x75, 0x51, 0xF0, 0x75, + 0x52, 0xFF, 0x81, 0x08, 0xE8, 0xFA, 0xD3, 0xE8, 0x13, 0xFC, 0x88, 0x51, 0xC3, 0xEF, 0x94, 0x03, + 0x60, 0x20, 0xEF, 0x20, 0xE0, 0x07, 0xE8, 0x2C, 0xF8, 0xEC, 0xFA, 0x61, 0xF4, 0xE8, 0x28, 0x24, + 0x02, 0xF8, 0x7A, 0xFE, 0xC3, 0xEF, 0x94, 0x03, 0x40, 0x08, 0x8A, 0x53, 0x88, 0x4D, 0x8C, 0x4F, + 0x81, 0x08, 0x88, 0x53, 0x8A, 0x4D, 0x8C, 0x4F, 0xE5, 0x68, 0xC3, 0x33, 0x50, 0x02, 0x64, 0x6B, + 0xF5, 0x68, 0x30, 0x60, 0x02, 0x81, 0x12, 0x75, 0x34, 0x02, 0xD2, 0x60, 0x43, 0xE6, 0x80, 0xE5, + 0x2D, 0x54, 0x06, 0x60, 0x2D, 0xA8, 0x40, 0xA9, 0x41, 0xC3, 0xE9, 0x13, 0xF9, 0xE8, 0x13, 0xF8, + 0x30, 0x69, 0x04, 0xE9, 0x24, 0x40, 0xF9, 0xC2, 0xAF, 0x53, 0xE6, 0x7F, 0x75, 0x91, 0x00, 0xC3, + 0xE4, 0x98, 0xF5, 0x94, 0xE4, 0x99, 0xF5, 0x95, 0x75, 0x91, 0x04, 0xD2, 0x60, 0x43, 0xE6, 0x80, + 0xD2, 0xAF, 0x22, 0xD2, 0x65, 0x75, 0x43, 0x00, 0x75, 0x20, 0x00, 0x30, 0x6C, 0x03, 0x75, 0x20, + 0x40, 0x02, 0x0C, 0x72, 0xD2, 0x65, 0x75, 0x43, 0x00, 0x75, 0x20, 0x40, 0x30, 0x6C, 0x03, 0x75, + 0x20, 0x00, 0x78, 0x01, 0x79, 0x01, 0x20, 0x67, 0x20, 0xE5, 0x2D, 0x54, 0x06, 0x60, 0x02, 0xC2, + 0x65, 0x79, 0x14, 0xE5, 0x41, 0xC3, 0x13, 0x70, 0x01, 0x04, 0xF8, 0xC3, 0x94, 0x14, 0x40, 0x02, + 0x78, 0x14, 0x30, 0x69, 0x04, 0x78, 0x1B, 0x79, 0x1B, 0xC3, 0xE8, 0x33, 0xF8, 0xC3, 0xE9, 0x33, + 0xF9, 0x20, 0x60, 0x10, 0xE5, 0x43, 0x60, 0x0C, 0x30, 0x69, 0x03, 0xD5, 0x34, 0x04, 0xD2, 0x6D, + 0xA1, 0x2A, 0x91, 0x1A, 0x05, 0x43, 0xE5, 0x9B, 0xF4, 0x54, 0x40, 0xB5, 0x20, 0x02, 0xA1, 0x14, + 0x30, 0x69, 0x09, 0x08, 0xC3, 0xE8, 0x99, 0x40, 0x01, 0x18, 0x81, 0xA1, 0x20, 0x65, 0x0A, 0x08, + 0xC3, 0xE8, 0x99, 0x40, 0x02, 0x81, 0x72, 0x81, 0xA1, 0xC2, 0x65, 0xC2, 0xAF, 0x53, 0xE6, 0x7F, + 0x75, 0x91, 0x00, 0x30, 0x67, 0x12, 0x75, 0x94, 0x00, 0x75, 0x95, 0xFC, 0x75, 0x91, 0x04, 0xD2, + 0x60, 0x43, 0xE6, 0x80, 0xD2, 0xAF, 0x81, 0x72, 0xAE, 0x40, 0xAF, 0x41, 0xC3, 0xEE, 0x33, 0xFE, + 0xEF, 0x33, 0xFF, 0x50, 0x04, 0x7E, 0xFF, 0x7F, 0xFF, 0xC3, 0xE4, 0x9E, 0xF5, 0x94, 0xE4, 0x9F, + 0xF5, 0x95, 0x81, 0xEC, 0xC3, 0xE5, 0x33, 0x94, 0x01, 0x50, 0x02, 0x81, 0x72, 0x30, 0x65, 0x02, + 0x81, 0x72, 0xD8, 0x02, 0xA1, 0x28, 0x81, 0xA1, 0xC2, 0x6D, 0xC2, 0xAF, 0x53, 0xE6, 0x7F, 0x75, + 0x91, 0x00, 0x85, 0x53, 0x94, 0x85, 0x54, 0x95, 0x75, 0x91, 0x04, 0x85, 0x4D, 0x92, 0x85, 0x4E, + 0x93, 0xD2, 0x60, 0x43, 0xE6, 0x80, 0xD2, 0xAF, 0xE5, 0x2D, 0x54, 0x06, 0x60, 0x08, 0x20, 0x6A, + 0x02, 0x05, 0x33, 0x02, 0x0D, 0x66, 0x30, 0x6D, 0x0D, 0x20, 0x6C, 0x0A, 0x20, 0x65, 0x07, 0x15, + 0x81, 0x15, 0x81, 0x02, 0x16, 0x03, 0x22, 0x78, 0x00, 0x30, 0x64, 0x05, 0x30, 0x65, 0x02, 0x78, + 0x01, 0xE5, 0x37, 0x75, 0xF0, 0x07, 0xA4, 0xF9, 0xE5, 0xF0, 0x28, 0xF5, 0xF0, 0xE9, 0xA2, 0xF0, + 0x13, 0xA2, 0xF1, 0x13, 0xA2, 0xF2, 0x13, 0xF5, 0x37, 0xC3, 0x94, 0x78, 0x50, 0x03, 0x75, 0x37, + 0x78, 0xC3, 0xE5, 0x37, 0x95, 0x38, 0x40, 0x08, 0xD2, 0x66, 0xC2, 0x93, 0xC2, 0x97, 0xC2, 0x94, + 0x30, 0x60, 0x02, 0xA1, 0xA0, 0x85, 0x4F, 0x92, 0x85, 0x50, 0x93, 0xD2, 0x60, 0x43, 0xE6, 0x80, + 0x22, 0x20, 0x7D, 0x16, 0xC2, 0xAF, 0x75, 0x42, 0x02, 0xC2, 0x95, 0xD2, 0x92, 0x30, 0x62, 0x02, + 0xD2, 0x97, 0xD2, 0xAF, 0x75, 0x9F, 0x80, 0x02, 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, 0x02, 0xC2, + 0x95, 0xD2, 0x96, 0x30, 0x62, 0x02, 0xD2, 0x93, 0xD2, 0xAF, 0x75, 0x9F, 0x80, 0x02, 0x0F, 0xA0, + 0x30, 0x72, 0x43, 0x20, 0x7D, 0x20, 0xC2, 0xAF, 0x75, 0x42, 0x03, 0x90, 0x01, 0x65, 0x75, 0x7B, + 0x20, 0xC2, 0x97, 0xC2, 0x96, 0x30, 0x62, 0x04, 0xD2, 0x94, 0xA1, 0xFE, 0xD2, 0x95, 0xD2, 0xAF, + 0x75, 0x9F, 0x89, 0x02, 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, 0x03, 0x90, 0x01, 0x65, 0x75, 0x7B, + 0x20, 0xC2, 0x93, 0xC2, 0x92, 0x30, 0x62, 0x04, 0xD2, 0x94, 0xC1, 0x1E, 0xD2, 0x95, 0xD2, 0xAF, + 0x75, 0x9F, 0x81, 0x02, 0x0F, 0xA0, 0x20, 0x7D, 0x17, 0xC2, 0xAF, 0x75, 0x42, 0x03, 0x90, 0x01, + 0x40, 0xC2, 0x97, 0x30, 0x62, 0x02, 0xD2, 0x94, 0xD2, 0xAF, 0x75, 0x9F, 0x89, 0x02, 0x0F, 0xA0, + 0xC2, 0xAF, 0x75, 0x42, 0x03, 0x90, 0x01, 0x40, 0xC2, 0x93, 0x30, 0x62, 0x02, 0xD2, 0x94, 0xD2, + 0xAF, 0x75, 0x9F, 0x81, 0x02, 0x0F, 0xA0, 0x20, 0x7D, 0x16, 0xC2, 0xAF, 0x75, 0x42, 0x04, 0xC2, + 0x92, 0xD2, 0x96, 0x30, 0x62, 0x02, 0xD2, 0x94, 0xD2, 0xAF, 0x75, 0x9F, 0x81, 0x02, 0x0F, 0xA0, + 0xC2, 0xAF, 0x75, 0x42, 0x04, 0xC2, 0x96, 0xD2, 0x92, 0x30, 0x62, 0x02, 0xD2, 0x94, 0xD2, 0xAF, + 0x75, 0x9F, 0x89, 0x02, 0x0F, 0xA0, 0x30, 0x72, 0x43, 0x20, 0x7D, 0x20, 0xC2, 0xAF, 0x75, 0x42, + 0x05, 0x90, 0x01, 0x54, 0x75, 0x7B, 0x04, 0xC2, 0x94, 0xC2, 0x95, 0x30, 0x62, 0x04, 0xD2, 0x93, + 0xC1, 0xA4, 0xD2, 0x92, 0xD2, 0xAF, 0x75, 0x9F, 0x80, 0x02, 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, + 0x05, 0x90, 0x01, 0x76, 0x75, 0x7B, 0x40, 0xC2, 0x94, 0xC2, 0x95, 0x30, 0x62, 0x04, 0xD2, 0x97, + 0xC1, 0xC4, 0xD2, 0x96, 0xD2, 0xAF, 0x75, 0x9F, 0x80, 0x02, 0x0F, 0xA0, 0x20, 0x7D, 0x17, 0xC2, + 0xAF, 0x75, 0x42, 0x05, 0x90, 0x01, 0x36, 0xC2, 0x94, 0x30, 0x62, 0x02, 0xD2, 0x93, 0xD2, 0xAF, + 0x75, 0x9F, 0x80, 0x02, 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, 0x05, 0x90, 0x01, 0x4A, 0xC2, 0x94, + 0x30, 0x62, 0xEB, 0xD2, 0x97, 0xD2, 0xAF, 0x75, 0x9F, 0x80, 0x02, 0x0F, 0xA0, 0x20, 0x7D, 0x16, + 0xC2, 0xAF, 0x75, 0x42, 0x06, 0xC2, 0x96, 0xD2, 0x95, 0x30, 0x62, 0x02, 0xD2, 0x93, 0xD2, 0xAF, + 0x75, 0x9F, 0x89, 0x02, 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, 0x06, 0xC2, 0x92, 0xD2, 0x95, 0x30, + 0x62, 0x02, 0xD2, 0x97, 0xD2, 0xAF, 0x75, 0x9F, 0x81, 0x02, 0x0F, 0xA0, 0x30, 0x72, 0x43, 0x20, + 0x7D, 0x20, 0xC2, 0xAF, 0x75, 0x42, 0x01, 0x90, 0x01, 0x76, 0x75, 0x7B, 0x40, 0xC2, 0x93, 0xC2, + 0x92, 0x30, 0x62, 0x04, 0xD2, 0x97, 0xE1, 0x4A, 0xD2, 0x96, 0xD2, 0xAF, 0x75, 0x9F, 0x81, 0x02, + 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, 0x01, 0x90, 0x01, 0x54, 0x75, 0x7B, 0x04, 0xC2, 0x97, 0xC2, + 0x96, 0x30, 0x62, 0x04, 0xD2, 0x93, 0xE1, 0x6A, 0xD2, 0x92, 0xD2, 0xAF, 0x75, 0x9F, 0x89, 0x02, + 0x0F, 0xA0, 0x20, 0x7D, 0x17, 0xC2, 0xAF, 0x75, 0x42, 0x01, 0x90, 0x01, 0x4A, 0xC2, 0x93, 0x30, + 0x62, 0x02, 0xD2, 0x97, 0xD2, 0xAF, 0x75, 0x9F, 0x81, 0x02, 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, + 0x01, 0x90, 0x01, 0x36, 0xC2, 0x97, 0x30, 0x62, 0x02, 0xD2, 0x93, 0xD2, 0xAF, 0x75, 0x9F, 0x89, + 0xC2, 0x66, 0x22, 0x90, 0x01, 0x34, 0x75, 0x7B, 0x00, 0xC2, 0x93, 0xC2, 0x97, 0xC2, 0x94, 0xC2, + 0x92, 0xC2, 0x96, 0xC2, 0x95, 0xC2, 0x62, 0x22, 0x78, 0x80, 0x76, 0x09, 0x08, 0x76, 0x09, 0x08, + 0x76, 0x04, 0x08, 0x76, 0xFF, 0x08, 0x76, 0x03, 0x08, 0x76, 0xFF, 0x08, 0x76, 0x09, 0x08, 0x76, + 0x03, 0x08, 0x76, 0x01, 0x08, 0x76, 0x01, 0x78, 0x8C, 0x76, 0x01, 0x08, 0x76, 0xFF, 0x08, 0x76, + 0xFF, 0x08, 0x76, 0xFF, 0x08, 0x76, 0xFF, 0x08, 0x76, 0xFF, 0x08, 0x76, 0x03, 0x08, 0x76, 0xFF, + 0x08, 0x76, 0xFF, 0x08, 0x76, 0xFF, 0x08, 0x76, 0x25, 0x08, 0x76, 0xD0, 0x08, 0x76, 0x28, 0x08, + 0x76, 0x50, 0x08, 0x76, 0x04, 0x08, 0x76, 0xFF, 0x08, 0x76, 0x02, 0x08, 0x76, 0x00, 0x08, 0x76, + 0x7A, 0x08, 0x76, 0xFF, 0x08, 0x76, 0x01, 0x08, 0x76, 0x01, 0x08, 0x76, 0x00, 0x08, 0x76, 0x03, + 0x08, 0x76, 0x00, 0x22, 0x78, 0x87, 0xE6, 0xFF, 0xC2, 0x72, 0xBF, 0x03, 0x02, 0xD2, 0x72, 0x78, + 0x88, 0xE6, 0xC3, 0x94, 0x03, 0x60, 0x08, 0xC2, 0x7D, 0xE6, 0x30, 0xE1, 0x02, 0xD2, 0x7D, 0xC2, + 0x7E, 0x78, 0x89, 0xE6, 0x30, 0xE1, 0x02, 0xD2, 0x7E, 0xC3, 0xEF, 0x94, 0x02, 0x60, 0x08, 0x75, + 0x8E, 0x01, 0xD2, 0x73, 0x02, 0x10, 0x5C, 0x75, 0x8E, 0x00, 0xC2, 0x73, 0x22, 0x78, 0x80, 0xE6, + 0x14, 0x90, 0x00, 0x80, 0x93, 0x78, 0xA5, 0xF6, 0x78, 0x81, 0xE6, 0x14, 0x90, 0x00, 0x80, 0x93, + 0x78, 0xA6, 0xF6, 0x78, 0x86, 0xE6, 0x14, 0x90, 0x00, 0x8D, 0x93, 0x78, 0xA7, 0xF6, 0x78, 0x86, + 0xE6, 0xF5, 0x39, 0xC3, 0x94, 0x02, 0x50, 0x03, 0x75, 0x39, 0x02, 0x78, 0x9C, 0xE6, 0x75, 0x38, + 0xFF, 0xB4, 0x02, 0x03, 0x75, 0x38, 0xA0, 0xB4, 0x03, 0x03, 0x75, 0x38, 0x82, 0x78, 0xA3, 0xE6, + 0x14, 0x90, 0x00, 0x9A, 0x93, 0xF5, 0x66, 0x12, 0x0F, 0xA3, 0x22, 0x22, 0x78, 0x96, 0xE6, 0xFA, + 0x78, 0x97, 0xE6, 0xFB, 0x78, 0x88, 0xE6, 0xB4, 0x03, 0x05, 0xC3, 0xEB, 0x94, 0x0E, 0xFB, 0x30, + 0x7F, 0x04, 0x7A, 0x00, 0x7B, 0xFF, 0xC3, 0xEB, 0x9A, 0xFC, 0xC3, 0x94, 0x82, 0x50, 0x02, 0x7C, + 0x82, 0x75, 0x72, 0x00, 0x05, 0x72, 0xEC, 0x85, 0x72, 0xF0, 0xA4, 0xC3, 0xE5, 0xF0, 0x94, 0x7D, + 0x40, 0xF2, 0x22, 0xD2, 0x7F, 0x11, 0xAC, 0x12, 0x06, 0x04, 0x7A, 0x00, 0x7B, 0x00, 0x7C, 0x10, + 0x12, 0x05, 0xFA, 0xE5, 0x5C, 0x2A, 0xFA, 0x74, 0x00, 0x3B, 0xFB, 0xDC, 0xF3, 0x7C, 0x04, 0xC3, + 0xEB, 0x13, 0xFB, 0xEA, 0x13, 0xFA, 0xDC, 0xF7, 0xFE, 0xC2, 0x7F, 0x11, 0xAC, 0x22, 0xE5, 0xEF, + 0x20, 0xE6, 0x03, 0x75, 0x20, 0x00, 0x05, 0x20, 0x90, 0x3F, 0xFF, 0xE5, 0x20, 0x14, 0x60, 0x06, + 0x90, 0x1F, 0xFF, 0x14, 0x60, 0x00, 0x93, 0x04, 0x60, 0x03, 0x75, 0xEF, 0x12, 0x53, 0xD9, 0xBF, + 0x75, 0x81, 0xC0, 0x43, 0xFF, 0x80, 0x12, 0x05, 0xF5, 0x75, 0xEF, 0x02, 0x43, 0xB2, 0x03, 0xE5, + 0xB3, 0x24, 0x02, 0x20, 0xE7, 0x0D, 0xF5, 0x21, 0xE5, 0xE3, 0x20, 0xE0, 0x06, 0x85, 0x21, 0xB3, + 0x43, 0xE3, 0x01, 0x12, 0x0F, 0xA3, 0x75, 0x80, 0xFF, 0x75, 0xA4, 0x00, 0x75, 0xF1, 0xF0, 0x75, + 0xD4, 0xDF, 0x75, 0x90, 0x02, 0x75, 0xA5, 0xFC, 0x75, 0xF2, 0xFD, 0x75, 0xD5, 0x02, 0x75, 0xA6, + 0x10, 0x75, 0xA0, 0xFF, 0x75, 0xF3, 0xF1, 0x75, 0xE2, 0x41, 0x12, 0x0F, 0xA3, 0xE4, 0xF8, 0xF6, + 0xD8, 0xFD, 0x75, 0x68, 0x01, 0x12, 0x0F, 0xB8, 0x12, 0x16, 0x5C, 0x78, 0x98, 0x86, 0x73, 0x75, + 0x30, 0x01, 0xC2, 0xAF, 0x12, 0x06, 0x0E, 0x12, 0x06, 0x1E, 0x12, 0x06, 0x04, 0x12, 0x06, 0x25, + 0x12, 0x06, 0x04, 0x12, 0x06, 0x2C, 0x12, 0x06, 0x04, 0xC2, 0xAF, 0x78, 0xFA, 0x79, 0xFA, 0x30, + 0x85, 0x07, 0xD9, 0xFB, 0xD8, 0xF7, 0x02, 0x1C, 0x00, 0x11, 0x24, 0x11, 0x5D, 0x11, 0xAB, 0x11, + 0xAC, 0x78, 0x98, 0x86, 0x73, 0x12, 0x0F, 0xA3, 0x75, 0xA9, 0x00, 0x75, 0xB6, 0x80, 0x75, 0x7A, + 0x00, 0x75, 0x88, 0x10, 0x75, 0x89, 0x02, 0x75, 0xC8, 0x24, 0x75, 0x91, 0x04, 0x75, 0xD8, 0x40, + 0x75, 0xA8, 0x22, 0x75, 0xB8, 0x02, 0x75, 0xE6, 0x90, 0x75, 0x9B, 0x80, 0x75, 0x9D, 0x00, 0x75, + 0xD1, 0x0E, 0x75, 0xBC, 0xC0, 0x75, 0xBB, 0x09, 0x75, 0xBA, 0x11, 0x75, 0xE8, 0x80, 0x12, 0x05, + 0xF5, 0xD2, 0xAF, 0x12, 0x09, 0x14, 0x75, 0x36, 0x00, 0x53, 0xDA, 0xCF, 0x20, 0x7E, 0x03, 0x43, + 0xDA, 0x20, 0x30, 0x7E, 0x03, 0x43, 0xDA, 0x10, 0x43, 0xDA, 0x01, 0xC2, 0xD8, 0xC2, 0x71, 0x12, + 0x06, 0x0E, 0xD2, 0x61, 0x7B, 0x03, 0x7A, 0x0C, 0xE5, 0x5B, 0x70, 0x07, 0x7A, 0x0C, 0xDB, 0x03, + 0x02, 0x11, 0xA9, 0x12, 0x06, 0x04, 0x20, 0x70, 0x03, 0x02, 0x11, 0xA9, 0xC2, 0x70, 0xE5, 0x5C, + 0xC3, 0x94, 0x02, 0x40, 0xE1, 0xE5, 0x2F, 0x54, 0x1F, 0x85, 0x5E, 0x5D, 0xF5, 0x5E, 0xB5, 0x5D, + 0xD5, 0xDA, 0xD5, 0xC2, 0x61, 0x53, 0xDA, 0xCF, 0x20, 0x7E, 0x03, 0x43, 0xDA, 0x20, 0x30, 0x7E, + 0x03, 0x43, 0xDA, 0x10, 0xC2, 0xD8, 0xC2, 0x71, 0x78, 0xA2, 0xE6, 0x70, 0x08, 0xD2, 0x74, 0xE5, + 0x2F, 0x54, 0xE0, 0xF5, 0x2F, 0xC2, 0x75, 0x75, 0x29, 0x00, 0x12, 0x06, 0x09, 0x30, 0x74, 0x09, + 0xC3, 0xE5, 0x29, 0x94, 0x0A, 0x40, 0x02, 0xD2, 0x75, 0x12, 0x05, 0xFA, 0x78, 0x02, 0x30, 0x74, + 0x02, 0x78, 0x00, 0xC3, 0xE5, 0x5C, 0x98, 0x40, 0xF0, 0xC2, 0xAF, 0x12, 0x06, 0x1E, 0x12, 0x06, + 0x1E, 0x12, 0x06, 0x1E, 0xD2, 0xAF, 0x12, 0x06, 0x0E, 0x75, 0x4C, 0x00, 0x78, 0x88, 0xE6, 0xB4, + 0x03, 0x02, 0x61, 0x88, 0x12, 0x05, 0xFA, 0x78, 0x8C, 0xE6, 0xC3, 0x94, 0x01, 0x50, 0x03, 0x02, + 0x13, 0x88, 0xE5, 0x30, 0xC3, 0x94, 0x01, 0x50, 0x03, 0x02, 0x13, 0x88, 0x20, 0x74, 0x35, 0xC3, + 0xE5, 0x5C, 0x94, 0xFF, 0x50, 0x03, 0x02, 0x13, 0x88, 0xC2, 0xAF, 0x12, 0x06, 0x33, 0xD2, 0xAF, + 0x12, 0x06, 0x09, 0xC3, 0xE5, 0x5C, 0x94, 0x01, 0x50, 0xEF, 0xC2, 0xAF, 0x12, 0x06, 0x1E, 0x12, + 0x05, 0xFF, 0x12, 0x06, 0x1E, 0xD2, 0xAF, 0x12, 0x06, 0x09, 0xC3, 0xE5, 0x5C, 0x94, 0xFF, 0x40, + 0xE9, 0x02, 0x18, 0x04, 0x7F, 0x02, 0xD2, 0x7F, 0x11, 0xAC, 0x12, 0x06, 0x09, 0xC2, 0xAF, 0xC2, + 0x7F, 0x11, 0xAC, 0xAE, 0x5C, 0xC3, 0xE5, 0x5C, 0x94, 0x7F, 0xD2, 0xAF, 0x40, 0x74, 0x12, 0x05, + 0xF5, 0xC2, 0xAF, 0x12, 0x06, 0x33, 0xD2, 0xAF, 0xDF, 0xDC, 0x11, 0xE3, 0xC3, 0xEE, 0x78, 0x97, + 0xF6, 0x12, 0x06, 0x0E, 0x12, 0x16, 0x9E, 0x12, 0x17, 0x87, 0x7F, 0x0A, 0xD2, 0x7F, 0x11, 0xAC, + 0x12, 0x06, 0x09, 0xC2, 0xAF, 0xC2, 0x7F, 0x11, 0xAC, 0xAE, 0x5C, 0xC3, 0xE5, 0x5C, 0x94, 0x7F, + 0xD2, 0xAF, 0x50, 0xE6, 0x12, 0x05, 0xF5, 0xC2, 0xAF, 0x12, 0x06, 0x1E, 0x12, 0x05, 0xFF, 0x12, + 0x06, 0x1E, 0xD2, 0xAF, 0xDF, 0xD6, 0x11, 0xE3, 0xEE, 0x24, 0x03, 0x78, 0x96, 0xF6, 0x12, 0x06, + 0x0E, 0x12, 0x16, 0x9E, 0x12, 0x17, 0xB6, 0x12, 0x06, 0x09, 0x11, 0xAC, 0xC3, 0xE5, 0x5C, 0x94, + 0xFF, 0x50, 0x02, 0x41, 0xCC, 0x02, 0x18, 0x04, 0xC3, 0xE5, 0x5C, 0x95, 0x4C, 0x40, 0x03, 0x85, + 0x5C, 0x4C, 0x12, 0x06, 0x09, 0x78, 0x01, 0x79, 0x88, 0xE7, 0xB4, 0x03, 0x02, 0x78, 0x05, 0xC3, + 0xE5, 0x5C, 0x98, 0x40, 0x02, 0x41, 0xAC, 0xC2, 0xAF, 0x12, 0x06, 0x33, 0x12, 0x06, 0x33, 0x12, + 0x06, 0x33, 0xD2, 0xAF, 0x12, 0x06, 0x0E, 0x75, 0x30, 0x00, 0xE4, 0xF5, 0x31, 0xF5, 0x32, 0x05, + 0x31, 0xE5, 0x31, 0xF4, 0x70, 0x3F, 0x05, 0x32, 0x78, 0x9A, 0xE6, 0x78, 0x19, 0x14, 0x60, 0x12, + 0x78, 0x32, 0x14, 0x60, 0x0D, 0x78, 0x7D, 0x14, 0x60, 0x08, 0x78, 0xFA, 0x14, 0x60, 0x03, 0x75, + 0x32, 0x00, 0xC3, 0xE5, 0x32, 0x98, 0x40, 0x1D, 0x12, 0x0F, 0xA3, 0x12, 0x05, 0xF5, 0x15, 0x32, + 0x75, 0x31, 0x00, 0x78, 0x99, 0x86, 0x73, 0xC2, 0xAF, 0x12, 0x06, 0x33, 0xD2, 0xAF, 0x78, 0x98, + 0x86, 0x73, 0x12, 0x06, 0x09, 0x12, 0x05, 0xFF, 0xE5, 0x2A, 0x70, 0x05, 0x30, 0x74, 0x02, 0x21, + 0xA9, 0x78, 0x01, 0x20, 0x74, 0x02, 0x78, 0x06, 0xC3, 0xE5, 0x5C, 0x98, 0x40, 0xA1, 0x78, 0x88, + 0xE6, 0xC3, 0x94, 0x03, 0x60, 0x03, 0x12, 0x06, 0x09, 0xE5, 0x2A, 0x70, 0x03, 0x02, 0x11, 0xA9, + 0xC2, 0xAF, 0x12, 0x0F, 0xA3, 0xE4, 0xF5, 0x22, 0xF5, 0x23, 0xF5, 0x24, 0xF5, 0x25, 0xF5, 0x26, + 0xF5, 0x67, 0xD2, 0xAF, 0x78, 0x85, 0xE6, 0xC3, 0x33, 0xF5, 0x65, 0xE4, 0xF5, 0x44, 0xF5, 0x45, + 0xF5, 0x46, 0xF5, 0x47, 0xF5, 0x48, 0xF5, 0x70, 0xF5, 0x2C, 0xF5, 0x2D, 0xF5, 0x37, 0x75, 0x70, + 0x08, 0x75, 0xBB, 0x10, 0x12, 0x05, 0xF5, 0x12, 0x09, 0x18, 0xE5, 0xE8, 0x20, 0xEC, 0xFB, 0xA8, + 0xBD, 0xA9, 0xBE, 0xE9, 0x70, 0x01, 0xF8, 0x88, 0x71, 0x12, 0x09, 0x1C, 0x75, 0x70, 0x08, 0x75, + 0xBB, 0x10, 0x11, 0x24, 0xC2, 0xAF, 0x75, 0x61, 0xFF, 0x12, 0x09, 0xB4, 0x85, 0x22, 0x61, 0x85, + 0x22, 0x62, 0x85, 0x22, 0x63, 0xD2, 0xAF, 0x75, 0x22, 0x01, 0x75, 0x24, 0x01, 0x75, 0x25, 0x01, + 0x75, 0x26, 0x01, 0x85, 0x60, 0x69, 0x75, 0x6A, 0x01, 0x75, 0xB6, 0x90, 0x75, 0xA9, 0x03, 0x75, + 0x7A, 0x01, 0x78, 0x88, 0xE6, 0xB4, 0x03, 0x07, 0xC2, 0x7D, 0x30, 0x76, 0x02, 0xD2, 0x7D, 0xD2, + 0x68, 0xD2, 0x69, 0x75, 0x33, 0x00, 0x12, 0x0E, 0xFD, 0x12, 0x0F, 0x2C, 0x12, 0x09, 0xD4, 0x12, + 0x09, 0xDB, 0x12, 0x09, 0xD4, 0x12, 0x09, 0xDB, 0x12, 0x09, 0xD4, 0x12, 0x0C, 0x64, 0x12, 0x07, + 0x0E, 0x12, 0x0D, 0x67, 0x12, 0x0D, 0xB1, 0x12, 0x09, 0xDB, 0x12, 0x0C, 0x53, 0x30, 0x6E, 0x03, + 0x12, 0x07, 0x6A, 0x20, 0x67, 0x03, 0x12, 0x08, 0xB7, 0x30, 0x67, 0x03, 0x12, 0x08, 0xFE, 0x12, + 0x0D, 0x67, 0x12, 0x0D, 0xE0, 0x12, 0x09, 0xDB, 0x12, 0x0C, 0x64, 0x30, 0x6E, 0x03, 0x12, 0x07, + 0x9F, 0x12, 0x0D, 0x67, 0x12, 0x0E, 0x57, 0x12, 0x09, 0xDB, 0x12, 0x0C, 0x53, 0x30, 0x6E, 0x03, + 0x12, 0x07, 0xFD, 0x12, 0x0D, 0x67, 0x12, 0x0E, 0x86, 0x12, 0x09, 0xDB, 0x12, 0x0C, 0x64, 0x30, + 0x6E, 0x03, 0x12, 0x08, 0x5B, 0x12, 0x0D, 0x67, 0x12, 0x0E, 0xFD, 0x12, 0x09, 0xDB, 0x12, 0x09, + 0x18, 0x12, 0x0C, 0x53, 0x12, 0x0D, 0x67, 0x12, 0x0F, 0x2C, 0x12, 0x09, 0x1C, 0x12, 0x09, 0xDB, + 0x30, 0x69, 0x32, 0x85, 0x64, 0x61, 0x85, 0x64, 0x62, 0x85, 0x60, 0x69, 0x75, 0x6A, 0x01, 0x79, + 0x18, 0x7A, 0x0C, 0xC3, 0xE5, 0x33, 0x99, 0x40, 0x0F, 0xC2, 0x69, 0xD2, 0x6A, 0x8A, 0x35, 0x85, + 0x64, 0x61, 0x85, 0x64, 0x63, 0x02, 0x15, 0x85, 0xC3, 0xE5, 0x5C, 0x94, 0x01, 0x40, 0x03, 0x02, + 0x14, 0xDB, 0x02, 0x16, 0x0B, 0x30, 0x6A, 0x1D, 0x20, 0x6C, 0x1A, 0xE5, 0x35, 0x14, 0x70, 0x06, + 0xC2, 0x6A, 0xD2, 0x6B, 0x81, 0xDB, 0xF5, 0x35, 0xC3, 0xE5, 0x5C, 0x94, 0x01, 0x40, 0x03, 0x02, + 0x14, 0xDB, 0x02, 0x16, 0x0B, 0x75, 0x36, 0x00, 0x78, 0xFA, 0x79, 0xA4, 0xE7, 0x60, 0x02, 0x78, + 0x03, 0xC3, 0xE5, 0x5F, 0x98, 0x50, 0x54, 0x30, 0x74, 0x04, 0xE5, 0x2A, 0x60, 0x4D, 0x78, 0x88, + 0xE6, 0xB4, 0x03, 0x17, 0x20, 0x7D, 0x05, 0x20, 0x76, 0x07, 0xA1, 0xDB, 0x30, 0x76, 0x02, 0xA1, + 0xDB, 0x20, 0x6C, 0x07, 0xD2, 0x6C, 0x85, 0x64, 0x61, 0xA1, 0x1A, 0x78, 0xF0, 0x30, 0x6C, 0x05, + 0x85, 0x64, 0x61, 0x78, 0x20, 0xC3, 0xE5, 0x41, 0x98, 0x50, 0x02, 0x81, 0xDB, 0x30, 0x6C, 0x1B, + 0xC2, 0x6C, 0xC2, 0x7D, 0x30, 0x76, 0x02, 0xD2, 0x7D, 0xD2, 0x6A, 0x75, 0x35, 0x12, 0x85, 0x64, + 0x61, 0x81, 0xDB, 0x05, 0x36, 0xE5, 0x5C, 0x60, 0x02, 0xC1, 0x0E, 0x75, 0x36, 0x00, 0xC2, 0xAF, + 0x12, 0x0F, 0xA3, 0x78, 0x87, 0xE6, 0xFE, 0x76, 0x02, 0x11, 0x24, 0x78, 0x87, 0xEE, 0xF6, 0xE4, + 0xF5, 0x22, 0xF5, 0x23, 0xF5, 0x24, 0xF5, 0x25, 0xF5, 0x26, 0xF5, 0x65, 0x75, 0x2C, 0x00, 0x75, + 0x2D, 0x00, 0x75, 0xA9, 0x00, 0x75, 0xB6, 0x80, 0x75, 0x7A, 0x00, 0xD2, 0xAF, 0x12, 0x06, 0x09, + 0x12, 0x0F, 0xA3, 0x78, 0xA4, 0xE6, 0x60, 0x06, 0xD2, 0x92, 0xD2, 0x95, 0xD2, 0x96, 0x30, 0x74, + 0x09, 0xC3, 0xE5, 0x36, 0x94, 0x04, 0x40, 0x02, 0x21, 0xA9, 0x61, 0xBA, 0x90, 0x1A, 0x0D, 0x78, + 0x20, 0x12, 0x16, 0xE0, 0xE5, 0x20, 0xB4, 0x55, 0x0C, 0xA3, 0x12, 0x16, 0xE0, 0xE5, 0x20, 0xB4, + 0xAA, 0x03, 0x02, 0x16, 0x7E, 0x12, 0x0F, 0xB8, 0x12, 0x16, 0x9E, 0x02, 0x16, 0x9D, 0x90, 0x1A, + 0x03, 0x78, 0x80, 0x7B, 0x0A, 0x12, 0x16, 0xE0, 0xA3, 0x08, 0xDB, 0xF9, 0x90, 0x1A, 0x0F, 0x78, + 0x8C, 0x7B, 0x19, 0x12, 0x16, 0xE0, 0xA3, 0x08, 0xDB, 0xF9, 0x90, 0x1A, 0x28, 0x22, 0xC2, 0xAF, + 0x12, 0x17, 0x1F, 0x12, 0x16, 0xF9, 0x90, 0x1A, 0x00, 0x74, 0x0E, 0x12, 0x16, 0xE5, 0xA3, 0x74, + 0x06, 0x12, 0x16, 0xE5, 0xA3, 0x74, 0x15, 0x12, 0x16, 0xE5, 0x90, 0x1A, 0x03, 0x78, 0x80, 0x7B, + 0x0A, 0x12, 0x16, 0xE4, 0xA3, 0x08, 0xDB, 0xF9, 0x90, 0x1A, 0x0F, 0x78, 0x8C, 0x7B, 0x19, 0x12, + 0x16, 0xE4, 0xA3, 0x08, 0xDB, 0xF9, 0x12, 0x17, 0x32, 0x12, 0x17, 0x10, 0x90, 0x1A, 0x28, 0x22, + 0xE4, 0x93, 0xF6, 0x22, 0xE6, 0x43, 0x8F, 0x01, 0x53, 0x8F, 0xFD, 0x75, 0xEF, 0x02, 0x75, 0xB7, + 0xA5, 0x75, 0xB7, 0xF1, 0xF0, 0x53, 0x8F, 0xFE, 0x22, 0x43, 0x8F, 0x02, 0x43, 0x8F, 0x01, 0x75, + 0xEF, 0x02, 0x75, 0xB7, 0xA5, 0x75, 0xB7, 0xF1, 0x90, 0x1A, 0x0D, 0xF0, 0x53, 0x8F, 0xFC, 0x22, + 0x90, 0x1A, 0x0D, 0x74, 0x55, 0xD1, 0xE5, 0x90, 0x1A, 0x0E, 0x74, 0xAA, 0xD1, 0xE5, 0x22, 0x7A, + 0x30, 0x79, 0xD0, 0x78, 0x20, 0x90, 0x1A, 0x40, 0xD1, 0xE0, 0xE5, 0x20, 0xF7, 0x09, 0xA3, 0xDA, + 0xF7, 0x22, 0x7A, 0x30, 0x79, 0xD0, 0x90, 0x1A, 0x40, 0xE7, 0xD1, 0xE5, 0x09, 0xA3, 0xDA, 0xF9, + 0x22, 0xAB, 0x74, 0xA8, 0x75, 0xBB, 0x01, 0x02, 0x79, 0x80, 0xBB, 0x02, 0x02, 0x79, 0x81, 0xBB, + 0x03, 0x02, 0x79, 0x82, 0xBB, 0x04, 0x02, 0x79, 0x84, 0xBB, 0x05, 0x02, 0x79, 0x86, 0xBB, 0x06, + 0x02, 0x79, 0x92, 0xBB, 0x07, 0x02, 0x79, 0x87, 0xBB, 0x08, 0x02, 0x79, 0xA3, 0xBB, 0x09, 0x02, + 0x79, 0x9C, 0xBB, 0x0A, 0x02, 0x79, 0x88, 0xBB, 0x0B, 0x02, 0x79, 0x89, 0xE8, 0xF7, 0x22, 0x7C, + 0x05, 0x12, 0x06, 0x0E, 0xDC, 0xFB, 0x22, 0xC2, 0xAF, 0x12, 0x06, 0x1E, 0x12, 0x06, 0x25, 0x12, + 0x06, 0x2C, 0x12, 0x06, 0x33, 0x12, 0x05, 0xFF, 0x12, 0x06, 0x1E, 0x12, 0x06, 0x25, 0x12, 0x06, + 0x2C, 0x12, 0x06, 0x33, 0x12, 0x05, 0xFF, 0x12, 0x06, 0x1E, 0x12, 0x06, 0x25, 0x12, 0x06, 0x2C, + 0x12, 0x06, 0x33, 0xD2, 0xAF, 0x22, 0xC2, 0xAF, 0x12, 0x06, 0x33, 0x12, 0x06, 0x2C, 0x12, 0x06, + 0x25, 0x12, 0x06, 0x1E, 0x12, 0x05, 0xFF, 0x12, 0x06, 0x33, 0x12, 0x06, 0x2C, 0x12, 0x06, 0x25, + 0x12, 0x06, 0x1E, 0x12, 0x05, 0xFF, 0x12, 0x06, 0x33, 0x12, 0x06, 0x2C, 0x12, 0x06, 0x25, 0x12, + 0x06, 0x1E, 0xD2, 0xAF, 0x22, 0xAE, 0x74, 0xAF, 0x75, 0xC2, 0xAF, 0x12, 0x06, 0x1E, 0x12, 0x06, + 0x1E, 0x12, 0x06, 0x1E, 0x12, 0x05, 0xFF, 0xDE, 0xF2, 0x12, 0x06, 0x33, 0x12, 0x05, 0xFF, 0xDF, + 0xF8, 0xD2, 0xAF, 0x22, 0x12, 0x17, 0x87, 0x12, 0x17, 0x7F, 0x12, 0x17, 0x7F, 0x75, 0x74, 0x01, + 0x75, 0x75, 0x01, 0x75, 0x76, 0x00, 0x12, 0x17, 0xE5, 0x7C, 0x05, 0xAD, 0x5C, 0x12, 0x06, 0x0E, + 0xC3, 0xED, 0x95, 0x5C, 0x70, 0xF5, 0xC3, 0xE5, 0x5C, 0x94, 0x01, 0x40, 0x0A, 0xC3, 0xE5, 0x5C, + 0x94, 0xFF, 0x40, 0x37, 0x02, 0x18, 0x48, 0x12, 0x17, 0x41, 0x12, 0x16, 0x9E, 0x12, 0x17, 0x87, + 0xC2, 0xAF, 0x75, 0xEF, 0x12, 0x12, 0x17, 0x7F, 0xDC, 0xD1, 0x05, 0x76, 0xC3, 0xE5, 0x76, 0x94, + 0x03, 0x50, 0x02, 0x80, 0xC1, 0x12, 0x17, 0x7F, 0x05, 0x75, 0xE5, 0x74, 0x14, 0x90, 0x00, 0x9F, + 0x93, 0xF8, 0x08, 0xC3, 0xE5, 0x75, 0x98, 0x50, 0x02, 0x80, 0xA8, 0x12, 0x17, 0x7F, 0x12, 0x17, + 0x7F, 0x05, 0x74, 0xC3, 0xE5, 0x74, 0x94, 0x0C, 0x50, 0x02, 0x80, 0x94, 0x12, 0x0F, 0xB8, 0x12, + 0x16, 0x9E, 0xC2, 0xAF, 0x75, 0xEF, 0x12, 0x12, 0x17, 0x7F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x11, 0x0E, + 0x0E, 0x06, 0x15, 0x09, 0x09, 0x04, 0xFF, 0x03, 0xFF, 0x09, 0x03, 0x01, 0x01, 0x55, 0xAA, 0x01, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x03, 0xFF, 0xFF, 0xFF, 0x25, 0xD0, 0x28, 0x50, 0x04, 0xFF, 0x02, + 0x00, 0x7A, 0xFF, 0x01, 0x01, 0x00, 0x03, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x23, 0x46, 0x56, 0x54, 0x4C, 0x69, 0x62, 0x65, 0x65, 0x33, 0x30, 0x41, 0x23, 0x20, 0x20, 0x20, + 0x23, 0x42, 0x4C, 0x48, 0x45, 0x4C, 0x49, 0x23, 0x46, 0x33, 0x39, 0x30, 0x23, 0x20, 0x20, 0x20, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xC2, 0xAF, 0xC2, 0xD3, 0x53, 0xD9, 0xBF, 0x75, 0x81, 0xC0, 0x43, 0xB2, 0x03, 0x43, 0xFF, 0x80, + 0x75, 0x24, 0x38, 0x75, 0x25, 0x03, 0xB1, 0xC9, 0x75, 0xEF, 0x02, 0x75, 0xE2, 0x40, 0x43, 0xF1, + 0x20, 0x53, 0xA4, 0xDF, 0x75, 0xD4, 0xFF, 0xD2, 0x85, 0x7D, 0xFA, 0x78, 0x03, 0x79, 0xE6, 0x7C, + 0x10, 0x75, 0x22, 0x00, 0x75, 0x23, 0x00, 0x90, 0x1D, 0xD6, 0x7B, 0x06, 0x75, 0x24, 0x98, 0x75, + 0x25, 0x01, 0x30, 0x85, 0x02, 0x81, 0x42, 0x20, 0x85, 0x08, 0xD8, 0xFB, 0xD9, 0xF9, 0xDC, 0xF7, + 0x81, 0xB4, 0x30, 0x85, 0x08, 0xD8, 0xFB, 0xD9, 0xF9, 0xDC, 0xF7, 0x81, 0xB4, 0xB1, 0x89, 0xE4, + 0x93, 0xA3, 0xC3, 0x9A, 0x60, 0x04, 0xDD, 0xC3, 0x81, 0xB4, 0xDB, 0xDB, 0xB1, 0x7B, 0x60, 0x02, + 0x81, 0x2B, 0x7B, 0x08, 0xE4, 0x93, 0xFA, 0xA3, 0xB1, 0x5C, 0xDB, 0xF8, 0x7A, 0x30, 0xB1, 0x5C, + 0x75, 0x22, 0x00, 0x75, 0x23, 0x00, 0xB1, 0x7B, 0xEA, 0xFC, 0xEB, 0xFD, 0xC3, 0xED, 0x94, 0xFE, + 0x40, 0x0E, 0xB1, 0x7B, 0x8A, 0x27, 0x8B, 0x28, 0xED, 0x30, 0xE0, 0x04, 0x8A, 0x82, 0x8B, 0x83, + 0xB1, 0x7B, 0x7A, 0xC2, 0x70, 0xD8, 0xC3, 0xED, 0x94, 0xFE, 0x60, 0x13, 0x50, 0xCE, 0xBD, 0x00, + 0x25, 0xEC, 0x60, 0x09, 0x75, 0x20, 0x00, 0x75, 0x21, 0xFF, 0x02, 0x00, 0x00, 0x81, 0x00, 0xA8, + 0x27, 0xA9, 0x28, 0x08, 0x09, 0xD8, 0x04, 0xD9, 0x02, 0x81, 0xD3, 0xB1, 0x7F, 0xEA, 0x89, 0xAA, + 0xF2, 0x81, 0xC5, 0x0D, 0x81, 0xA0, 0xC3, 0xED, 0x94, 0x03, 0x50, 0x49, 0xED, 0xA2, 0xE0, 0x92, + 0x00, 0x7A, 0xC5, 0xC3, 0xE5, 0x82, 0x94, 0x00, 0xE5, 0x83, 0x94, 0x1C, 0x50, 0x90, 0x20, 0x00, + 0x10, 0x43, 0x8F, 0x02, 0x43, 0x8F, 0x01, 0x75, 0xB7, 0xA5, 0x75, 0xB7, 0xF1, 0xF0, 0x30, 0x00, + 0x1F, 0xA8, 0x27, 0xA9, 0x28, 0x08, 0x09, 0x43, 0x8F, 0x01, 0x53, 0x8F, 0xFD, 0xD8, 0x04, 0xD9, + 0x02, 0xA1, 0x20, 0x89, 0xAA, 0xE2, 0x75, 0xB7, 0xA5, 0x75, 0xB7, 0xF1, 0xF0, 0xA3, 0xA1, 0x0D, + 0x53, 0x8F, 0xFC, 0x81, 0x7C, 0x7A, 0xC1, 0xBD, 0x03, 0x0C, 0xE4, 0x93, 0xA3, 0xFA, 0xB1, 0x40, + 0xDC, 0xF8, 0xB1, 0x38, 0x81, 0x7C, 0x81, 0x7E, 0xAA, 0x22, 0xAB, 0x23, 0xB1, 0x5C, 0xEB, 0xFA, + 0xEA, 0x62, 0x22, 0x75, 0x26, 0x08, 0xC3, 0xE5, 0x23, 0x13, 0xF5, 0x23, 0xE5, 0x22, 0x13, 0xF5, + 0x22, 0x50, 0x06, 0x63, 0x23, 0xA0, 0x63, 0x22, 0x01, 0xD5, 0x26, 0xEA, 0xB1, 0xC9, 0xB1, 0xC9, + 0x75, 0x26, 0x0A, 0xEA, 0xF4, 0x20, 0x01, 0x02, 0xD2, 0x85, 0x30, 0x01, 0x02, 0xC2, 0x85, 0xB1, + 0xC9, 0xC3, 0x13, 0x40, 0x02, 0xC2, 0x01, 0xD5, 0x26, 0xEB, 0x22, 0xB1, 0x7F, 0xEA, 0xFB, 0x20, + 0x85, 0x02, 0xA1, 0x7F, 0x30, 0x85, 0x02, 0xA1, 0x84, 0x75, 0x26, 0x08, 0xAE, 0x24, 0xAF, 0x25, + 0xC3, 0xEF, 0x13, 0xFF, 0xEE, 0x13, 0xFE, 0xB1, 0xCD, 0xB1, 0xC9, 0xC3, 0xEA, 0x13, 0x30, 0x85, + 0x02, 0x44, 0x80, 0xFA, 0x30, 0xE7, 0x03, 0x63, 0x22, 0x01, 0xC3, 0xE5, 0x23, 0x13, 0xF5, 0x23, + 0xE5, 0x22, 0x13, 0xF5, 0x22, 0x50, 0x06, 0x63, 0x23, 0xA0, 0x63, 0x22, 0x01, 0xD5, 0x26, 0xD9, + 0xE5, 0x22, 0x65, 0x23, 0x65, 0x23, 0xF5, 0x22, 0x22, 0xAE, 0x24, 0xAF, 0x25, 0x0E, 0x0F, 0xDE, + 0xFE, 0xDF, 0xFC, 0xD2, 0x01, 0x22, 0x42, 0x4C, 0x48, 0x65, 0x6C, 0x69, 0x34, 0x37, 0x31, 0x63, + 0xF3, 0x90, 0x06, 0x01 + }; + +uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo) +{ + //only 2 bytes used $1E9307 -> 0x9307 + pDeviceInfo->dword = 0x163F390; + return true; +} + +uint8_t BL_SendCMDKeepAlive(void) +{ + return true; +} + +void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo) +{ + pDeviceInfo->bytes[0] = 1; + return; +} + +static uint8_t BL_ReadA(uint8_t cmd, ioMem_t *pMem) +{ + UNUSED(cmd); + uint16_t address = pMem->D_FLASH_ADDR_H << 8 | pMem->D_FLASH_ADDR_L; + + uint16_t bytes = pMem->D_NUM_BYTES; + if (bytes == 0) bytes = 256; + + memcpy(pMem->D_PTR_I, &fakeFlash[address], bytes); + return true; +} + +static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout) +{ + UNUSED(cmd); + UNUSED(timeout); + uint16_t address = pMem->D_FLASH_ADDR_H << 8 | pMem->D_FLASH_ADDR_L; + + uint16_t bytes = pMem->D_NUM_BYTES; + if (bytes == 0) bytes = 256; + memcpy(&fakeFlash[address], pMem->D_PTR_I, bytes); + return true; +} + +uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem) +{ + UNUSED(interface_mode); + return BL_ReadA(0, pMem); +} + +uint8_t BL_ReadEEprom(ioMem_t *pMem) +{ + return BL_ReadA(0, pMem); +} + +uint8_t BL_PageErase(ioMem_t *pMem) +{ + uint16_t address = pMem->D_FLASH_ADDR_H << 8 | pMem->D_FLASH_ADDR_L; + if (address + FAKE_PAGE_SIZE > FAKE_FLASH_SIZE) + return false; + memset(&fakeFlash[address], 0xFF, FAKE_PAGE_SIZE); + return true; +} + +uint8_t BL_WriteEEprom(ioMem_t *pMem) +{ + return BL_WriteA(0, pMem, 0); +} + +uint8_t BL_WriteFlash(ioMem_t *pMem) +{ + return BL_WriteA(0, pMem, 0); +} + #endif #endif diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 879b2edc66..3322ee94be 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -34,18 +34,24 @@ LINE_CODING g_lc; extern __IO uint8_t USB_Tx_State; __IO uint32_t bDeviceState = UNCONNECTED; /* USB device status */ -/* These are external variables imported from CDC core to be used for IN - transfer management. */ -extern uint8_t APP_Rx_Buffer[]; /* Write CDC received data in this buffer. +/* These are external variables imported from CDC core to be used for IN transfer management. */ + +/* Write CDC received data in this buffer. These data will be sent over USB IN endpoint in the CDC core functions. */ +extern uint8_t APP_Rx_Buffer[]; + extern uint32_t APP_Rx_ptr_out; -extern uint32_t APP_Rx_ptr_in; /* Increment this pointer or roll it back to + +/* Increment this pointer or roll it back to start address when writing received data in the buffer APP_Rx_Buffer. */ __IO uint32_t receiveLength=0; +extern uint32_t APP_Rx_ptr_in; -usbStruct_t usbData; +static uint8_t APP_Tx_Buffer[USB_RX_BUFSIZE]; +static uint32_t APP_Tx_ptr_out; +static uint32_t APP_Tx_ptr_in; /* Private function prototypes -----------------------------------------------*/ static uint16_t VCP_Init(void); @@ -153,9 +159,11 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) *******************************************************************************/ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) { - if (USB_Tx_State) - return 0; - + static uint32_t stateCount = 0; + if (USB_Tx_State) { + stateCount++; + while (USB_Tx_State); + } VCP_DataTx(ptrBuffer, sendLength); return sendLength; } @@ -174,12 +182,13 @@ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; } + return USBD_OK; } uint8_t usbAvailable(void) { - return (usbData.bufferInPosition != usbData.bufferOutPosition); + return (APP_Tx_ptr_out != APP_Tx_ptr_in); } /******************************************************************************* @@ -194,8 +203,8 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) uint32_t count = 0; while (usbAvailable() && count < len) { - recvBuf[count] = usbData.buffer[usbData.bufferOutPosition]; - usbData.bufferOutPosition = (usbData.bufferOutPosition + 1) % USB_RX_BUFSIZE; + recvBuf[count] = APP_Tx_Buffer[APP_Tx_ptr_out]; + APP_Tx_ptr_out = (APP_Tx_ptr_out + 1) % USB_RX_BUFSIZE; count++; receiveLength--; } @@ -227,8 +236,8 @@ static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) rxPackets++; for (uint32_t i = 0; i < Len; i++) { - usbData.buffer[usbData.bufferInPosition] = Buf[i]; - usbData.bufferInPosition = (usbData.bufferInPosition + 1) % USB_RX_BUFSIZE; + APP_Tx_Buffer[APP_Tx_ptr_in] = Buf[i]; + APP_Tx_ptr_in = (APP_Tx_ptr_in + 1) % USB_RX_BUFSIZE; receiveLength++; rxTotalBytes++; } diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index 553017ff28..097b1b1ee1 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -34,7 +34,7 @@ #include "usbd_usr.h" #include "usbd_desc.h" -#define USB_RX_BUFSIZE 2048 +#define USB_RX_BUFSIZE 1024 __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; @@ -70,12 +70,6 @@ typedef struct uint8_t datatype; } LINE_CODING; -typedef struct { - uint8_t buffer[USB_RX_BUFSIZE]; - uint16_t bufferInPosition; - uint16_t bufferOutPosition; -} usbStruct_t; - #endif /* __USBD_CDC_VCP_H */ diff --git a/src/main/vcpf4/usbd_conf.h b/src/main/vcpf4/usbd_conf.h index 02ceeae545..87481c2ef1 100644 --- a/src/main/vcpf4/usbd_conf.h +++ b/src/main/vcpf4/usbd_conf.h @@ -53,8 +53,8 @@ #define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ #define CDC_IN_FRAME_INTERVAL 15 /* Number of frames between IN transfers */ - #define APP_RX_DATA_SIZE 2048 /* Total size of IN buffer: - APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL */ + #define APP_RX_DATA_SIZE 1024 /* Total size of IN buffer: + APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL */ #endif /* USE_USB_OTG_HS */ #define APP_FOPS VCP_fops From bf780cd6be00a6c16ca2f5d77b9c724fa2b10da6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 25 Jul 2016 08:28:07 +0100 Subject: [PATCH 087/456] Reserved feature bits for v3.1 features so they are consistent with iNav --- src/main/config/config.h | 2 ++ src/main/io/beeper.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/config/config.h b/src/main/config/config.h index b18febe8cd..b97ac6b45b 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -54,6 +54,8 @@ typedef enum { FEATURE_SUPEREXPO_RATES = 1 << 23, FEATURE_OSD = 1 << 24, FEATURE_VTX = 1 << 25, + FEATURE_RX_NRF24 = 1 << 26, + FEATURE_SOFTSPI = 1 << 27, } features_e; void latchActiveFeatures(void); diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index b4ed924130..ed02aee30a 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -41,7 +41,7 @@ typedef enum { BEEPER_USB, // Some boards have beeper powered USB connected BEEPER_ALL, // Turn ON or OFF all beeper conditions - BEEPER_PREFERENCE, // Save prefered beeper configuration + BEEPER_PREFERENCE, // Save preferred beeper configuration // BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum } beeperMode_e; From d9733b820604df8aa8e9aa5329b29c556aca31f1 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 24 Jul 2016 00:38:59 +0100 Subject: [PATCH 088/456] Various measures to save ROM --- src/main/config/config.c | 8 +++++ src/main/drivers/adc_stm32f10x.c | 3 ++ src/main/flight/pid.c | 11 ++++++- src/main/io/serial_cli.c | 56 ++++++++++++++++++++++++++++---- src/main/mw.c | 5 +-- src/main/sensors/battery.c | 2 +- src/main/sensors/battery.h | 1 - src/main/target/CC3D/target.h | 19 ++++------- src/main/target/CJMCU/target.h | 15 +++++---- 9 files changed, 91 insertions(+), 29 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 74399f1bd8..4181050211 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -255,6 +255,7 @@ void resetGpsProfile(gpsProfile_t *gpsProfile) } #endif +#ifdef BARO void resetBarometerConfig(barometerConfig_t *barometerConfig) { barometerConfig->baro_sample_count = 21; @@ -262,6 +263,7 @@ void resetBarometerConfig(barometerConfig_t *barometerConfig) barometerConfig->baro_cf_vel = 0.985f; barometerConfig->baro_cf_alt = 0.965f; } +#endif void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) { @@ -291,6 +293,7 @@ void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) flight3DConfig->deadband3d_throttle = 50; } +#ifdef TELEMETRY void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) { telemetryConfig->telemetry_inversion = 0; @@ -303,6 +306,7 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) telemetryConfig->frsky_vfas_cell_voltage = 0; telemetryConfig->hottAlarmSoundInterval = 5; } +#endif void resetBatteryConfig(batteryConfig_t *batteryConfig) { @@ -483,7 +487,9 @@ static void resetConf(void) resetBatteryConfig(&masterConfig.batteryConfig); +#ifdef TELEMETRY resetTelemetryConfig(&masterConfig.telemetryConfig); +#endif #ifdef SERIALRX_PROVIDER masterConfig.rxConfig.serialrx_provider = SERIALRX_PROVIDER; @@ -573,7 +579,9 @@ static void resetConf(void) masterConfig.accDeadband.z = 40; masterConfig.acc_unarmedcal = 1; +#ifdef BARO resetBarometerConfig(&masterConfig.barometerConfig); +#endif // Radio parseRcChannels("AETR1234", &masterConfig.rxConfig); diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 3104bcd5e9..1ac85621d7 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -21,6 +21,8 @@ #include "platform.h" +#ifdef USE_ADC + #include "build_config.h" #include "system.h" @@ -176,3 +178,4 @@ void adcInit(drv_adc_config_t *init) ADC_SoftwareStartConvCmd(adc.ADCx, ENABLE); } +#endif diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 41c8ef838b..e1d408e5c6 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -65,10 +65,15 @@ uint8_t PIDweight[3]; static int32_t errorGyroI[3]; static float errorGyroIf[3]; +#ifdef SKIP_PID_FLOAT +static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); +pidControllerFuncPtr pid_controller = pidLegacy; // which pid controller are we using +#else static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); - pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using +#endif void setTargetPidLooptime(uint8_t pidProcessDenom) { @@ -101,6 +106,7 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static pt1Filter_t deltaFilter[3]; static pt1Filter_t yawFilter; +#ifndef SKIP_PID_FLOAT // Experimental betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) @@ -288,6 +294,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc #endif } } +#endif // Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, @@ -438,8 +445,10 @@ void pidSetController(pidControllerType_e type) case PID_CONTROLLER_LEGACY: pid_controller = pidLegacy; break; +#ifndef SKIP_PID_FLOAT case PID_CONTROLLER_BETAFLIGHT: pid_controller = pidBetaflight; +#endif } } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index fccab2747c..654c09c028 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -144,8 +144,9 @@ static void cliTasks(char *cmdline); #endif static void cliVersion(char *cmdline); static void cliRxRange(char *cmdline); +#if (FLASH_SIZE > 64) static void cliResource(char *cmdline); - +#endif #ifdef GPS static void cliGpsPassthrough(char *cmdline); #endif @@ -218,7 +219,7 @@ static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT { RX_FAILSAFE_MODE_INVALID, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET } }; -#ifndef CJMCU +#if (FLASH_SIZE > 64) // sync this with sensors_e static const char * const sensorTypeNames[] = { "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL @@ -306,7 +307,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), +#if (FLASH_SIZE > 64) CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), +#endif CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), CLI_COMMAND_DEF("rxfail", "show/set rx failsafe settings", NULL, cliRxFail), CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave), @@ -381,19 +384,23 @@ static const char * const lookupTableCurrentSensor[] = { "NONE", "ADC", "VIRTUAL" }; +#ifdef USE_SERVOS static const char * const lookupTableGimbalMode[] = { "NORMAL", "MIXTILT" }; +#endif +#ifdef BLACKBOX static const char * const lookupTableBlackboxDevice[] = { "SERIAL", "SPIFLASH", "SDCARD" }; - +#endif static const char * const lookupTablePidController[] = { "LEGACY", "BETAFLIGHT" }; +#ifdef SERIAL_RX static const char * const lookupTableSerialRX[] = { "SPEK1024", "SPEK2048", @@ -405,6 +412,7 @@ static const char * const lookupTableSerialRX[] = { "IBUS", "JETIEXBUS" }; +#endif static const char * const lookupTableGyroLpf[] = { "OFF", @@ -430,6 +438,7 @@ static const char * const lookupTableAccHardware[] = { "FAKE" }; +#ifdef BARO static const char * const lookupTableBaroHardware[] = { "AUTO", "NONE", @@ -437,7 +446,9 @@ static const char * const lookupTableBaroHardware[] = { "MS5611", "BMP280" }; +#endif +#ifdef MAG static const char * const lookupTableMagHardware[] = { "AUTO", "NONE", @@ -445,6 +456,7 @@ static const char * const lookupTableMagHardware[] = { "AK8975", "AK8963" }; +#endif static const char * const lookupTableDebug[DEBUG_COUNT] = { "NONE", @@ -491,16 +503,24 @@ typedef enum { TABLE_GPS_SBAS_MODE, #endif #ifdef BLACKBOX - TABLE_BLACKBOX_DEVICE, + TABLE_BLACKBOX_DEVICE, #endif TABLE_CURRENT_SENSOR, +#ifdef USE_SERVOS TABLE_GIMBAL_MODE, +#endif TABLE_PID_CONTROLLER, +#ifdef SERIAL_RX TABLE_SERIAL_RX, +#endif TABLE_GYRO_LPF, TABLE_ACC_HARDWARE, +#ifdef BARO TABLE_BARO_HARDWARE, +#endif +#ifdef MAG TABLE_MAG_HARDWARE, +#endif TABLE_DEBUG, TABLE_SUPEREXPO_YAW, TABLE_MOTOR_PWM_PROTOCOL, @@ -522,13 +542,21 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableBlackboxDevice, sizeof(lookupTableBlackboxDevice) / sizeof(char *) }, #endif { lookupTableCurrentSensor, sizeof(lookupTableCurrentSensor) / sizeof(char *) }, +#ifdef USE_SERVOS { lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) }, +#endif { lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) }, +#ifdef SERIAL_RX { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, +#endif { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }, { lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) }, +#ifdef BARO { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) }, +#endif +#ifdef MAG { lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) }, +#endif { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }, { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }, { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) }, @@ -576,7 +604,6 @@ typedef struct cliLookupTableConfig_s { typedef union { cliLookupTableConfig_t lookup; cliMinMaxConfig_t minmax; - } cliValueConfig_t; typedef struct { @@ -660,11 +687,16 @@ const clivalue_t valueTable[] = { { "gtune_average_cycles", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_average_cycles, .config.minmax = { 8, 128 } }, #endif +#ifdef SERIAL_RX { "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.serialrx_provider, .config.lookup = { TABLE_SERIAL_RX } }, +#endif { "sbus_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.sbus_inversion, .config.lookup = { TABLE_OFF_ON } }, +#ifdef SPEKTRUM_BIND { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX} }, { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind_autoreset, .config.minmax = { 0, 1} }, +#endif +#ifdef TELEMETRY { "telemetry_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.telemetryConfig.telemetry_switch, .config.lookup = { TABLE_OFF_ON } }, { "telemetry_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.telemetryConfig.telemetry_inversion, .config.lookup = { TABLE_OFF_ON } }, { "frsky_default_lattitude", VAR_FLOAT | MASTER_VALUE, &masterConfig.telemetryConfig.gpsNoFixLatitude, .config.minmax = { -90.0, 90.0 } }, @@ -674,6 +706,7 @@ const clivalue_t valueTable[] = { { "frsky_vfas_precision", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.frsky_vfas_precision, .config.minmax = { FRSKY_VFAS_PRECISION_LOW, FRSKY_VFAS_PRECISION_HIGH } }, { "frsky_vfas_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.telemetryConfig.frsky_vfas_cell_voltage, .config.lookup = { TABLE_OFF_ON } }, { "hott_alarm_sound_interval", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.hottAlarmSoundInterval, .config.minmax = { 0, 120 } }, +#endif { "battery_capacity", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.batteryCapacity, .config.minmax = { 0, 20000 } }, { "vbat_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatscale, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX } }, @@ -758,14 +791,18 @@ const clivalue_t valueTable[] = { { "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.pitch, .config.minmax = { -300, 300 } }, { "acc_trim_roll", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.roll, .config.minmax = { -300, 300 } }, +#ifdef BARO { "baro_tab_size", VAR_UINT8 | MASTER_VALUE, &masterConfig.barometerConfig.baro_sample_count, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX } }, { "baro_noise_lpf", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_noise_lpf, .config.minmax = { 0 , 1 } }, { "baro_cf_vel", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_cf_vel, .config.minmax = { 0 , 1 } }, { "baro_cf_alt", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_cf_alt, .config.minmax = { 0 , 1 } }, { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } }, +#endif +#ifdef MAG { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, +#endif { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } }, @@ -817,9 +854,11 @@ const clivalue_t valueTable[] = { { "vtx_mhz", VAR_UINT16 | MASTER_VALUE, &masterConfig.vtx_mhz, .config.minmax = { 5600, 5950 } }, #endif +#ifdef MAG { "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[X], .config.minmax = { -32768, 32767 } }, { "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Y], .config.minmax = { -32768, 32767 } }, { "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Z], .config.minmax = { -32768, 32767 } }, +#endif #ifdef LED_STRIP { "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } }, #endif @@ -2619,7 +2658,8 @@ static void cliProfile(char *cmdline) } } -static void cliRateProfile(char *cmdline) { +static void cliRateProfile(char *cmdline) +{ int i; if (isEmpty(cmdline)) { @@ -2751,6 +2791,7 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full) break; } } + static void cliPrintVarRange(const clivalue_t *var) { switch (var->type & VALUE_MODE_MASK) { @@ -2772,6 +2813,7 @@ static void cliPrintVarRange(const clivalue_t *var) break; } } + static void cliSetVar(const clivalue_t *var, const int_float_value_t value) { void *ptr = var->ptr; @@ -3148,6 +3190,7 @@ void cliProcess(void) } } +#if (FLASH_SIZE > 64) static void cliResource(char *cmdline) { UNUSED(cmdline); @@ -3166,6 +3209,7 @@ static void cliResource(char *cmdline) } } } +#endif void cliDfu(char *cmdLine) { diff --git a/src/main/mw.c b/src/main/mw.c index 371961e013..404e871dc7 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -856,16 +856,17 @@ void taskUpdateBeeper(void) void taskUpdateBattery(void) { +#ifdef USE_ADC static uint32_t vbatLastServiced = 0; - static uint32_t ibatLastServiced = 0; - if (feature(FEATURE_VBAT)) { if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) { vbatLastServiced = currentTime; updateBattery(); } } +#endif + static uint32_t ibatLastServiced = 0; if (feature(FEATURE_CURRENT_METER)) { int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 2bb1f973f0..342fcc1fdd 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -52,7 +52,7 @@ int32_t mAhDrawn = 0; // milliampere hours drawn from the battery static batteryState_e batteryState; -uint16_t batteryAdcToVoltage(uint16_t src) +static uint16_t batteryAdcToVoltage(uint16_t src) { // calculate battery voltage based on ADC reading // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index e52541db70..9d6afab5dc 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -69,7 +69,6 @@ extern uint16_t amperageLatestADC; extern int32_t amperage; extern int32_t mAhDrawn; -uint16_t batteryAdcToVoltage(uint16_t src); batteryState_e getBatteryState(void); const char * getBatteryStateString(void); void updateBattery(void); diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index c06f397138..5465eb0b9d 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -109,21 +109,16 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define DISPLAY +#undef GPS +#ifdef CC3D_OPBL +#undef LED_STRIP +#define SKIP_CLI_COMMAND_HELP +#undef BARO +//#undef MAG +#else #define SONAR #define SONAR_ECHO_PIN PB0 #define SONAR_TRIGGER_PIN PB5 - -#undef GPS - -#undef BARO -#undef MAG - -#ifdef CC3D_OPBL -#define SKIP_CLI_COMMAND_HELP -#undef LED_STRIP -#undef DISPLAY -#undef SONAR #endif #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 4af75b2b2e..973e0984f9 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -47,22 +47,25 @@ // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB67 -#define SPEKTRUM_BIND -// USART2, PA3 -#define BIND_PIN PA3 - #if (FLASH_SIZE > 64) #define BLACKBOX #define USE_SERVOS +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PIN PA3 #else -// Since the CJMCU PCB has holes for 4 motors in each corner we can save same flash space by disabling support for other mixers. #undef USE_CLI -#define USE_QUAD_MIXER_ONLY +#undef SERIAL_RX #define SKIP_TASK_STATISTICS #define SKIP_CLI_COMMAND_HELP +#define SKIP_PID_FLOAT #endif +// Since the CJMCU PCB has holes for 4 motors in each corner we can save same flash space by disabling support for other mixers. +#define USE_QUAD_MIXER_ONLY +#define SKIP_SERIAL_PASSTHROUGH + // IO - assuming all IOs on 48pin package TODO #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff From 770b1b63605a2379651e741d124374e96ce43309 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 25 Jul 2016 09:52:59 +0100 Subject: [PATCH 089/456] Tidid CC3D target.h --- src/main/target/CC3D/target.h | 35 ++++++++++++++++------------------- 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 5465eb0b9d..091b45b07a 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -26,8 +26,16 @@ #define BEEPER_OPT PB2 #define USE_EXTI -#define USE_MPU_DATA_READY_SIGNAL #define MPU_INT_EXTI PA3 +#define USE_MPU_DATA_READY_SIGNAL +//#define DEBUG_MPU_DATA_READY_INTERRUPT + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11 #define MPU6000_CS_GPIO GPIOA #define MPU6000_CS_PIN PA4 @@ -40,19 +48,12 @@ #define USE_FLASHFS #define USE_FLASH_M25P16 -#define USABLE_TIMER_CHANNEL_COUNT 12 - -#define DEBUG_MPU_DATA_READY_INTERRUPT -#define USE_MPU_DATA_READY_SIGNAL - #define GYRO #define USE_GYRO_SPI_MPU6000 - #define GYRO_MPU6000_ALIGN CW270_DEG #define ACC #define USE_ACC_SPI_MPU6000 - #define ACC_MPU6000_ALIGN CW270_DEG // MPU6000 interrupts @@ -85,13 +86,6 @@ #define UART3_RX_PIN PB11 #define UART3_TX_PIN PB10 -#define USE_SPI -#define USE_SPI_DEVICE_1 -#define USE_SPI_DEVICE_2 - -#define USE_I2C -#define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11 - #define USE_ADC #define CURRENT_METER_ADC_PIN PB1 #define VBAT_ADC_PIN PA0 @@ -109,16 +103,18 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define SONAR +#define SONAR_ECHO_PIN PB0 +#define SONAR_TRIGGER_PIN PB5 + #undef GPS + #ifdef CC3D_OPBL #undef LED_STRIP #define SKIP_CLI_COMMAND_HELP #undef BARO //#undef MAG -#else -#define SONAR -#define SONAR_ECHO_PIN PB0 -#define SONAR_TRIGGER_PIN PB5 +#undef SONAR #endif #define DEFAULT_RX_FEATURE FEATURE_RX_PPM @@ -128,4 +124,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(14) ) +#define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) From 7cb86a7552f15daf716b3bc5c966925af900ad8a Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 25 Jul 2016 21:28:49 +1000 Subject: [PATCH 090/456] Fix for VCP issue on F4 flashing of ESCs --- src/main/io/serial_4way.c | 4 - src/main/io/serial_4way_avrootloader.c | 120 ++++++++++++------------- src/main/vcp/usb_desc.c | 29 +++--- src/main/vcpf4/usbd_cdc_vcp.c | 44 ++++----- src/main/vcpf4/usbd_desc.c | 32 +++---- 5 files changed, 107 insertions(+), 122 deletions(-) diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 80a150b4e5..9bd2ba69e9 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -815,10 +815,6 @@ void esc4wayProcess(serialPort_t *mspPort) WriteByte(CRCout.bytes[0]); serialEndWrite(port); -#if defined(STM32F4) && defined(USE_VCP) - if (port->identifier == SERIAL_PORT_USB_VCP) - delay(75); /* we need some processing cycles to ensure VCP4 transmission completes and it doesn't lock up */ -#endif TX_LED_OFF; if (isExitScheduled) { esc4wayRelease(); diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 761efd2504..26621df4bb 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -33,7 +33,7 @@ #include "io/serial_4way.h" #include "io/serial_4way_impl.h" #include "io/serial_4way_avrootloader.h" -#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && 1 +#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && !defined(USE_FAKE_ESC) // Bootloader commands @@ -337,14 +337,14 @@ uint8_t BL_WriteFlash(ioMem_t *pMem) static uint8_t fakeFlash[FAKE_FLASH_SIZE] = { - 0x02, 0x19, 0xFD, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0xAA, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x01, 0x87, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x44, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x02, 0x03, 0x31, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x02, 0x19, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0x00, 0xAA, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0x01, 0x87, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0x03, 0x44, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0x02, 0x03, 0x31, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0x03, 0x04, 0x06, 0x08, 0x0C, 0x10, 0x18, 0x20, 0x30, 0x40, 0x60, 0x80, 0x04, 0x06, 0x08, 0x0C, 0x10, 0x18, 0x20, 0x30, 0x40, 0x60, 0x80, 0xA0, 0xC0, 0x00, 0x03, 0x07, 0x0F, 0x1F, 0x0D, 0x0D, 0x04, 0x05, 0x0D, 0x05, 0x03, 0x05, 0x03, 0x03, 0x02, 0xC2, 0xAF, 0x30, 0x63, 0x08, 0xC2, @@ -729,62 +729,62 @@ static uint8_t fakeFlash[FAKE_FLASH_SIZE] = 0x03, 0x50, 0x02, 0x80, 0xC1, 0x12, 0x17, 0x7F, 0x05, 0x75, 0xE5, 0x74, 0x14, 0x90, 0x00, 0x9F, 0x93, 0xF8, 0x08, 0xC3, 0xE5, 0x75, 0x98, 0x50, 0x02, 0x80, 0xA8, 0x12, 0x17, 0x7F, 0x12, 0x17, 0x7F, 0x05, 0x74, 0xC3, 0xE5, 0x74, 0x94, 0x0C, 0x50, 0x02, 0x80, 0x94, 0x12, 0x0F, 0xB8, 0x12, - 0x16, 0x9E, 0xC2, 0xAF, 0x75, 0xEF, 0x12, 0x12, 0x17, 0x7F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x11, 0x0E, + 0x16, 0x9E, 0xC2, 0xAF, 0x75, 0xEF, 0x12, 0x12, 0x17, 0x7F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0x11, 0x0E, 0x0E, 0x06, 0x15, 0x09, 0x09, 0x04, 0xFF, 0x03, 0xFF, 0x09, 0x03, 0x01, 0x01, 0x55, 0xAA, 0x01, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x03, 0xFF, 0xFF, 0xFF, 0x25, 0xD0, 0x28, 0x50, 0x04, 0xFF, 0x02, - 0x00, 0x7A, 0xFF, 0x01, 0x01, 0x00, 0x03, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x7A, 0xFF, 0x01, 0x01, 0x00, 0x03, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x23, 0x46, 0x56, 0x54, 0x4C, 0x69, 0x62, 0x65, 0x65, 0x33, 0x30, 0x41, 0x23, 0x20, 0x20, 0x20, 0x23, 0x42, 0x4C, 0x48, 0x45, 0x4C, 0x49, 0x23, 0x46, 0x33, 0x39, 0x30, 0x23, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xC2, 0xAF, 0xC2, 0xD3, 0x53, 0xD9, 0xBF, 0x75, 0x81, 0xC0, 0x43, 0xB2, 0x03, 0x43, 0xFF, 0x80, 0x75, 0x24, 0x38, 0x75, 0x25, 0x03, 0xB1, 0xC9, 0x75, 0xEF, 0x02, 0x75, 0xE2, 0x40, 0x43, 0xF1, 0x20, 0x53, 0xA4, 0xDF, 0x75, 0xD4, 0xFF, 0xD2, 0x85, 0x7D, 0xFA, 0x78, 0x03, 0x79, 0xE6, 0x7C, diff --git a/src/main/vcp/usb_desc.c b/src/main/vcp/usb_desc.c index 3db4216b68..5b162d7bd1 100644 --- a/src/main/vcp/usb_desc.c +++ b/src/main/vcp/usb_desc.c @@ -30,20 +30,21 @@ #include "usb_desc.h" /* USB Standard Device Descriptor */ -const uint8_t Virtual_Com_Port_DeviceDescriptor[] = { 0x12, /* bLength */ -USB_DEVICE_DESCRIPTOR_TYPE, /* bDescriptorType */ -0x00, 0x02, /* bcdUSB = 2.00 */ -0x02, /* bDeviceClass: CDC */ -0x00, /* bDeviceSubClass */ -0x00, /* bDeviceProtocol */ -0x40, /* bMaxPacketSize0 */ -0x83, 0x04, /* idVendor = 0x0483 */ -0x40, 0x57, /* idProduct = 0x7540 */ -0x00, 0x02, /* bcdDevice = 2.00 */ -1, /* Index of string descriptor describing manufacturer */ -2, /* Index of string descriptor describing product */ -3, /* Index of string descriptor describing the device's serial number */ -0x01 /* bNumConfigurations */ +const uint8_t Virtual_Com_Port_DeviceDescriptor[] = { + 0x12, /* bLength */ + USB_DEVICE_DESCRIPTOR_TYPE, /* bDescriptorType */ + 0x00, 0x02, /* bcdUSB = 2.00 */ + 0x02, /* bDeviceClass: CDC */ + 0x00, /* bDeviceSubClass */ + 0x00, /* bDeviceProtocol */ + 0x40, /* bMaxPacketSize0 */ + 0x83, 0x04, /* idVendor = 0x0483 */ + 0x40, 0x57, /* idProduct = 0x5740 */ + 0x00, 0x02, /* bcdDevice = 2.00 */ + 1, /* Index of string descriptor describing manufacturer */ + 2, /* Index of string descriptor describing product */ + 3, /* Index of string descriptor describing the device's serial number */ + 0x01 /* bNumConfigurations */ }; const uint8_t Virtual_Com_Port_ConfigDescriptor[] = { diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 3322ee94be..4a94678476 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -29,6 +29,8 @@ #include "stdbool.h" #include "drivers/system.h" +#include + LINE_CODING g_lc; extern __IO uint8_t USB_Tx_State; @@ -36,22 +38,20 @@ __IO uint32_t bDeviceState = UNCONNECTED; /* USB device status */ /* These are external variables imported from CDC core to be used for IN transfer management. */ -/* Write CDC received data in this buffer. - These data will be sent over USB IN endpoint - in the CDC core functions. */ +/* This is the buffer for data received from the MCU to APP (i.e. MCU TX, APP RX) */ extern uint8_t APP_Rx_Buffer[]; - extern uint32_t APP_Rx_ptr_out; -/* Increment this pointer or roll it back to +/* Increment this buffer position or roll it back to start address when writing received data in the buffer APP_Rx_Buffer. */ -__IO uint32_t receiveLength=0; extern uint32_t APP_Rx_ptr_in; +__IO uint32_t receiveLength = 0; + static uint8_t APP_Tx_Buffer[USB_RX_BUFSIZE]; -static uint32_t APP_Tx_ptr_out; -static uint32_t APP_Tx_ptr_in; +static uint32_t APP_Tx_ptr_out = 0; +static uint32_t APP_Tx_ptr_in = 0; /* Private function prototypes -----------------------------------------------*/ static uint16_t VCP_Init(void); @@ -159,11 +159,6 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) *******************************************************************************/ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) { - static uint32_t stateCount = 0; - if (USB_Tx_State) { - stateCount++; - while (USB_Tx_State); - } VCP_DataTx(ptrBuffer, sendLength); return sendLength; } @@ -178,6 +173,8 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) */ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) { + while (USB_Tx_State); + for (uint32_t i = 0; i < Len; i++) { APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; @@ -186,11 +183,6 @@ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) return USBD_OK; } -uint8_t usbAvailable(void) -{ - return (APP_Tx_ptr_out != APP_Tx_ptr_in); -} - /******************************************************************************* * Function Name : Receive DATA . * Description : receive the data from the PC to STM32 and send it through USB @@ -202,12 +194,17 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) { uint32_t count = 0; - while (usbAvailable() && count < len) { + while (APP_Tx_ptr_out != APP_Tx_ptr_in && count < len) { recvBuf[count] = APP_Tx_Buffer[APP_Tx_ptr_out]; APP_Tx_ptr_out = (APP_Tx_ptr_out + 1) % USB_RX_BUFSIZE; count++; receiveLength--; } + + if (!receiveLength) { + receiveLength = APP_Tx_ptr_out != APP_Tx_ptr_in; + } + return count; } @@ -226,23 +223,18 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) * @param Len: Number of data received (in bytes) * @retval Result of the opeartion: USBD_OK if all operations are OK else VCP_FAIL */ -static uint32_t rxTotalBytes = 0; -static uint32_t rxPackets = 0; - static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) { __disable_irq(); - rxPackets++; - + receiveLength += Len; for (uint32_t i = 0; i < Len; i++) { APP_Tx_Buffer[APP_Tx_ptr_in] = Buf[i]; APP_Tx_ptr_in = (APP_Tx_ptr_in + 1) % USB_RX_BUFSIZE; - receiveLength++; - rxTotalBytes++; } __enable_irq(); + if(receiveLength > (USB_RX_BUFSIZE-1)) return USBD_FAIL; diff --git a/src/main/vcpf4/usbd_desc.c b/src/main/vcpf4/usbd_desc.c index a0cf881916..cdf6c4fe7a 100644 --- a/src/main/vcpf4/usbd_desc.c +++ b/src/main/vcpf4/usbd_desc.c @@ -118,24 +118,20 @@ USBD_DEVICE USR_desc = /* USB Standard Device Descriptor */ __ALIGN_BEGIN uint8_t USBD_DeviceDesc[USB_SIZ_DEVICE_DESC] __ALIGN_END = { - 0x12, /*bLength */ - USB_DEVICE_DESCRIPTOR_TYPE, /*bDescriptorType*/ - 0x00, /*bcdUSB */ - 0x02, - 0xef, /*bDeviceClass*/ - 0x02, /*bDeviceSubClass*/ - 0x01, /*bDeviceProtocol*/ - USB_OTG_MAX_EP0_SIZE, /*bMaxPacketSize*/ - LOBYTE(USBD_VID), /*idVendor*/ - HIBYTE(USBD_VID), /*idVendor*/ - LOBYTE(USBD_PID), /*idVendor*/ - HIBYTE(USBD_PID), /*idVendor*/ - 0x00, /*bcdDevice rel. 2.00*/ - 0x02, - USBD_IDX_MFC_STR, /*Index of manufacturer string*/ - USBD_IDX_PRODUCT_STR, /*Index of product string*/ - USBD_IDX_SERIAL_STR, /*Index of serial number string*/ - USBD_CFG_MAX_NUM /*bNumConfigurations*/ + 0x12, /*bLength */ + USB_DEVICE_DESCRIPTOR_TYPE, /*bDescriptorType*/ + 0x00, 0x02, /*bcdUSB */ + 0x02, /*bDeviceClass*/ + 0x02, /*bDeviceSubClass*/ + 0x00, /*bDeviceProtocol*/ + USB_OTG_MAX_EP0_SIZE, /*bMaxPacketSize*/ + LOBYTE(USBD_VID), HIBYTE(USBD_VID), /*idVendor*/ + LOBYTE(USBD_PID), HIBYTE(USBD_PID), /*idProduct*/ + 0x00, 0x02, /*bcdDevice rel. 2.00*/ + USBD_IDX_MFC_STR, /*Index of manufacturer string*/ + USBD_IDX_PRODUCT_STR, /*Index of product string*/ + USBD_IDX_SERIAL_STR, /*Index of serial number string*/ + USBD_CFG_MAX_NUM /*bNumConfigurations*/ } ; /* USB_DeviceDescriptor */ #ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED From f9120c023a627835ebcf4a32300334154b61679d Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 25 Jul 2016 21:33:53 +1000 Subject: [PATCH 091/456] removed unneeded #include (used during debugging) --- src/main/vcpf4/usbd_cdc_vcp.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 4a94678476..bff0205004 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -29,8 +29,6 @@ #include "stdbool.h" #include "drivers/system.h" -#include - LINE_CODING g_lc; extern __IO uint8_t USB_Tx_State; From ba2d8d115f292fdf5b6371737430053e25958405 Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Mon, 25 Jul 2016 13:59:34 +0200 Subject: [PATCH 092/456] cleaner cli dump output --- src/main/io/serial_cli.c | 49 ++++++++++++++-------------------------- 1 file changed, 17 insertions(+), 32 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index fccab2747c..3c03d4e8e4 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2006,9 +2006,10 @@ static void cliDump(char *cmdline) cliPrint("\r\n# version\r\n"); cliVersion(NULL); + printSectionBreak(); cliPrint("\r\n# name\r\n"); cliName(NULL); - cliPrint("\r\n# dump master\r\n"); + cliPrint("\r\n# mixer\r\n"); #ifndef USE_QUAD_MIXER_ONLY @@ -2069,8 +2070,7 @@ static void cliDump(char *cmdline) #endif #endif - cliPrint("\r\n\r\n# feature\r\n"); - + cliPrint("\r\n# feature\r\n"); mask = featureMask(); for (i = 0; ; i++) { // disable all feature first if (featureNames[i] == NULL) @@ -2090,10 +2090,8 @@ static void cliDump(char *cmdline) #endif } - #ifdef BEEPER - cliPrint("\r\n\r\n# beeper\r\n"); - + cliPrint("\r\n# beeper\r\n"); uint8_t beeperCount = beeperTableEntryCount(); mask = getBeeperOffMask(); for (int i = 0; i < (beeperCount-2); i++) { @@ -2104,43 +2102,38 @@ static void cliDump(char *cmdline) } #endif - - cliPrint("\r\n\r\n# map\r\n"); + cliPrint("\r\n# map\r\n"); for (i = 0; i < 8; i++) buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; buf[i] = '\0'; cliPrintf("map %s\r\n", buf); - cliPrint("\r\n\r\n# serial\r\n"); + cliPrint("\r\n# serial\r\n"); cliSerial(""); #ifdef LED_STRIP - cliPrint("\r\n\r\n# led\r\n"); + cliPrint("\r\n# led\r\n"); cliLed(""); - cliPrint("\r\n\r\n# color\r\n"); + cliPrint("\r\n# color\r\n"); cliColor(""); - cliPrint("\r\n\r\n# mode_color\r\n"); + cliPrint("\r\n# mode_color\r\n"); cliModeColor(""); #endif cliPrint("\r\n# aux\r\n"); - cliAux(""); cliPrint("\r\n# adjrange\r\n"); - cliAdjustmentRange(""); cliPrintf("\r\n# rxrange\r\n"); - cliRxRange(""); #ifdef USE_SERVOS cliPrint("\r\n# servo\r\n"); - cliServo(""); // print servo directions @@ -2160,11 +2153,10 @@ static void cliDump(char *cmdline) #ifdef VTX cliPrint("\r\n# vtx\r\n"); - cliVtx(""); #endif - printSectionBreak(); + cliPrint("\r\n# dump master\r\n"); dumpValues(MASTER_VALUE); cliPrint("\r\n# rxfail\r\n"); @@ -2214,30 +2206,23 @@ static void cliDump(char *cmdline) void cliDumpProfile(uint8_t profileIndex) { - if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values - return; - - changeProfile(profileIndex); - cliPrint("\r\n# profile\r\n"); - cliProfile(""); - cliPrintf("############################# PROFILE VALUES ####################################\r\n"); - - printSectionBreak(); - dumpValues(PROFILE_VALUE); - - cliRateProfile(""); + if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values + return; + changeProfile(profileIndex); + cliPrint("\r\n# profile\r\n"); + cliProfile(""); + printSectionBreak(); + dumpValues(PROFILE_VALUE); } void cliDumpRateProfile(uint8_t rateProfileIndex) { if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values return; - changeControlRateProfile(rateProfileIndex); cliPrint("\r\n# rateprofile\r\n"); cliRateProfile(""); printSectionBreak(); - dumpValues(PROFILE_RATE_VALUE); } From 6f286315d242a57910e45bd6d5728c9cb86473f6 Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Mon, 25 Jul 2016 16:10:48 +0200 Subject: [PATCH 093/456] Update serial_cli.c --- src/main/io/serial_cli.c | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3c03d4e8e4..fa822f8314 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1971,7 +1971,6 @@ typedef enum { DUMP_ALL = (1 << 3), } dumpFlags_e; - static const char* const sectionBreak = "\r\n"; #define printSectionBreak() cliPrintf((char *)sectionBreak) @@ -2103,7 +2102,6 @@ static void cliDump(char *cmdline) #endif cliPrint("\r\n# map\r\n"); - for (i = 0; i < 8; i++) buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; buf[i] = '\0'; @@ -2156,7 +2154,7 @@ static void cliDump(char *cmdline) cliVtx(""); #endif - cliPrint("\r\n# dump master\r\n"); + cliPrint("\r\n# master\r\n"); dumpValues(MASTER_VALUE); cliPrint("\r\n# rxfail\r\n"); @@ -2174,7 +2172,6 @@ static void cliDump(char *cmdline) cliDumpRateProfile(rateCount); cliPrint("\r\n# restore original rateprofile selection\r\n"); - changeControlRateProfile(currentRateIndex); cliRateProfile(""); #ifdef USE_SLOW_SERIAL_CLI @@ -2183,7 +2180,6 @@ static void cliDump(char *cmdline) } cliPrint("\r\n# restore original profile selection\r\n"); - changeProfile(activeProfile); cliProfile(""); printSectionBreak(); From 5124ad1179ac9497954ea8f8c10a2b2dced70a96 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Mon, 25 Jul 2016 10:32:52 +0200 Subject: [PATCH 094/456] Enable Mag for SPRacing EVO target target.h cleanup --- src/main/target/SPRACINGF3EVO/target.h | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 9494f612c5..94117a125c 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -40,11 +40,9 @@ #define GYRO -#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define ACC -#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 #define ACC_MPU6500_ALIGN CW180_DEG @@ -53,12 +51,11 @@ #define BARO #define USE_BARO_BMP280 -//#define MAG -//#define USE_MPU9250_MAG // Enables bypass configuration -//#define USE_MAG_AK8963 +#define MAG +#define USE_MAG_AK8963 //#define USE_MAG_HMC5883 // External -//#define MAG_AK8963_ALIGN CW90_DEG_FLIP +#define MAG_AK8963_ALIGN CW90_DEG_FLIP //#define SONAR From 03cf9b516816712c16a7cd6aea922484583f20b5 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Tue, 26 Jul 2016 08:21:32 +1200 Subject: [PATCH 095/456] Added rateprofile support to MSP. Also fixed number of profiles supported by MSP. --- src/main/io/serial_msp.c | 38 +++++++++++++++++++++++++------------- 1 file changed, 25 insertions(+), 13 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index ecba5090de..1dd23a3846 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -188,6 +188,8 @@ STATIC_UNIT_TESTED mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; STATIC_UNIT_TESTED mspPort_t *currentPort; STATIC_UNIT_TESTED bufWriter_t *writer; +#define RATEPROFILE_MASK (1 << 7) + static void serialize8(uint8_t a) { bufWriterAppend(writer, a); @@ -674,7 +676,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_STATUS_EX: - headSerialReply(14); + headSerialReply(15); serialize16(cycleTime); #ifdef USE_I2C serialize16(i2cGetErrorCounter()); @@ -683,9 +685,10 @@ static bool processOutCommand(uint8_t cmdMSP) #endif serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); serialize32(packFlightModeFlags()); - serialize8(masterConfig.current_profile_index); + serialize8(getCurrentProfile()); serialize16(constrain(averageSystemLoadPercent, 0, 100)); serialize8(MAX_PROFILE_COUNT); + serialize8(getCurrentControlRateProfile()); break; case MSP_NAME: @@ -1261,7 +1264,7 @@ static bool processInCommand(void) { uint32_t i; uint16_t tmp; - uint8_t rate; + uint8_t value; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0, alt = 0; @@ -1271,14 +1274,23 @@ static bool processInCommand(void) #endif switch (currentPort->cmdMSP) { case MSP_SELECT_SETTING: - if (!ARMING_FLAG(ARMED)) { - masterConfig.current_profile_index = read8(); - if (masterConfig.current_profile_index > 1) { - masterConfig.current_profile_index = 0; + value = read8(); + if ((value & RATEPROFILE_MASK) == 0) { + if (!ARMING_FLAG(ARMED)) { + if (value >= MAX_PROFILE_COUNT) { + value = 0; + } + changeProfile(value); } - writeEEPROM(); - readEEPROM(); + } else { + value = value & ~RATEPROFILE_MASK; + + if (value >= MAX_RATEPROFILES) { + value = 0; + } + changeControlRateProfile(value); } + break; case MSP_SET_HEAD: magHold = read16(); @@ -1366,11 +1378,11 @@ static bool processInCommand(void) currentControlRateProfile->rcRate8 = read8(); currentControlRateProfile->rcExpo8 = read8(); for (i = 0; i < 3; i++) { - rate = read8(); - currentControlRateProfile->rates[i] = MIN(rate, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + value = read8(); + currentControlRateProfile->rates[i] = MIN(value, i == FD_YAW ? CONTROL_RATE_CONFIG_YAW_RATE_MAX : CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); } - rate = read8(); - currentControlRateProfile->dynThrPID = MIN(rate, CONTROL_RATE_CONFIG_TPA_MAX); + value = read8(); + currentControlRateProfile->dynThrPID = MIN(value, CONTROL_RATE_CONFIG_TPA_MAX); currentControlRateProfile->thrMid8 = read8(); currentControlRateProfile->thrExpo8 = read8(); currentControlRateProfile->tpa_breakpoint = read16(); From 14c9e8773693a5dc3a00278a96ba2926ea5a5b2f Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Mon, 25 Jul 2016 22:39:31 +0200 Subject: [PATCH 096/456] Moved ALIENFLIGHTF1 to unique target --- .../target/{NAZE => ALIENFLIGHTF1}/config.c | 0 src/main/target/ALIENFLIGHTF1/target.c | 106 +++++++++++ src/main/target/ALIENFLIGHTF1/target.h | 172 ++++++++++++++++++ src/main/target/ALIENFLIGHTF1/target.mk | 20 ++ src/main/target/NAZE/ALIENFLIGHTF1.mk | 0 src/main/target/NAZE/target.h | 20 -- 6 files changed, 298 insertions(+), 20 deletions(-) rename src/main/target/{NAZE => ALIENFLIGHTF1}/config.c (100%) create mode 100644 src/main/target/ALIENFLIGHTF1/target.c create mode 100644 src/main/target/ALIENFLIGHTF1/target.h create mode 100644 src/main/target/ALIENFLIGHTF1/target.mk delete mode 100644 src/main/target/NAZE/ALIENFLIGHTF1.mk diff --git a/src/main/target/NAZE/config.c b/src/main/target/ALIENFLIGHTF1/config.c similarity index 100% rename from src/main/target/NAZE/config.c rename to src/main/target/ALIENFLIGHTF1/config.c diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c new file mode 100644 index 0000000000..7116b53551 --- /dev/null +++ b/src/main/target/ALIENFLIGHTF1/target.c @@ -0,0 +1,106 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 +}; + diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h new file mode 100644 index 0000000000..41adcaf6f8 --- /dev/null +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -0,0 +1,172 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "AFF1" // AlienFlight F1. + +#define LED0 PB3 +#define LED1 PB4 + +#define BEEPER PA12 + +#define BARO_XCLR_PIN PC13 +#define BARO_EOC_PIN PC14 + +#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO +#define INVERTER_USART USART2 + +#define USE_EXTI +#define MAG_INT_EXTI PC14 +#define EXTI_CALLBACK_HANDLER_COUNT 3 // MPU data ready, MAG data ready, BMP085 EOC +//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define USE_MPU_DATA_READY_SIGNAL +//#define DEBUG_MAG_DATA_READY_INTERRUPT +#define USE_MAG_DATA_READY_SIGNAL + +// SPI2 +// PB15 28 SPI2_MOSI +// PB14 27 SPI2_MISO +// PB13 26 SPI2_SCK +// PB12 25 SPI2_NSS + +#define USE_SPI +#define USE_SPI_DEVICE_2 + +#define NAZE_SPI_INSTANCE SPI2 +#define NAZE_SPI_CS_GPIO GPIOB +#define NAZE_SPI_CS_PIN PB12 +#define NAZE_CS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOB + +// We either have this 16mbit flash chip on SPI or the MPU6500 acc/gyro depending on board revision: +#define M25P16_CS_GPIO NAZE_SPI_CS_GPIO +#define M25P16_CS_PIN NAZE_SPI_CS_PIN +#define M25P16_SPI_INSTANCE NAZE_SPI_INSTANCE + +#define MPU6500_CS_GPIO_CLK_PERIPHERAL NAZE_CS_GPIO_CLK_PERIPHERAL +#define MPU6500_CS_GPIO NAZE_SPI_CS_GPIO +#define MPU6500_CS_PIN NAZE_SPI_CS_PIN +#define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define GYRO +#define USE_GYRO_MPU3050 +#define USE_GYRO_MPU6050 +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 + +#define GYRO_MPU3050_ALIGN CW0_DEG +#define GYRO_MPU6050_ALIGN CW0_DEG +#define GYRO_MPU6500_ALIGN CW0_DEG + +#define ACC +#define USE_ACC_ADXL345 +#define USE_ACC_BMA280 +#define USE_ACC_MMA8452 +#define USE_ACC_MPU6050 +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 + +#define ACC_ADXL345_ALIGN CW270_DEG +#define ACC_MPU6050_ALIGN CW0_DEG +#define ACC_MMA8452_ALIGN CW90_DEG +#define ACC_BMA280_ALIGN CW0_DEG +#define ACC_MPU6500_ALIGN CW0_DEG + +#define BARO +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_BMP280 + +#define MAG +#define USE_MAG_HMC5883 +#define MAG_HMC5883_ALIGN CW180_DEG + +#define SONAR +#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN_PWM PB8 +#define SONAR_ECHO_PIN_PWM PB9 + +#define DISPLAY + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_SOFTSERIAL1 +#define USE_SOFTSERIAL2 +#define SERIAL_PORT_COUNT 5 + +#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 +#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 +#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 + +// USART3 only on NAZE32_SP - Flex Port +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_2) + +// #define SOFT_I2C // enable to test software i2c +// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) +// #define SOFT_I2C_PB67 + +#define USE_ADC +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA4 +#define RSSI_ADC_PIN PA1 +#define EXTERNAL1_ADC_PIN PA5 + + +#define LED_STRIP +#define WS2811_TIMER TIM3 +#define WS2811_PIN PA6 +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER + +#undef GPS + +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PIN PA3 + +// alternative defaults for AlienFlight F1 target +#define TARGET_CONFIG + +#define BRUSHED_MOTORS +#define DEFAULT_FEATURES FEATURE_MOTOR_STOP +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 +#define SERIALRX_UART SERIAL_PORT_USART2 + +#define HARDWARE_BIND_PLUG +// Hardware bind plug at PB5 (Pin 41) +#define BINDPLUG_PIN PB5 + + +// IO - assuming all IOs on 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) + +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/ALIENFLIGHTF1/target.mk b/src/main/target/ALIENFLIGHTF1/target.mk new file mode 100644 index 0000000000..07d81cf447 --- /dev/null +++ b/src/main/target/ALIENFLIGHTF1/target.mk @@ -0,0 +1,20 @@ +F1_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH HIGHEND + +TARGET_SRC = \ + drivers/accgyro_adxl345.c \ + drivers/accgyro_bma280.c \ + drivers/accgyro_l3g4200d.c \ + drivers/accgyro_mma845x.c \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu3050.c \ + drivers/accgyro_mpu6050.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp085.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f10x.c \ + drivers/sonar_hcsr04.c diff --git a/src/main/target/NAZE/ALIENFLIGHTF1.mk b/src/main/target/NAZE/ALIENFLIGHTF1.mk deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 512746d003..0e8ba62be2 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -158,26 +158,6 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#ifdef ALIENFLIGHTF1 -// alternative defaults for AlienFlight F1 target -#undef TARGET_BOARD_IDENTIFIER -#define TARGET_BOARD_IDENTIFIER "AFF1" // AlienFlight F1. -#define TARGET_CONFIG - -#undef BOARD_HAS_VOLTAGE_DIVIDER -#undef USE_SERIAL_4WAY_BLHELI_INTERFACE - -#define BRUSHED_MOTORS -#define DEFAULT_FEATURES FEATURE_MOTOR_STOP -#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 -#define SERIALRX_UART SERIAL_PORT_USART2 - -#define HARDWARE_BIND_PLUG -// Hardware bind plug at PB5 (Pin 41) -#define BINDPLUG_PIN PB5 -#endif // ALIENFLIGHTF1 - // IO - assuming all IOs on 48pin package #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff From 9c1933315464666c2573316a322689ca2d31ad32 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 26 Jul 2016 07:45:05 +1000 Subject: [PATCH 097/456] A few comments and a rename for better clarity. --- src/main/vcpf4/usbd_cdc_vcp.c | 19 ++++++++++++++----- src/main/vcpf4/usbd_cdc_vcp.h | 2 -- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index bff0205004..99f9f5c04e 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -46,8 +46,12 @@ extern uint32_t APP_Rx_ptr_out; extern uint32_t APP_Rx_ptr_in; __IO uint32_t receiveLength = 0; - -static uint8_t APP_Tx_Buffer[USB_RX_BUFSIZE]; +/* + APP TX is the circular buffer for data that is transmitted from the APP (host) + to the USB device (flight controller). +*/ +#define APP_TX_DATA_SIZE 1024 +static uint8_t APP_Tx_Buffer[APP_TX_DATA_SIZE]; static uint32_t APP_Tx_ptr_out = 0; static uint32_t APP_Tx_ptr_in = 0; @@ -171,6 +175,11 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) */ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) { + /* + make sure that any paragraph end frame is not in play + could just check for: USB_CDC_ZLP, but better to be safe + and wait for any existing transmission to complete. + */ while (USB_Tx_State); for (uint32_t i = 0; i < Len; i++) { @@ -194,7 +203,7 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) while (APP_Tx_ptr_out != APP_Tx_ptr_in && count < len) { recvBuf[count] = APP_Tx_Buffer[APP_Tx_ptr_out]; - APP_Tx_ptr_out = (APP_Tx_ptr_out + 1) % USB_RX_BUFSIZE; + APP_Tx_ptr_out = (APP_Tx_ptr_out + 1) % APP_TX_DATA_SIZE; count++; receiveLength--; } @@ -228,12 +237,12 @@ static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) receiveLength += Len; for (uint32_t i = 0; i < Len; i++) { APP_Tx_Buffer[APP_Tx_ptr_in] = Buf[i]; - APP_Tx_ptr_in = (APP_Tx_ptr_in + 1) % USB_RX_BUFSIZE; + APP_Tx_ptr_in = (APP_Tx_ptr_in + 1) % APP_TX_DATA_SIZE; } __enable_irq(); - if(receiveLength > (USB_RX_BUFSIZE-1)) + if(receiveLength > APP_TX_DATA_SIZE) return USBD_FAIL; return USBD_OK; diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index 097b1b1ee1..daad8ad0f9 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -34,8 +34,6 @@ #include "usbd_usr.h" #include "usbd_desc.h" -#define USB_RX_BUFSIZE 1024 - __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength); // HJI From c1eecb7a5b8a3707d1754b18b1209f9f7c2cfda9 Mon Sep 17 00:00:00 2001 From: blckmn Date: Tue, 26 Jul 2016 07:50:30 +1000 Subject: [PATCH 098/456] Increased UART buffer size on STM32F4, and Ensured FAKE ESC will only apply if both USE_SERIAL_4WAY_BLHELI_BOOTLOADER and USE_FAKE_ESC defined. --- src/main/drivers/serial_uart_stm32f4xx.c | 4 ++-- src/main/io/serial_4way_avrootloader.c | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c index f79a99ae6e..d6bd58024e 100644 --- a/src/main/drivers/serial_uart_stm32f4xx.c +++ b/src/main/drivers/serial_uart_stm32f4xx.c @@ -30,8 +30,8 @@ #include "serial_uart.h" #include "serial_uart_impl.h" -#define UART_RX_BUFFER_SIZE UART1_RX_BUFFER_SIZE -#define UART_TX_BUFFER_SIZE UART1_TX_BUFFER_SIZE +#define UART_RX_BUFFER_SIZE 512 +#define UART_TX_BUFFER_SIZE 512 typedef enum UARTDevice { UARTDEV_1 = 0, diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 26621df4bb..4ccea1601a 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -330,7 +330,8 @@ uint8_t BL_WriteFlash(ioMem_t *pMem) return BL_WriteA(CMD_PROG_FLASH, pMem, (40 / START_BIT_TIMEOUT_MS)); } -#else +#endif +#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_FAKE_ESC) #define FAKE_PAGE_SIZE 512 #define FAKE_FLASH_SIZE 16385 From 1b98484a72f9f0cea07ed8908a5e01334de7b201 Mon Sep 17 00:00:00 2001 From: tianbin4279 Date: Tue, 26 Jul 2016 12:49:48 +0800 Subject: [PATCH 099/456] Add target for AIORACERF3 Add target for AIORACERF3 --- src/main/target/AIORACERF3/target.c | 98 ++++++++++++++++ src/main/target/AIORACERF3/target.h | 169 +++++++++++++++++++++++++++ src/main/target/AIORACERF3/target.mk | 16 +++ 3 files changed, 283 insertions(+) create mode 100644 src/main/target/AIORACERF3/target.c create mode 100644 src/main/target/AIORACERF3/target.h create mode 100644 src/main/target/AIORACERF3/target.mk diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c new file mode 100644 index 0000000000..ea932f276a --- /dev/null +++ b/src/main/target/AIORACERF3/target.c @@ -0,0 +1,98 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + // PPM / UART2 RX + { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PPM + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM1 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM3 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM8 + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP +}; + diff --git a/src/main/target/AIORACERF3/target.h b/src/main/target/AIORACERF3/target.h new file mode 100644 index 0000000000..8b6b4a98bd --- /dev/null +++ b/src/main/target/AIORACERF3/target.h @@ -0,0 +1,169 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "ARF3" + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT + +#define LED0 PB8 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USABLE_TIMER_CHANNEL_COUNT 12 // PPM, 8 PWM, UART3 RX/TX, LED Strip + +#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect + +#define USE_EXTI +#define MPU_INT_EXTI PC13 +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + + +#define GYRO +#define USE_GYRO_MPU6500 +#define USE_GYRO_SPI_MPU6500 + +#define ACC +#define USE_ACC_MPU6500 +#define USE_ACC_SPI_MPU6500 + +#define ACC_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG + +#define BARO +#define USE_BARO_BMP280 + +#define MAG +#define USE_MPU9250_MAG // Enables bypass configuration +#define USE_MAG_AK8963 +//#define USE_MAG_HMC5883 // External + +#define MAG_AK8963_ALIGN CW90_DEG_FLIP + +//#define SONAR + +#define USB_IO + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define SERIAL_PORT_COUNT 4 + +#define UART1_TX_PIN PA9 // PA9 +#define UART1_RX_PIN PA10 // PA10 + +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 // PA15 + +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define USE_SPI +#define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 SPI2 (SDCard) + +#define SPI1_NSS_PIN PB9 +#define SPI1_SCK_PIN PB3 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PB5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SDCARD +#define USE_SDCARD_SPI2 + +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PC14 +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +// SPI2 is on the APB1 bus whose clock runs at 36MHz. Divide to under 400kHz for init: +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +// Divide to under 25MHz for normal operation: +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +// Note, this is the same DMA channel as UART1_RX. Luckily we don't use DMA for USART Rx. +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 + +#define MPU6500_CS_PIN PB9 +#define MPU6500_SPI_INSTANCE SPI1 + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER + +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PA4 +#define RSSI_ADC_PIN PB2 + +#define LED_STRIP +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_PIN PA8 +#define WS2811_TIMER TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define TRANSPONDER +#define TRANSPONDER_GPIO GPIOA +#define TRANSPONDER_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA +#define TRANSPONDER_GPIO_AF GPIO_AF_6 +#define TRANSPONDER_PIN GPIO_Pin_8 +#define TRANSPONDER_PIN_SOURCE GPIO_PinSource8 +#define TRANSPONDER_TIMER TIM1 +#define TRANSPONDER_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 +#define TRANSPONDER_DMA_CHANNEL DMA1_Channel2 +#define TRANSPONDER_IRQ DMA1_Channel2_IRQn +#define TRANSPONDER_DMA_TC_FLAG DMA1_FLAG_TC2 +#define TRANSPONDER_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// IO - stm32f303cc in 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) + + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) + diff --git a/src/main/target/AIORACERF3/target.mk b/src/main/target/AIORACERF3/target.mk new file mode 100644 index 0000000000..9b179afe4b --- /dev/null +++ b/src/main/target/AIORACERF3/target.mk @@ -0,0 +1,16 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/compass_ak8963.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_usb_vcp.c \ + drivers/transponder_ir.c \ + drivers/transponder_ir_stm32f30x.c \ + io/transponder_ir.c + From bf04fba5a50647f29a98378689ecce484d7d3de7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=88=B5=E7=88=B7?= Date: Tue, 26 Jul 2016 16:16:09 +0800 Subject: [PATCH 100/456] Update target.c --- src/main/target/AIORACERF3/target.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index ea932f276a..e6352f01a6 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -83,13 +83,13 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PPM - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM1 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM3 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM1 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM3 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM8 { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) From edbd49cbeff01004fbf1ffbc56fb00ff7d0401df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=88=B5=E7=88=B7?= Date: Tue, 26 Jul 2016 16:18:07 +0800 Subject: [PATCH 101/456] Update target.h --- src/main/target/AIORACERF3/target.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/target/AIORACERF3/target.h b/src/main/target/AIORACERF3/target.h index 8b6b4a98bd..0707437015 100644 --- a/src/main/target/AIORACERF3/target.h +++ b/src/main/target/AIORACERF3/target.h @@ -40,11 +40,11 @@ #define GYRO -#define USE_GYRO_MPU6500 +//#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define ACC -#define USE_ACC_MPU6500 +//#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 #define ACC_MPU6500_ALIGN CW180_DEG @@ -54,7 +54,7 @@ #define USE_BARO_BMP280 #define MAG -#define USE_MPU9250_MAG // Enables bypass configuration +//#define USE_MPU9250_MAG // Enables bypass configuration #define USE_MAG_AK8963 //#define USE_MAG_HMC5883 // External From 75368fb6a2db5fbdd1d318119814edf20654c6d9 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Wed, 27 Jul 2016 07:27:41 +1200 Subject: [PATCH 102/456] Moved resetting rateprofile values into 'resetPidProfile' to make them get reset on 'Reset PID' in GUI. --- src/main/config/config.c | 49 ++++++++++++++++++++++------------------ src/main/io/serial_msp.c | 4 ++-- 2 files changed, 29 insertions(+), 24 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 74399f1bd8..56b5acf198 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -180,7 +180,23 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) accelerometerTrims->values.yaw = 0; } -void resetPidProfile(pidProfile_t *pidProfile) +static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) +{ + controlRateConfig->rcRate8 = 100; + controlRateConfig->rcYawRate8 = 100; + controlRateConfig->rcExpo8 = 10; + controlRateConfig->thrMid8 = 50; + controlRateConfig->thrExpo8 = 0; + controlRateConfig->dynThrPID = 20; + controlRateConfig->rcYawExpo8 = 10; + controlRateConfig->tpa_breakpoint = 1650; + + for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { + controlRateConfig->rates[axis] = 70; + } +} + +static void resetPidProfile(pidProfile_t *pidProfile) { pidProfile->pidController = PID_CONTROLLER_BETAFLIGHT; @@ -242,6 +258,15 @@ void resetPidProfile(pidProfile_t *pidProfile) #endif } +void resetProfile(profile_t *profile) +{ + resetPidProfile(&profile->pidProfile); + + for (int rI = 0; rIcontrolRateProfile[rI]); + } +} + #ifdef GPS void resetGpsProfile(gpsProfile_t *gpsProfile) { @@ -350,23 +375,6 @@ void resetSerialConfig(serialConfig_t *serialConfig) serialConfig->reboot_character = 'R'; } -static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) -{ - controlRateConfig->rcRate8 = 100; - controlRateConfig->rcYawRate8 = 100; - controlRateConfig->rcExpo8 = 10; - controlRateConfig->thrMid8 = 50; - controlRateConfig->thrExpo8 = 0; - controlRateConfig->dynThrPID = 20; - controlRateConfig->rcYawExpo8 = 10; - controlRateConfig->tpa_breakpoint = 1650; - - for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { - controlRateConfig->rates[axis] = 70; - } - -} - void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { rcControlsConfig->deadband = 0; @@ -560,11 +568,8 @@ static void resetConf(void) masterConfig.emf_avoidance = 0; // TODO - needs removal - resetPidProfile(¤tProfile->pidProfile); + resetProfile(currentProfile); - for (int rI = 0; rIpidProfile); + resetProfile(currentProfile); break; case MSP_SET_SENSOR_ALIGNMENT: From 718cab04997c278d62c91a562047d5aa953d0fc2 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 26 Jul 2016 23:33:08 +0100 Subject: [PATCH 103/456] Changed CC3D_OPBL build to include LEDs rather than softserial --- src/main/target/CC3D/target.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 091b45b07a..037fe2cf5d 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -72,8 +72,12 @@ #define USE_VCP #define USE_UART1 #define USE_UART3 +#ifdef CC3D_OPBL +#define SERIAL_PORT_COUNT 3 +#else #define USE_SOFTSERIAL1 #define SERIAL_PORT_COUNT 4 +#endif #ifdef USE_UART1_RX_DMA #undef USE_UART1_RX_DMA @@ -110,10 +114,10 @@ #undef GPS #ifdef CC3D_OPBL -#undef LED_STRIP +//#undef LED_STRIP #define SKIP_CLI_COMMAND_HELP #undef BARO -//#undef MAG +#undef MAG #undef SONAR #endif From 469489812f18b5c2a067183f4b5245922962c06d Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Wed, 27 Jul 2016 11:06:45 +1200 Subject: [PATCH 104/456] Added reset of current rateprofile to 'resetProfile'. --- src/main/config/config.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/config/config.c b/src/main/config/config.c index be255eeccc..ea82c79984 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -265,6 +265,8 @@ void resetProfile(profile_t *profile) for (int rI = 0; rIcontrolRateProfile[rI]); } + + profile->activeRateProfile = 0; } #ifdef GPS From eee4d9a14d36efadddecba2eb3d07db653932c00 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Wed, 27 Jul 2016 15:40:16 +1200 Subject: [PATCH 105/456] Changed Vbat hysteresis to be applied only downwards of the battery level, instead of downwards and upwards. --- src/main/sensors/battery.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 342fcc1fdd..75aac6cf0a 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -127,14 +127,14 @@ void updateBattery(void) if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) { batteryState = BATTERY_CRITICAL; beeper(BEEPER_BAT_CRIT_LOW); - } else if (vbat > (batteryWarningVoltage + batteryConfig->vbathysteresis)){ + } else if (vbat > batteryWarningVoltage) { batteryState = BATTERY_OK; } else { beeper(BEEPER_BAT_LOW); } break; case BATTERY_CRITICAL: - if (vbat > (batteryCriticalVoltage + batteryConfig->vbathysteresis)){ + if (vbat > batteryCriticalVoltage) { batteryState = BATTERY_WARNING; beeper(BEEPER_BAT_LOW); } else { From 780aeca3f9553a3a198a3af32e772d144ca4bb96 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 27 Jul 2016 09:04:35 +0100 Subject: [PATCH 106/456] Removed spurious CRLF line ending that was causing problems with git --- src/main/io/serial_4way_avrootloader.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 4ccea1601a..f0f5525813 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -330,7 +330,7 @@ uint8_t BL_WriteFlash(ioMem_t *pMem) return BL_WriteA(CMD_PROG_FLASH, pMem, (40 / START_BIT_TIMEOUT_MS)); } -#endif +#endif #if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_FAKE_ESC) #define FAKE_PAGE_SIZE 512 @@ -877,7 +877,7 @@ uint8_t BL_PageErase(ioMem_t *pMem) uint16_t address = pMem->D_FLASH_ADDR_H << 8 | pMem->D_FLASH_ADDR_L; if (address + FAKE_PAGE_SIZE > FAKE_FLASH_SIZE) return false; - memset(&fakeFlash[address], 0xFF, FAKE_PAGE_SIZE); + memset(&fakeFlash[address], 0xFF, FAKE_PAGE_SIZE); return true; } From b2db6289388e9d3d8d4a705d97b7d6891848001b Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Wed, 27 Jul 2016 15:09:02 +0200 Subject: [PATCH 107/456] git issue --- src/main/io/serial_4way_avrootloader.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 4ccea1601a..20925c88d2 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -330,7 +330,7 @@ uint8_t BL_WriteFlash(ioMem_t *pMem) return BL_WriteA(CMD_PROG_FLASH, pMem, (40 / START_BIT_TIMEOUT_MS)); } -#endif +#endif #if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_FAKE_ESC) #define FAKE_PAGE_SIZE 512 From 3686b8912119d2c07a2475b4c5bf101631180c8b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 27 Jul 2016 15:25:09 +0100 Subject: [PATCH 108/456] Improved formatting of tasks info in cli --- src/main/io/serial_cli.c | 35 +++++++++++++---------------------- 1 file changed, 13 insertions(+), 22 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f852dd490c..34b21217bb 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -469,6 +469,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "PIDLOOP", "NOTCH", }; + #ifdef OSD static const char * const lookupTableOsdType[] = { "AUTO", @@ -2964,7 +2965,7 @@ static void cliStatus(char *cmdline) cliPrintf("CPU Clock=%dMHz", (SystemCoreClock / 1000000)); -#ifndef CJMCU +#if (FLASH_SIZE > 64) uint8_t i; uint32_t mask; uint32_t detectedSensorsMask = sensorsMask(); @@ -3008,44 +3009,34 @@ static void cliTasks(char *cmdline) { UNUSED(cmdline); - cfTaskId_e taskId; - cfTaskInfo_t taskInfo; - - cliPrintf("Task list:\r\n"); - for (taskId = 0; taskId < TASK_COUNT; taskId++) { + cliPrintf("Task list max/us avg/us rate/hz total/ms\r\n"); + for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) { + cfTaskInfo_t taskInfo; getTaskInfo(taskId, &taskInfo); if (taskInfo.isEnabled) { uint16_t taskFrequency; uint16_t subTaskFrequency; - uint32_t taskTotalTime = taskInfo.totalExecutionTime / 1000; - if (taskId == TASK_GYROPID) { subTaskFrequency = (uint16_t)(1.0f / ((float)cycleTime * 0.000001f)); + taskFrequency = subTaskFrequency / masterConfig.pid_process_denom; if (masterConfig.pid_process_denom > 1) { - taskFrequency = subTaskFrequency / masterConfig.pid_process_denom; - cliPrintf("%02d - (%s) ", taskId, taskInfo.taskName); + cliPrintf("%02d - (%12s) ", taskId, taskInfo.taskName); } else { taskFrequency = subTaskFrequency; - cliPrintf("%02d - (%s/%s) ", taskId, taskInfo.subTaskName, taskInfo.taskName); + cliPrintf("%02d - (%8s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName); } } else { taskFrequency = (uint16_t)(1.0f / ((float)taskInfo.latestDeltaTime * 0.000001f)); - cliPrintf("%02d - (%s) ", taskId, taskInfo.taskName); + cliPrintf("%02d - (%12s) ", taskId, taskInfo.taskName); } - - cliPrintf("max: %dus, avg: %dus, rate: %dhz, total: ", taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency); - - if (taskTotalTime >= 1000) { - cliPrintf("%dsec", taskTotalTime / 1000); - } else { - cliPrintf("%dms", taskTotalTime); + cliPrintf("%6d %5d %5d %8d\r\n", taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency, taskInfo.totalExecutionTime / 1000); + if (taskId == TASK_GYROPID && masterConfig.pid_process_denom > 1) { + cliPrintf(" - (%12s) rate: %d\r\n", taskInfo.subTaskName, subTaskFrequency); } - - if (taskId == TASK_GYROPID && masterConfig.pid_process_denom > 1) cliPrintf("\r\n - - (%s) rate: %dhz", taskInfo.subTaskName, subTaskFrequency); - cliPrintf("\r\n", taskTotalTime); } } + cliPrintf("CPU load: %d%%\r\n", constrain(averageSystemLoadPercent, 0, 100)); } #endif From 393fa5afae19ad91ba718237232c0a14c511c28b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 27 Jul 2016 16:59:59 +0200 Subject: [PATCH 109/456] Restore original KISS mapping for PPM --- src/main/target/KISSFC/target.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index f0b36fd605..f9641219f3 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -22,7 +22,7 @@ #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { - PWM12 | (MAP_TO_PPM_INPUT << 8), + PWM7 | (MAP_TO_PPM_INPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), @@ -30,8 +30,10 @@ const uint16_t multiPPM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; @@ -42,7 +44,6 @@ const uint16_t multiPWM[] = { PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM7 | (MAP_TO_PWM_INPUT << 8), PWM8 | (MAP_TO_PWM_INPUT << 8), PWM9 | (MAP_TO_PWM_INPUT << 8), From b5c62df3f8c28847d0661acbc99fdf521f3828ae Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 27 Jul 2016 16:13:14 +0100 Subject: [PATCH 110/456] Added some further build flags for ROM saving --- src/main/drivers/pwm_mapping.c | 6 ++++++ src/main/drivers/pwm_rx.c | 3 +++ src/main/io/rc_controls.c | 6 +++--- src/main/io/rc_controls.h | 1 - src/main/io/serial_msp.c | 7 ++++++- src/main/main.c | 2 ++ src/main/mw.c | 2 +- src/main/rx/msp.c | 3 +++ src/main/rx/pwm.c | 7 +++++-- src/main/rx/rx.c | 10 ++++++++-- 10 files changed, 37 insertions(+), 10 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index d6f41808cb..3cec1ab3ef 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -92,7 +92,9 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) int i = 0; const uint16_t *setup; +#ifndef SKIP_RX_PWM_PPM int channelIndex = 0; +#endif memset(&pwmOutputConfiguration, 0, sizeof(pwmOutputConfiguration)); @@ -289,15 +291,19 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) #endif if (type == MAP_TO_PPM_INPUT) { +#ifndef SKIP_RX_PWM_PPM #if defined(SPARKY) || defined(ALIENFLIGHTF3) if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2); } #endif ppmInConfig(timerHardwarePtr); +#endif } else if (type == MAP_TO_PWM_INPUT) { +#ifndef SKIP_RX_PWM_PPM pwmInConfig(timerHardwarePtr, channelIndex); channelIndex++; +#endif } else if (type == MAP_TO_MOTOR_OUTPUT) { #ifdef CC3D diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index cde900c52b..42f0a2fed0 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -20,6 +20,8 @@ #include +#ifndef SKIP_RX_PWM_PPM + #include "build_config.h" #include "debug.h" @@ -419,3 +421,4 @@ uint16_t pwmRead(uint8_t channel) { return captures[channel]; } +#endif diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index f354577f87..c3ba253b87 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -477,7 +477,7 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU adjustmentState_t adjustmentStates[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT]; -void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) { +static void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adjustmentConfig_t *adjustmentConfig) { adjustmentState_t *adjustmentState = &adjustmentStates[index]; if (adjustmentState->config == adjustmentConfig) { @@ -491,7 +491,7 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj MARK_ADJUSTMENT_FUNCTION_AS_READY(index); } -void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { +static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { int newValue; if (delta > 0) { @@ -600,7 +600,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm }; } -void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) +static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) { bool applied = false; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index be5e7311da..ab7ffcfa1f 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -249,7 +249,6 @@ typedef struct adjustmentState_s { bool isAirmodeActive(void); bool isSuperExpoActive(void); void resetAdjustmentStates(void); -void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 6b484461bf..726c9b38ce 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -517,12 +517,15 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXOSD; +#ifdef TELEMETRY if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch) activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY; - +#endif +#ifdef SONAR if (feature(FEATURE_SONAR)){ activeBoxIds[activeBoxIdCount++] = BOXSONAR; } +#endif #ifdef USE_SERVOS if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) { @@ -1296,6 +1299,7 @@ static bool processInCommand(void) magHold = read16(); break; case MSP_SET_RAW_RC: +#ifndef SKIP_RX_MSP { uint8_t channelCount = currentPort->dataSize / sizeof(uint16_t); if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) { @@ -1310,6 +1314,7 @@ static bool processInCommand(void) rxMspFrameReceive(frame, channelCount); } } +#endif break; case MSP_SET_ACC_TRIM: masterConfig.accelerometerTrims.values.pitch = read16(); diff --git a/src/main/main.c b/src/main/main.c index 63869ab52c..519df60289 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -327,7 +327,9 @@ void init(void) #ifdef CC3D pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false; #endif +#ifndef SKIP_RX_PWM_PPM pwmRxInit(masterConfig.inputFilteringMode); +#endif // pwmInit() needs to be called as soon as possible for ESC compatibility reasons pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); diff --git a/src/main/mw.c b/src/main/mw.c index 404e871dc7..67653fc658 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -451,7 +451,7 @@ void handleInflightCalibrationStickPosition(void) } } -void updateInflightCalibrationState(void) +static void updateInflightCalibrationState(void) { if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !IS_RC_MODE_ACTIVE(BOXARM)) { // Copter is airborne and you are turning it off via boxarm : start measurement InflightcalibratingA = 50; diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index 86399a9094..141d37a773 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -20,6 +20,8 @@ #include "platform.h" +#ifndef SKIP_RX_MSP + #include "build_config.h" #include "drivers/system.h" @@ -71,3 +73,4 @@ void rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadR if (callback) *callback = rxMspReadRawRC; } +#endif diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c index 894eb27a42..517a361e6b 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -21,10 +21,12 @@ #include -#include "build_config.h" - #include "platform.h" +#ifndef SKIP_RX_PWM_PPM + +#include "build_config.h" + #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" @@ -59,4 +61,5 @@ void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfigPtr, rcReadRawDataPtr *callback *callback = ppmReadRawRC; } } +#endif // SKIP_RX_PWM_PPM diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 50fe4f4578..a7a09a44b7 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -189,14 +189,18 @@ void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationCondi } #endif +#ifndef SKIP_RX_MSP if (feature(FEATURE_RX_MSP)) { rxMspInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); } +#endif +#ifndef SKIP_RX_PWM_PPM if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) { rxRefreshRate = 20000; rxPwmInit(&rxRuntimeConfig, &rcReadRawFunc); } +#endif rxRuntimeConfig.auxChannelCount = rxRuntimeConfig.channelCount - STICK_CHANNEL_COUNT; } @@ -349,16 +353,18 @@ void updateRx(uint32_t currentTime) } #endif +#ifndef SKIP_RX_MSP if (feature(FEATURE_RX_MSP)) { rxDataReceived = rxMspFrameComplete(); - if (rxDataReceived) { rxSignalReceived = true; rxIsInFailsafeMode = false; needRxSignalBefore = currentTime + DELAY_5_HZ; } } +#endif +#ifndef SKIP_RX_PWM_PPM if (feature(FEATURE_RX_PPM)) { if (isPPMDataBeingReceived()) { rxSignalReceivedNotDataDriven = true; @@ -375,7 +381,7 @@ void updateRx(uint32_t currentTime) needRxSignalBefore = currentTime + DELAY_10_HZ; } } - +#endif } bool shouldProcessRx(uint32_t currentTime) From 4505623403d7fe833f25417c6191d6f666879879 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 27 Jul 2016 16:50:06 +0100 Subject: [PATCH 111/456] Added average and maximum loads to cli task info --- src/main/io/serial_cli.c | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 34b21217bb..e943a5364f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -3008,14 +3008,16 @@ static void cliStatus(char *cmdline) static void cliTasks(char *cmdline) { UNUSED(cmdline); + int maxLoadSum = 0; + int averageLoadSum = 0; - cliPrintf("Task list max/us avg/us rate/hz total/ms\r\n"); + cliPrintf("Task list max/us avg/us rate/hz maxload avgload total/ms\r\n"); for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) { cfTaskInfo_t taskInfo; getTaskInfo(taskId, &taskInfo); if (taskInfo.isEnabled) { - uint16_t taskFrequency; - uint16_t subTaskFrequency; + int taskFrequency; + int subTaskFrequency; if (taskId == TASK_GYROPID) { subTaskFrequency = (uint16_t)(1.0f / ((float)cycleTime * 0.000001f)); @@ -3030,13 +3032,21 @@ static void cliTasks(char *cmdline) taskFrequency = (uint16_t)(1.0f / ((float)taskInfo.latestDeltaTime * 0.000001f)); cliPrintf("%02d - (%12s) ", taskId, taskInfo.taskName); } - cliPrintf("%6d %5d %5d %8d\r\n", taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency, taskInfo.totalExecutionTime / 1000); + const int maxLoad = (taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000; + const int averageLoad = (taskInfo.averageExecutionTime * taskFrequency + 5000) / 1000; + if (taskId != TASK_SERIAL) { + maxLoadSum += maxLoad; + averageLoadSum += averageLoad; + } + cliPrintf("%6d %5d %5d %4d.%1d%% %4d.%1d%% %8d\r\n", + taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency, + maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTime / 1000); if (taskId == TASK_GYROPID && masterConfig.pid_process_denom > 1) { cliPrintf(" - (%12s) rate: %d\r\n", taskInfo.subTaskName, subTaskFrequency); } } } - cliPrintf("CPU load: %d%%\r\n", constrain(averageSystemLoadPercent, 0, 100)); + cliPrintf("Total (excluding SERIAL) %22d.%1d%% %4d.%1d%%\r\n", maxLoadSum/10, maxLoadSum%10, averageLoadSum/10, averageLoadSum%10); } #endif From 4a769eb55f244bb937a7a4b8000106000f6f061d Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 27 Jul 2016 17:12:16 +0100 Subject: [PATCH 112/456] Set default acc rate to 1000Hz, for all targets --- src/main/main.c | 24 ++++++++++++------------ src/main/scheduler/scheduler_tasks.c | 2 +- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index 519df60289..1b06c200cf 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -690,21 +690,21 @@ void main_init(void) rescheduleTask(TASK_GYROPID, gyro.targetLooptime); setTaskEnabled(TASK_GYROPID, true); - if(sensors(SENSOR_ACC)) { + if (sensors(SENSOR_ACC)) { setTaskEnabled(TASK_ACCEL, true); - switch(gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future - case(500): - case(375): - case(250): - case(125): - accTargetLooptime = 1000; - break; - default: - case(1000): + switch (gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future + case 500: + case 375: + case 250: + case 125: + accTargetLooptime = 1000; + break; + default: + case 1000: #ifdef STM32F10X - accTargetLooptime = 3000; + accTargetLooptime = 1000; #else - accTargetLooptime = 1000; + accTargetLooptime = 1000; #endif } rescheduleTask(TASK_ACCEL, accTargetLooptime); diff --git a/src/main/scheduler/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c index d7852c55bf..3699442c96 100644 --- a/src/main/scheduler/scheduler_tasks.c +++ b/src/main/scheduler/scheduler_tasks.c @@ -43,7 +43,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_ACCEL] = { .taskName = "ACCEL", .taskFunc = taskUpdateAccelerometer, - .desiredPeriod = 1000000 / 100, // every 10ms + .desiredPeriod = 1000000 / 1000, // every 1ms .staticPriority = TASK_PRIORITY_MEDIUM, }, From 4272bf6b6ed397dd6767eb727c6e8eefbaa2f433 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 27 Jul 2016 17:20:02 +0100 Subject: [PATCH 113/456] Added back soft serial into CC3D_OPBL target --- src/main/target/CC3D/target.h | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 037fe2cf5d..03d10272e4 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -72,12 +72,8 @@ #define USE_VCP #define USE_UART1 #define USE_UART3 -#ifdef CC3D_OPBL -#define SERIAL_PORT_COUNT 3 -#else #define USE_SOFTSERIAL1 #define SERIAL_PORT_COUNT 4 -#endif #ifdef USE_UART1_RX_DMA #undef USE_UART1_RX_DMA @@ -116,6 +112,7 @@ #ifdef CC3D_OPBL //#undef LED_STRIP #define SKIP_CLI_COMMAND_HELP +#define SKIP_PID_FLOAT #undef BARO #undef MAG #undef SONAR From 6691eafaf568a328b1def705810151b9b0ba431d Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Wed, 27 Jul 2016 21:17:18 +0200 Subject: [PATCH 114/456] delete space to see all rate columns in a row --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e943a5364f..b1bf053163 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -3042,7 +3042,7 @@ static void cliTasks(char *cmdline) taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency, maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTime / 1000); if (taskId == TASK_GYROPID && masterConfig.pid_process_denom > 1) { - cliPrintf(" - (%12s) rate: %d\r\n", taskInfo.subTaskName, subTaskFrequency); + cliPrintf(" - (%12s) rate: %d\r\n", taskInfo.subTaskName, subTaskFrequency); } } } From 167aaade9b06f5321445da20d4c7ffff1f846def Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 27 Jul 2016 23:16:43 +0200 Subject: [PATCH 115/456] F3 UART IO fix --- src/main/drivers/serial_uart_stm32f30x.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 852a60831e..29bebfb954 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -132,7 +132,7 @@ void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, ui } if (mode & MODE_RX) { - IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TX, index); + IOInit(rx, OWNER_SERIAL, RESOURCE_UART_RX, index); IOConfigGPIOAF(rx, ioCfg, af); } } From c11c50287ea0b85dad7e17e9e9504f0f79225589 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 27 Jul 2016 14:30:51 +0200 Subject: [PATCH 116/456] Finalisation of Betaflight PID controller // RC smoothing added back --- src/main/config/config.c | 14 ++-- src/main/debug.h | 3 + src/main/flight/mixer.c | 23 ------ src/main/flight/pid.c | 152 +++++++++++++++++++------------------- src/main/flight/pid.h | 11 ++- src/main/io/rc_controls.h | 7 ++ src/main/io/serial_cli.c | 21 +++++- src/main/io/serial_msp.c | 4 +- src/main/mw.c | 103 +++++++++++++++----------- src/main/rx/rx.h | 1 + 10 files changed, 186 insertions(+), 153 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index ea82c79984..6669dd5b07 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -239,11 +239,14 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; // Betaflight PID controller parameters + pidProfile->ptermSetpointWeight = 80; + pidProfile->dtermSetpointWeight = 0; + pidProfile->pidMaxVelocity = 1000; + pidProfile->pidMaxVelocityYaw = 50; pidProfile->toleranceBand = 15; - pidProfile->toleranceBandReduction = 35; - pidProfile->zeroCrossAllowanceCount = 3; - pidProfile->accelerationLimitPercent = 20; - pidProfile->itermThrottleGain = 10; + pidProfile->toleranceBandReduction = 40; + pidProfile->zeroCrossAllowanceCount = 2; + pidProfile->itermThrottleGain = 15; #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. @@ -524,7 +527,8 @@ static void resetConf(void) masterConfig.rxConfig.rssi_channel = 0; masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; masterConfig.rxConfig.rssi_ppm_invert = 0; - masterConfig.rxConfig.rcSmoothInterval = 0; // 0 is predefined + masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_AUTO; + masterConfig.rxConfig.rcSmoothInterval = 9; masterConfig.rxConfig.fpvCamAngleDegrees = 0; #ifdef STM32F4 masterConfig.rxConfig.max_aux_channel = 99; diff --git a/src/main/debug.h b/src/main/debug.h index 19124266d0..2e2a90e89c 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -51,5 +51,8 @@ typedef enum { DEBUG_AIRMODE, DEBUG_PIDLOOP, DEBUG_NOTCH, + DEBUG_RC_SMOOTHING, + DEBUG_VELOCITY, + DEBUG_DTERM_FILTER, DEBUG_COUNT } debugType_e; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f8ac94b22b..303f6960bc 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -835,34 +835,11 @@ void mixTable(void *pidProfilePtr) throttleMax = throttleMax - (rollPitchYawMixRange / 2); } - // Keep track for motor update timing // Only for Betaflight pid controller to keep legacy pidc basic and free from additional float math - float motorDtms; - if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) { - static uint32_t previousMotorTime; - uint32_t currentMotorTime = micros(); - motorDtms = (float) (currentMotorTime - previousMotorTime) / 1000.0f; - previousMotorTime = currentMotorTime; - } - // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (i = 0; i < motorCount; i++) { motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax); - // Accel limit. Prevent PID controller to output huge ramps to the motors. Only limiting acceleration. Only for Betaflight pid controller to keep legacy pidc basic - if (pidProfile->accelerationLimitPercent && pidProfile->pidController == PID_CONTROLLER_BETAFLIGHT) { - static int16_t lastFilteredMotor[MAX_SUPPORTED_MOTORS]; - - // acceleration limit - float delta = motor[i] - lastFilteredMotor[i]; - const float maxDeltaPerMs = throttleRange * ((float)pidProfile->accelerationLimitPercent / 100.0f); - float maxDelta = maxDeltaPerMs * motorDtms; - if (delta > maxDelta) { // accelerating too hard - motor[i] = lastFilteredMotor[i] + maxDelta; - } - lastFilteredMotor[i] = motor[i]; - } - if (isFailsafeActive) { motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); } else if (feature(FEATURE_3D)) { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e1d408e5c6..16aa6e45ac 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -49,7 +49,8 @@ extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float angleRate[3], angleRateSmooth[3]; +extern float setpointRate[3]; +extern float rcInput[3]; static bool pidStabilisationEnabled; @@ -106,25 +107,23 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static pt1Filter_t deltaFilter[3]; static pt1Filter_t yawFilter; -#ifndef SKIP_PID_FLOAT -// Experimental betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage +// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab) static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) { - float RateError = 0, gyroRate = 0, RateErrorSmooth = 0; + float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; float delta; int axis; float horizonLevelStrength = 1; + static int16_t axisPIDState[3]; + static float velocityWindupFactor[3] = { 1.0f, 1.0f, 1.0f }; + + const float velocityFactor = getdT() * 1000.0f; float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float - // Scaling factors for Pids for better tunable range in configurator - static const float PTermScale = 0.032029f; - static const float ITermScale = 0.244381f; - static const float DTermScale = 0.000529f; - if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); @@ -142,7 +141,6 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // Yet Highly experimental and under test and development // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) static float kiThrottleGain = 1.0f; - if (pidProfile->itermThrottleGain) { const uint16_t maxLoopCount = 20000 / targetPidLooptime; const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; @@ -161,126 +159,136 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { + // Prepare all parameters for PID controller + float Kp = PTERM_SCALE * pidProfile->P8[axis]; + float Ki = ITERM_SCALE * pidProfile->I8[axis]; + float Kd = DTERM_SCALE * pidProfile->D8[axis]; + float b = pidProfile->ptermSetpointWeight / 100.0f; + float c = pidProfile->dtermSetpointWeight / 100.0f; + float velocityMax = (axis == YAW) ? (float)pidProfile->pidMaxVelocityYaw * velocityFactor : (float)pidProfile->pidMaxVelocity * velocityFactor; + // Yaw control is GYRO based, direct sticks control is applied to rate PID if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { // calculate error angle and limit the angle to the max inclination #ifdef GPS - const float errorAngle = (constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination), + const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #else - const float errorAngle = (constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination), + const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here #endif if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed - angleRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; + setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; } else { // HORIZON mode - direct sticks control is applied to rate PID // mix up angle error to desired AngleRate to add a little auto-level feel - angleRate[axis] = angleRateSmooth[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); + setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); } } - gyroRate = gyroADCf[axis] / 16.4f; // gyro output deg/sec + PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec - // --------low-level gyro-based PID. ---------- + // --------low-level gyro-based PID based on 2DOF PID controller. ---------- + // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // -----calculate scaled error.AngleRates - // multiplication of rcCommand corresponds to changing the sticks scaling here - RateError = angleRate[axis] - gyroRate; + // ----- calculate error / angle rates ---------- + errorRate = setpointRate[axis] - PVRate; // r - y + rP = b * setpointRate[axis] - PVRate; // br - y + rD = c * setpointRate[axis] - PVRate; // cr - y + + // Slowly restore original setpoint with more stick input + float diffRate = errorRate - rP; + rP += diffRate * rcInput[axis]; - float dynReduction = tpaFactor; // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount + float dynReduction = tpaFactor; if (pidProfile->toleranceBand) { const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; static uint8_t zeroCrossCount[3]; static uint8_t currentErrorPolarity[3]; - if (ABS(RateError) < pidProfile->toleranceBand) { + if (ABS(errorRate) < pidProfile->toleranceBand) { if (zeroCrossCount[axis]) { if (currentErrorPolarity[axis] == POSITIVE_ERROR) { - if (RateError < 0 ) { + if (errorRate < 0 ) { zeroCrossCount[axis]--; currentErrorPolarity[axis] = NEGATIVE_ERROR; } } else { - if (RateError > 0 ) { + if (errorRate > 0 ) { zeroCrossCount[axis]--; currentErrorPolarity[axis] = POSITIVE_ERROR; } } } else { - dynReduction *= constrainf(ABS(RateError) / pidProfile->toleranceBand, minReduction, 1.0f); + dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); } } else { zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; - currentErrorPolarity[axis] = (RateError > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; + currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; } } - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - // Smoothed Error for Derivative when delta from error selected - if (flightModeFlags && axis != YAW) - RateErrorSmooth = RateError; - else - RateErrorSmooth = angleRateSmooth[axis] - gyroRate; - } - // -----calculate P component - PTerm = PTermScale * RateError * pidProfile->P8[axis] * dynReduction; - - // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply - if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { - PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); - } + PTerm = Kp * rP * dynReduction; // -----calculate I component. - // Prevent strong Iterm accumulation during stick inputs + // Reduce strong Iterm accumulation during higher stick inputs float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float antiWindupScaler = constrainf(1.0f - (1.5f * (ABS(angleRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); + float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + ITermScale * RateError * getdT() * pidProfile->I8[axis] * antiWindupScaler * kiThrottleGain, -250.0f, 250.0f); + // Handle All windup Scenarios + // limit maximum integrator value to prevent WindUp + float itermScaler = setpointRateScaler * kiThrottleGain * velocityWindupFactor[axis]; + + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki * errorRate * getdT() * itermScaler, -250.0f, 250.0f); - // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings ITerm = errorGyroIf[axis]; - //-----calculate D-term + //-----calculate D-term (Yaw D not yet supported) if (axis == YAW) { if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); axisPID[axis] = lrintf(PTerm + ITerm); - if (motorCount >= 4) { - int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); - - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); - } - DTerm = 0.0f; // needed for blackbox } else { - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateErrorSmooth - lastRateError[axis]; - lastRateError[axis] = RateErrorSmooth; - } else { - delta = -(gyroRate - lastRateError[axis]); - lastRateError[axis] = gyroRate; - } + delta = rD - lastRateError[axis]; + lastRateError[axis] = rD; // Divide delta by targetLooptime to get differential (ie dr/dt) delta *= (1.0f / getdT()); + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd * delta * dynReduction; + // Filter delta if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - DTerm = constrainf(DTermScale * delta * (float)pidProfile->D8[axis] * dynReduction, -300.0f, 300.0f); + DTerm = constrainf(Kd * delta * dynReduction, -300.0f, 300.0f); // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); } + // Disable PID control at zero throttle if (!pidStabilisationEnabled) axisPID[axis] = 0; + // Velocity limit only active below 1000 + if (velocityMax < 1000) { + int16_t currentVelocity = axisPID[axis] - axisPIDState[axis]; + if (debugMode == DEBUG_VELOCITY) debug[axis] = currentVelocity; + if (ABS(currentVelocity) > velocityMax) { + axisPID[axis] = (currentVelocity > 0) ? axisPIDState[axis] + velocityMax : axisPIDState[axis] - velocityMax; + velocityWindupFactor[axis] = ABS(currentVelocity) / velocityMax; + } + else { + velocityWindupFactor[axis] = 1.0f; + } + + axisPIDState[axis] = axisPID[axis]; + } + #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); @@ -294,7 +302,6 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc #endif } } -#endif // Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, @@ -303,7 +310,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina int axis; int32_t PTerm, ITerm, DTerm, delta; static int32_t lastRateError[3]; - int32_t AngleRateTmp = 0, AngleRateTmpSmooth = 0, RateError = 0, gyroRate = 0, RateErrorSmooth = 0; + int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; int8_t horizonLevelStrength = 100; @@ -323,16 +330,15 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina for (axis = 0; axis < 3; axis++) { // -----Get the desired angle rate depending on flight mode - AngleRateTmp = (int32_t)angleRate[axis]; - if (axis != YAW) AngleRateTmpSmooth = (int32_t)angleRateSmooth[axis]; + AngleRateTmp = (int32_t)setpointRate[axis]; if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { // calculate error angle and limit the angle to max configured inclination #ifdef GPS - const int32_t errorAngle = constrain(2 * rcCommandSmooth[axis] + GPS_angle[axis], -((int) max_angle_inclination), + const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #else - const int32_t errorAngle = constrain(2 * rcCommandSmooth[axis], -((int) max_angle_inclination), + const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; #endif if (FLIGHT_MODE(ANGLE_MODE)) { @@ -341,7 +347,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina } else { // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, // horizonLevelStrength is scaled to the stick input - AngleRateTmp = AngleRateTmpSmooth + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); + AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); } } @@ -353,14 +359,6 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina RateError = AngleRateTmp - gyroRate; - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - // Smoothed Error for Derivative when delta from error selected - if (flightModeFlags && axis != YAW) - RateErrorSmooth = RateError; - else - RateErrorSmooth = AngleRateTmpSmooth - gyroRate; - } - // -----calculate P component PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; @@ -403,8 +401,8 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina DTerm = 0; // needed for blackbox } else { if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateErrorSmooth - lastRateError[axis]; - lastRateError[axis] = RateErrorSmooth; + delta = RateError - lastRateError[axis]; + lastRateError[axis] = RateError; } else { delta = -(gyroRate - lastRateError[axis]); lastRateError[axis] = gyroRate; @@ -413,6 +411,8 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina // Divide delta by targetLooptime to get differential (ie dr/dt) delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + // Filter delta if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT()); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 64182f8256..e4fe96ce89 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -26,6 +26,12 @@ #define DYNAMIC_PTERM_STICK_THRESHOLD 400 + +// Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float +#define PTERM_SCALE 0.032029f +#define ITERM_SCALE 0.244381f +#define DTERM_SCALE 0.000529f + typedef enum { PIDROLL, PIDPITCH, @@ -88,8 +94,11 @@ typedef struct pidProfile_s { uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances uint8_t toleranceBandReduction; // Lowest possible P and D reduction in percentage uint8_t zeroCrossAllowanceCount; // Amount of bouncebacks within tolerance band allowed before reduction kicks in - uint16_t accelerationLimitPercent; // Percentage that motor is allowed to increase or decrease in a period of 1ms uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm + uint8_t ptermSetpointWeight; // Setpoint weight for Pterm (lower means more PV tracking) + uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) + uint16_t pidMaxVelocity; // velocity limiter for pid controller (per ms) + uint16_t pidMaxVelocityYaw; // velocity limiter for pid controller (per ms) yaw #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index ab7ffcfa1f..aace839d9b 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -86,6 +86,13 @@ typedef enum { CENTERED } rollPitchStatus_e; +typedef enum { + RC_SMOOTHING_OFF = 0, + RC_SMOOTHING_DEFAULT, + RC_SMOOTHING_AUTO, + RC_SMOOTHING_MANUAL +} rcSmoothing_t; + #define ROL_LO (1 << (2 * ROLL)) #define ROL_CE (3 << (2 * ROLL)) #define ROL_HI (2 << (2 * ROLL)) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b1bf053163..78df13d917 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -468,6 +468,9 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "AIRMODE", "PIDLOOP", "NOTCH", + "RC_SMOOTHING", + "VELOCITY", + "DFILTER", }; #ifdef OSD @@ -486,10 +489,14 @@ static const char * const lookupTablePwmProtocol[] = { "OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED" }; -static const char * const lookupDeltaMethod[] = { +static const char * const lookupTableDeltaMethod[] = { "ERROR", "MEASUREMENT" }; +static const char * const lookupTableRcSmoothing[] = { + "OFF", "DEFAULT", "AUTO", "MANUAL" +}; + typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -526,6 +533,7 @@ typedef enum { TABLE_SUPEREXPO_YAW, TABLE_MOTOR_PWM_PROTOCOL, TABLE_DELTA_METHOD, + TABLE_RC_SMOOTHING, #ifdef OSD TABLE_OSD, #endif @@ -561,7 +569,8 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }, { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }, { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) }, - { lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) }, + { lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) }, + { lookupTableRcSmoothing, sizeof(lookupTableRcSmoothing) / sizeof(char *) }, #ifdef OSD { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, #endif @@ -621,7 +630,8 @@ const clivalue_t valueTable[] = { { "max_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, { "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } }, - { "rc_smooth_interval_ms", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rcSmoothInterval, .config.minmax = { 0, 255 } }, + { "rc_smoothing", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_RC_SMOOTHING } }, + { "rc_smooth_interval_ms", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rcSmoothInterval, .config.minmax = { 1, 50 } }, { "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } }, { "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON } }, { "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } }, @@ -807,11 +817,14 @@ const clivalue_t valueTable[] = { { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } }, - { "motor_accel_limit_percent", VAR_UINT16 | MASTER_VALUE, &masterConfig.profile[0].pidProfile.accelerationLimitPercent, .config.minmax = { 0, 10000 } }, { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, + { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } }, + { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 200 } }, + { "pid_max_velocity", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocity, .config.minmax = {0, 2000 } }, + { "pid_max_velocity_yaw", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 2000 } }, { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 726c9b38ce..df47e70359 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1248,7 +1248,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentControlRateProfile->rcYawRate8); serialize16(masterConfig.rxConfig.airModeActivateThreshold); serialize8(masterConfig.rxConfig.rcSmoothInterval); - serialize16(currentProfile->pidProfile.accelerationLimitPercent); + serialize16(currentProfile->pidProfile.pidMaxVelocity); break; case MSP_SENSOR_CONFIG: headSerialReply(3); @@ -1829,7 +1829,7 @@ static bool processInCommand(void) currentControlRateProfile->rcYawRate8 = read8(); masterConfig.rxConfig.airModeActivateThreshold = read16(); masterConfig.rxConfig.rcSmoothInterval = read8(); - currentProfile->pidProfile.accelerationLimitPercent = read16(); + read16(); break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); diff --git a/src/main/mw.c b/src/main/mw.c index 67653fc658..073e795017 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -123,7 +123,8 @@ extern uint8_t PIDweight[3]; uint16_t filteredCycleTime; static bool isRXDataNew; static bool armingCalibrationWasInitialised; -float angleRate[3], angleRateSmooth[3]; +float setpointRate[3]; +float rcInput[3]; extern pidControllerFuncPtr pid_controller; @@ -172,12 +173,12 @@ bool isCalibrating() return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } -float calculateRate(int axis, int16_t rc) { +float calculateSetpointRate(int axis, int16_t rc) { float angleRate; if (isSuperExpoActive()) { - float rcFactor = (axis == YAW) ? (ABS(rc) / (500.0f * (currentControlRateProfile->rcYawRate8 / 100.0f))) : (ABS(rc) / (500.0f * (currentControlRateProfile->rcRate8 / 100.0f))); - rcFactor = 1.0f / (constrainf(1.0f - (rcFactor * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); + rcInput[axis] = (axis == YAW) ? (ABS(rc) / (500.0f * (currentControlRateProfile->rcYawRate8 / 100.0f))) : (ABS(rc) / (500.0f * (currentControlRateProfile->rcRate8 / 100.0f))); + float rcFactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); angleRate = rcFactor * ((27 * rc) / 16.0f); } else { @@ -202,10 +203,10 @@ void scaleRcCommandToFpvCamAngle(void) { sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD); } - int16_t roll = angleRate[ROLL]; - int16_t yaw = angleRate[YAW]; - angleRate[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500); - angleRate[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500); + int16_t roll = setpointRate[ROLL]; + int16_t yaw = setpointRate[YAW]; + setpointRate[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500); + setpointRate[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500); } void processRcCommand(void) @@ -214,44 +215,62 @@ void processRcCommand(void) static int16_t deltaRC[4] = { 0, 0, 0, 0 }; static int16_t factor, rcInterpolationFactor; uint16_t rxRefreshRate; - int axis; + bool readyToCalculateRate = false; - // Set RC refresh rate for sampling and channels to filter - if (masterConfig.rxConfig.rcSmoothInterval) { - rxRefreshRate = 1000 * masterConfig.rxConfig.rcSmoothInterval; + if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) { + if (isRXDataNew) { + // Set RC refresh rate for sampling and channels to filter + switch (masterConfig.rxConfig.rcSmoothing) { + case(RC_SMOOTHING_AUTO): + rxRefreshRate = constrain(getTaskDeltaTime(TASK_RX), 1000, 20000) + 1000; // Add slight overhead to prevent ramps + break; + case(RC_SMOOTHING_MANUAL): + rxRefreshRate = 1000 * masterConfig.rxConfig.rcSmoothInterval; + break; + case(RC_SMOOTHING_OFF): + case(RC_SMOOTHING_DEFAULT): + default: + initRxRefreshRate(&rxRefreshRate); + } + + rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; + + if (debugMode == DEBUG_RC_SMOOTHING) { + for (int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis]; + debug[3] = rxRefreshRate; + } + + isRXDataNew = false; + for (int channel=0; channel < 4; channel++) { + deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); + lastCommand[channel] = rcCommand[channel]; + } + + factor = rcInterpolationFactor - 1; + } else { + factor--; + } + + // Interpolate steps of rcCommand + if (factor > 0) { + for (int channel=0; channel < 4; channel++) rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; + } else { + factor = 0; + } + + readyToCalculateRate = true; } else { - initRxRefreshRate(&rxRefreshRate); + factor = 0; // reset factor in case of level modes flip flopping } - rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; + if (readyToCalculateRate || isRXDataNew) { + isRXDataNew = false; - if (isRXDataNew) { - for (axis = 0; axis < 3; axis++) angleRate[axis] = calculateRate(axis, rcCommand[axis]); + for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) - if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) { + if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) scaleRcCommandToFpvCamAngle(); - } - - for (int channel=0; channel < 4; channel++) { - deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); - lastCommand[channel] = rcCommand[channel]; - } - - isRXDataNew = false; - factor = rcInterpolationFactor - 1; - } else { - factor--; - } - - // Interpolate steps of rcCommand - if (factor > 0) { - for (int channel=0; channel < 4; channel++) { - rcCommandSmooth[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; - } - for (axis = 0; axis < 2; axis++) angleRateSmooth[axis] = calculateRate(axis, rcCommandSmooth[axis]); - } else { - factor = 0; } } @@ -688,8 +707,6 @@ void subTaskMainSubprocesses(void) { const uint32_t startTime = micros(); - processRcCommand(); - // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) { gyro.temperature(&telemTemperature1); @@ -728,14 +745,16 @@ void subTaskMainSubprocesses(void) { && masterConfig.mixerMode != MIXER_FLYING_WING #endif ) { - rcCommand[YAW] = rcCommandSmooth[YAW] = 0; - angleRate[YAW] = angleRateSmooth[YAW] = 0; + rcCommand[YAW] = 0; + setpointRate[YAW] = 0; } if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttle_correction_value); } + processRcCommand(); + #ifdef GPS if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 1cec24053a..60625f608c 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -121,6 +121,7 @@ typedef struct rxConfig_s { uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t mincheck; // minimum rc end uint16_t maxcheck; // maximum rc end + uint8_t rcSmoothing; uint8_t rcSmoothInterval; uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands uint8_t max_aux_channel; From 6dcefedde66a21f55907a98b38b858f51505dc3e Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 28 Jul 2016 01:15:11 +0200 Subject: [PATCH 117/456] Add new MSP parameters --- src/main/io/msp_protocol.h | 6 ++++++ src/main/io/serial_msp.c | 28 ++++++++++++++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/src/main/io/msp_protocol.h b/src/main/io/msp_protocol.h index 66ab4b9c58..1244186ffe 100644 --- a/src/main/io/msp_protocol.h +++ b/src/main/io/msp_protocol.h @@ -213,6 +213,12 @@ #define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility #define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility +#define MSP_PIDC_BETAFLIGHT 246 +#define MSP_SET_PIDC_BETAFLIGHT 247 + +#define MSP_RC_SMOOTHING 248 +#define MSP_SET_RC_SMOOTHING 249 + // // OSD specific // diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index df47e70359..3e897532e0 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1250,6 +1250,21 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.rxConfig.rcSmoothInterval); serialize16(currentProfile->pidProfile.pidMaxVelocity); break; + case MSP_PIDC_BETAFLIGHT: + headSerialReply(9); + serialize8(currentProfile->pidProfile.ptermSetpointWeight); + serialize8(currentProfile->pidProfile.dtermSetpointWeight); + serialize8(currentProfile->pidProfile.toleranceBand); + serialize8(currentProfile->pidProfile.toleranceBandReduction); + serialize8(currentProfile->pidProfile.itermThrottleGain); + serialize16(currentProfile->pidProfile.pidMaxVelocity); + serialize16(currentProfile->pidProfile.pidMaxVelocityYaw); + break; + case MSP_RC_SMOOTHING: + headSerialReply(2); + serialize8(masterConfig.rxConfig.rcSmoothing); + serialize8(masterConfig.rxConfig.rcSmoothInterval); + break; case MSP_SENSOR_CONFIG: headSerialReply(3); serialize8(masterConfig.acc_hardware); @@ -1831,6 +1846,19 @@ static bool processInCommand(void) masterConfig.rxConfig.rcSmoothInterval = read8(); read16(); break; + case MSP_SET_PIDC_BETAFLIGHT: + currentProfile->pidProfile.ptermSetpointWeight = read8(); + currentProfile->pidProfile.dtermSetpointWeight = read8(); + currentProfile->pidProfile.toleranceBand = read8(); + currentProfile->pidProfile.toleranceBandReduction = read8(); + currentProfile->pidProfile.itermThrottleGain = read8(); + currentProfile->pidProfile.pidMaxVelocity = read16(); + currentProfile->pidProfile.pidMaxVelocityYaw = read16(); + break; + case MSP_SET_RC_SMOOTHING: + masterConfig.rxConfig.rcSmoothing = read8(); + masterConfig.rxConfig.rcSmoothInterval = read8(); + break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); masterConfig.baro_hardware = read8(); From 5c1c64fc4141f75db12f016dbc61b1b9fefe5713 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 28 Jul 2016 00:40:04 +0100 Subject: [PATCH 118/456] Avoids initialisation of UART2 when PWM or PPM RX is used --- src/main/io/serial.c | 8 +++++++- src/main/io/serial.h | 1 + src/main/main.c | 8 ++++++-- src/main/telemetry/smartport.c | 2 -- 4 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 15aae3e469..0f94c7bc15 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -364,7 +364,7 @@ void closeSerialPort(serialPort_t *serialPort) { serialPortUsage->serialPort = NULL; } -void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled) +void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable) { uint8_t index; @@ -376,6 +376,12 @@ void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled) for (index = 0; index < SERIAL_PORT_COUNT; index++) { serialPortUsageList[index].identifier = serialPortIdentifiers[index]; + if (serialPortToDisable != SERIAL_PORT_NONE) { + if (serialPortUsageList[index].identifier == serialPortToDisable) { + serialPortUsageList[index].identifier = SERIAL_PORT_NONE; + serialPortCount--; + } + } if (!softserialEnabled) { if (0 #ifdef USE_SOFTSERIAL1 diff --git a/src/main/io/serial.h b/src/main/io/serial.h index ccee8d1501..077ad07293 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -100,6 +100,7 @@ typedef void serialConsumer(uint8_t); // // configuration // +void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); void serialRemovePort(serialPortIdentifier_e identifier); uint8_t serialGetAvailablePortCount(void); bool serialIsPortAvailable(serialPortIdentifier_e identifier); diff --git a/src/main/main.c b/src/main/main.c index 1b06c200cf..96bd510bf9 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -118,7 +118,6 @@ serialPort_t *loopbackPort; void printfSupportInit(void); void timerInit(void); void telemetryInit(void); -void serialInit(serialConfig_t *initialSerialConfig, bool softserialEnabled); void mspInit(serialConfig_t *serialConfig); void cliInit(serialConfig_t *serialConfig); void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle); @@ -248,7 +247,12 @@ void init(void) dmaInit(); - serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL)); +#ifdef AVOID_UART2_PWM_PPM + serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), + feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); +#else + serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); +#endif #ifdef USE_SERVOS mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 28daadc40b..03f257d05f 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -151,8 +151,6 @@ static telemetryConfig_t *telemetryConfig; static bool smartPortTelemetryEnabled = false; static portSharing_e smartPortPortSharing; -extern void serialInit(serialConfig_t *); // from main.c // FIXME remove this dependency - char smartPortState = SPSTATE_UNINITIALIZED; static uint8_t smartPortHasRequest = 0; static uint8_t smartPortIdCnt = 0; From e85da1ecfb2fd724e39c06625e748c8b0fef5305 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 28 Jul 2016 01:00:24 +0100 Subject: [PATCH 119/456] Changed name of #define --- src/main/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/main.c b/src/main/main.c index 96bd510bf9..06e2be9ee5 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -247,7 +247,7 @@ void init(void) dmaInit(); -#ifdef AVOID_UART2_PWM_PPM +#ifdef AVOID_UART2_FOR_PWM_PPM serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); #else From 4541f003b854c00852d751ccb674ab9ee743955c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 28 Jul 2016 09:13:27 +0200 Subject: [PATCH 120/456] Add avoiding of UART2 for KISS target --- src/main/target/KISSFC/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 7224db6b6a..05346145d6 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -73,6 +73,8 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART2 +#define AVOID_UART2_FOR_PWM_PPM + #define SPEKTRUM_BIND #define BIND_PIN PB4 From edb35a024c1340ee22fec3763d48a6ac25a888df Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 28 Jul 2016 09:26:56 +0200 Subject: [PATCH 121/456] Avoid UART3 for PWM/PPM for ALIENWIIF3 and SPARKY --- src/main/main.c | 5 ++++- src/main/target/ALIENFLIGHTF3/target.h | 1 + src/main/target/SPARKY/target.h | 1 + 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/main.c b/src/main/main.c index 06e2be9ee5..15d2b6cbab 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -247,9 +247,12 @@ void init(void) dmaInit(); -#ifdef AVOID_UART2_FOR_PWM_PPM +#if defined(AVOID_UART2_FOR_PWM_PPM) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART2 : SERIAL_PORT_NONE); +#elif defined(AVOID_UART3_FOR_PWM_PPM) + serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), + feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART3 : SERIAL_PORT_NONE); #else serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 8ce5876683..ce9704fdf3 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -72,6 +72,7 @@ #define USE_UART2 // Receiver - RX (PA3) #define USE_UART3 // Not connected - 10/RX (PB11) 11/TX (PB10) #define SERIAL_PORT_COUNT 4 +#define AVOID_UART3_FOR_PWM_PPM #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index ab498ff5ac..9dbce83c11 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -56,6 +56,7 @@ #define USE_UART2 // Input - RX (PA3) #define USE_UART3 // Servo out - 10/RX (PB11) 11/TX (PB10) #define SERIAL_PORT_COUNT 4 +#define AVOID_UART3_FOR_PWM_PPM #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 From 0cc07e75fe6744c1d1652c358f95dca10d93b97c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 28 Jul 2016 09:11:03 +0100 Subject: [PATCH 122/456] Improved formatting of cli tasks --- src/main/io/serial_cli.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 78df13d917..9ac1946b19 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -3024,16 +3024,15 @@ static void cliTasks(char *cmdline) int maxLoadSum = 0; int averageLoadSum = 0; - cliPrintf("Task list max/us avg/us rate/hz maxload avgload total/ms\r\n"); + cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n"); for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) { cfTaskInfo_t taskInfo; getTaskInfo(taskId, &taskInfo); if (taskInfo.isEnabled) { int taskFrequency; int subTaskFrequency; - if (taskId == TASK_GYROPID) { - subTaskFrequency = (uint16_t)(1.0f / ((float)cycleTime * 0.000001f)); + subTaskFrequency = (int)(1000000.0f / ((float)cycleTime)); taskFrequency = subTaskFrequency / masterConfig.pid_process_denom; if (masterConfig.pid_process_denom > 1) { cliPrintf("%02d - (%12s) ", taskId, taskInfo.taskName); @@ -3042,7 +3041,7 @@ static void cliTasks(char *cmdline) cliPrintf("%02d - (%8s/%3s) ", taskId, taskInfo.subTaskName, taskInfo.taskName); } } else { - taskFrequency = (uint16_t)(1.0f / ((float)taskInfo.latestDeltaTime * 0.000001f)); + taskFrequency = (int)(1000000.0f / ((float)taskInfo.latestDeltaTime)); cliPrintf("%02d - (%12s) ", taskId, taskInfo.taskName); } const int maxLoad = (taskInfo.maxExecutionTime * taskFrequency + 5000) / 1000; @@ -3051,11 +3050,11 @@ static void cliTasks(char *cmdline) maxLoadSum += maxLoad; averageLoadSum += averageLoad; } - cliPrintf("%6d %5d %5d %4d.%1d%% %4d.%1d%% %8d\r\n", - taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, taskFrequency, + cliPrintf("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d\r\n", + taskFrequency, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTime / 1000); if (taskId == TASK_GYROPID && masterConfig.pid_process_denom > 1) { - cliPrintf(" - (%12s) rate: %d\r\n", taskInfo.subTaskName, subTaskFrequency); + cliPrintf(" - (%12s) %6d\r\n", taskInfo.subTaskName, subTaskFrequency); } } } From 6581bea2d5bf4be54933cfa57888be355c1bc67f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 28 Jul 2016 10:02:54 +0200 Subject: [PATCH 123/456] Cleanup MSP // compatibility Correct double rcSmoothInterval fix size --- src/main/io/msp_protocol.h | 17 +++--------- src/main/io/serial_msp.c | 53 ++++++++++++++------------------------ 2 files changed, 23 insertions(+), 47 deletions(-) diff --git a/src/main/io/msp_protocol.h b/src/main/io/msp_protocol.h index 1244186ffe..25f821d057 100644 --- a/src/main/io/msp_protocol.h +++ b/src/main/io/msp_protocol.h @@ -198,27 +198,18 @@ #define MSP_SET_VTX_CONFIG 89 //in message Set vtx settings - betaflight // Betaflight Additional Commands -#define MSP_PID_ADVANCED_CONFIG 90 -#define MSP_SET_PID_ADVANCED_CONFIG 91 +#define MSP_ADVANCED_CONFIG 90 +#define MSP_SET_ADVANCED_CONFIG 91 #define MSP_FILTER_CONFIG 92 #define MSP_SET_FILTER_CONFIG 93 -#define MSP_ADVANCED_TUNING 94 -#define MSP_SET_ADVANCED_TUNING 95 +#define MSP_PID_ADVANCED 94 +#define MSP_SET_PID_ADVANCED 95 #define MSP_SENSOR_CONFIG 96 #define MSP_SET_SENSOR_CONFIG 97 -#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility -#define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility - -#define MSP_PIDC_BETAFLIGHT 246 -#define MSP_SET_PIDC_BETAFLIGHT 247 - -#define MSP_RC_SMOOTHING 248 -#define MSP_SET_RC_SMOOTHING 249 - // // OSD specific // diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 3e897532e0..924610d129 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -808,7 +808,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16((uint16_t)gyro.targetLooptime); break; case MSP_RC_TUNING: - headSerialReply(11); + headSerialReply(12); serialize8(currentControlRateProfile->rcRate8); serialize8(currentControlRateProfile->rcExpo8); for (i = 0 ; i < 3; i++) { @@ -819,6 +819,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentControlRateProfile->thrExpo8); serialize16(currentControlRateProfile->tpa_breakpoint); serialize8(currentControlRateProfile->rcYawExpo8); + serialize8(currentControlRateProfile->rcYawRate8); break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); @@ -1020,7 +1021,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_RX_CONFIG: - headSerialReply(12); + headSerialReply(16); serialize8(masterConfig.rxConfig.serialrx_provider); serialize16(masterConfig.rxConfig.maxcheck); serialize16(masterConfig.rxConfig.midrc); @@ -1028,6 +1029,9 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.rxConfig.spektrum_sat_bind); serialize16(masterConfig.rxConfig.rx_min_usec); serialize16(masterConfig.rxConfig.rx_max_usec); + serialize8(masterConfig.rxConfig.rcSmoothing); + serialize8(masterConfig.rxConfig.rcSmoothInterval); + serialize16(masterConfig.rxConfig.airModeActivateThreshold); break; case MSP_FAILSAFE_CONFIG: @@ -1221,7 +1225,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.sensorAlignmentConfig.acc_align); serialize8(masterConfig.sensorAlignmentConfig.mag_align); break; - case MSP_PID_ADVANCED_CONFIG : + case MSP_ADVANCED_CONFIG : headSerialReply(6); serialize8(masterConfig.gyro_sync_denom); serialize8(masterConfig.pid_process_denom); @@ -1235,23 +1239,13 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentProfile->pidProfile.dterm_lpf_hz); serialize16(currentProfile->pidProfile.yaw_lpf_hz); break; - case MSP_ADVANCED_TUNING: - headSerialReply(3 * 2 + 2); + case MSP_PID_ADVANCED: + headSerialReply(17); serialize16(currentProfile->pidProfile.rollPitchItermIgnoreRate); serialize16(currentProfile->pidProfile.yawItermIgnoreRate); serialize16(currentProfile->pidProfile.yaw_p_limit); serialize8(currentProfile->pidProfile.deltaMethod); serialize8(currentProfile->pidProfile.vbatPidCompensation); - break; - case MSP_SPECIAL_PARAMETERS: - headSerialReply(1 + 2 + 1 + 2); - serialize8(currentControlRateProfile->rcYawRate8); - serialize16(masterConfig.rxConfig.airModeActivateThreshold); - serialize8(masterConfig.rxConfig.rcSmoothInterval); - serialize16(currentProfile->pidProfile.pidMaxVelocity); - break; - case MSP_PIDC_BETAFLIGHT: - headSerialReply(9); serialize8(currentProfile->pidProfile.ptermSetpointWeight); serialize8(currentProfile->pidProfile.dtermSetpointWeight); serialize8(currentProfile->pidProfile.toleranceBand); @@ -1260,11 +1254,6 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentProfile->pidProfile.pidMaxVelocity); serialize16(currentProfile->pidProfile.pidMaxVelocityYaw); break; - case MSP_RC_SMOOTHING: - headSerialReply(2); - serialize8(masterConfig.rxConfig.rcSmoothing); - serialize8(masterConfig.rxConfig.rcSmoothInterval); - break; case MSP_SENSOR_CONFIG: headSerialReply(3); serialize8(masterConfig.acc_hardware); @@ -1409,6 +1398,9 @@ static bool processInCommand(void) if (currentPort->dataSize >= 11) { currentControlRateProfile->rcYawExpo8 = read8(); } + if (currentPort->dataSize >= 12) { + currentControlRateProfile->rcYawRate8 = read8(); + } } else { headSerialError(0); } @@ -1678,6 +1670,11 @@ static bool processInCommand(void) masterConfig.rxConfig.rx_min_usec = read16(); masterConfig.rxConfig.rx_max_usec = read16(); } + if (currentPort->dataSize > 12) { + masterConfig.rxConfig.rcSmoothing = read8(); + masterConfig.rxConfig.rcSmoothInterval = read8(); + masterConfig.rxConfig.airModeActivateThreshold = read16(); + } break; case MSP_SET_FAILSAFE_CONFIG: @@ -1821,7 +1818,7 @@ static bool processInCommand(void) break; #endif - case MSP_SET_PID_ADVANCED_CONFIG : + case MSP_SET_ADVANCED_CONFIG : masterConfig.gyro_sync_denom = read8(); masterConfig.pid_process_denom = read8(); masterConfig.use_unsyncedPwm = read8(); @@ -1833,20 +1830,12 @@ static bool processInCommand(void) currentProfile->pidProfile.dterm_lpf_hz = read16(); currentProfile->pidProfile.yaw_lpf_hz = read16(); break; - case MSP_SET_ADVANCED_TUNING: + case MSP_SET_PID_ADVANCED: currentProfile->pidProfile.rollPitchItermIgnoreRate = read16(); currentProfile->pidProfile.yawItermIgnoreRate = read16(); currentProfile->pidProfile.yaw_p_limit = read16(); currentProfile->pidProfile.deltaMethod = read8(); currentProfile->pidProfile.vbatPidCompensation = read8(); - break; - case MSP_SET_SPECIAL_PARAMETERS: - currentControlRateProfile->rcYawRate8 = read8(); - masterConfig.rxConfig.airModeActivateThreshold = read16(); - masterConfig.rxConfig.rcSmoothInterval = read8(); - read16(); - break; - case MSP_SET_PIDC_BETAFLIGHT: currentProfile->pidProfile.ptermSetpointWeight = read8(); currentProfile->pidProfile.dtermSetpointWeight = read8(); currentProfile->pidProfile.toleranceBand = read8(); @@ -1855,10 +1844,6 @@ static bool processInCommand(void) currentProfile->pidProfile.pidMaxVelocity = read16(); currentProfile->pidProfile.pidMaxVelocityYaw = read16(); break; - case MSP_SET_RC_SMOOTHING: - masterConfig.rxConfig.rcSmoothing = read8(); - masterConfig.rxConfig.rcSmoothInterval = read8(); - break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); masterConfig.baro_hardware = read8(); From f0dfeea9bc45ce0d310b4b82ec00a5eb4c77f8f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=88=B5=E7=88=B7?= Date: Thu, 28 Jul 2016 22:14:07 +0800 Subject: [PATCH 124/456] Update target.h --- src/main/target/AIORACERF3/target.h | 59 +++++++++++++---------------- 1 file changed, 27 insertions(+), 32 deletions(-) diff --git a/src/main/target/AIORACERF3/target.h b/src/main/target/AIORACERF3/target.h index 0707437015..343d1038af 100644 --- a/src/main/target/AIORACERF3/target.h +++ b/src/main/target/AIORACERF3/target.h @@ -20,10 +20,10 @@ #define TARGET_BOARD_IDENTIFIER "ARF3" #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT - -#define LED0 PB8 -#define BEEPER PC15 +#define LED0 PB8 + +#define BEEPER PC15 #define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 12 // PPM, 8 PWM, UART3 RX/TX, LED Strip @@ -31,7 +31,7 @@ #define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -40,21 +40,18 @@ #define GYRO -//#define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define ACC -//#define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG -#define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW180_DEG #define BARO #define USE_BARO_BMP280 #define MAG -//#define USE_MPU9250_MAG // Enables bypass configuration #define USE_MAG_AK8963 //#define USE_MAG_HMC5883 // External @@ -68,19 +65,19 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 -#define UART1_TX_PIN PA9 // PA9 -#define UART1_RX_PIN PA10 // PA10 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C -#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_SPI #define USE_SPI_DEVICE_1 // PB9,3,4,5 on AF5 SPI1 (MPU) @@ -117,13 +114,12 @@ #define MPU6500_CS_PIN PB9 #define MPU6500_SPI_INSTANCE SPI1 -#define USE_ADC #define BOARD_HAS_VOLTAGE_DIVIDER - -#define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA5 -#define CURRENT_METER_ADC_PIN PA4 -#define RSSI_ADC_PIN PB2 +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 #define LED_STRIP #define USE_LED_STRIP_ON_DMA1_CHANNEL2 @@ -149,21 +145,20 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_BLACKBOX | FEATURE_RSSI_ADC | FEATURE_CURRENT_METER | FEATURE_TELEMETRY) #define SPEKTRUM_BIND // USART3, -#define BIND_PIN PB11 +#define BIND_PIN PB11 #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) From ae5686c07ddd1994ffe89da6f36bc1b1cc671a2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=88=B5=E7=88=B7?= Date: Thu, 28 Jul 2016 22:20:49 +0800 Subject: [PATCH 125/456] Update target.c --- src/main/target/AIORACERF3/target.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index e6352f01a6..419c76d9f0 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -91,8 +91,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM8 - { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // UART3_TX (AF7) + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // UART3_RX (AF7) + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, //LED_STRIP }; - From 54ad5a29e3324a313211f2d8892d77c99140738e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=88=B5=E7=88=B7?= Date: Thu, 28 Jul 2016 22:42:52 +0800 Subject: [PATCH 126/456] Update target.h --- src/main/target/AIORACERF3/target.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/AIORACERF3/target.h b/src/main/target/AIORACERF3/target.h index 343d1038af..1bf8886220 100644 --- a/src/main/target/AIORACERF3/target.h +++ b/src/main/target/AIORACERF3/target.h @@ -117,8 +117,8 @@ #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC #define ADC_INSTANCE ADC2 -#define VBAT_ADC_PIN PA4 -#define CURRENT_METER_ADC_PIN PA5 +#define VBAT_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PA4 #define RSSI_ADC_PIN PB2 #define LED_STRIP From de4a50594566b5bec2a5ceb04e4c6727f51e9bd1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=88=B5=E7=88=B7?= Date: Thu, 28 Jul 2016 22:49:53 +0800 Subject: [PATCH 127/456] Update target.c --- src/main/target/AIORACERF3/target.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index 419c76d9f0..2529e73f17 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -23,7 +23,6 @@ const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), From 3eb224836af8151b5027c59c5ff169a8d7358454 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=88=B5=E7=88=B7?= Date: Thu, 28 Jul 2016 23:07:40 +0800 Subject: [PATCH 128/456] Update target.c --- src/main/target/AIORACERF3/target.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index 2529e73f17..e780364ec2 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -82,14 +82,14 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM / UART2 RX { TIM8, IO_TAG(PA15), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_2 }, // PPM - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM1 - { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM2 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM3 - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM4 - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM8 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM4 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // UART3_TX (AF7) { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // UART3_RX (AF7) { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, //LED_STRIP From 6c89d547de09044279d4626261ebc8e9b6d6fae0 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 29 Jul 2016 00:29:14 +0200 Subject: [PATCH 129/456] Change Starter Defaults --- src/main/config/config.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 6669dd5b07..b0271ef278 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -204,8 +204,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->I8[ROLL] = 40; pidProfile->D8[ROLL] = 18; pidProfile->P8[PITCH] = 50; - pidProfile->I8[PITCH] = 40; - pidProfile->D8[PITCH] = 18; + pidProfile->I8[PITCH] = 55; + pidProfile->D8[PITCH] = 22; pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; @@ -239,14 +239,14 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; // Betaflight PID controller parameters - pidProfile->ptermSetpointWeight = 80; + pidProfile->ptermSetpointWeight = 75; pidProfile->dtermSetpointWeight = 0; pidProfile->pidMaxVelocity = 1000; pidProfile->pidMaxVelocityYaw = 50; pidProfile->toleranceBand = 15; pidProfile->toleranceBandReduction = 40; pidProfile->zeroCrossAllowanceCount = 2; - pidProfile->itermThrottleGain = 15; + pidProfile->itermThrottleGain = 10; #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. @@ -527,7 +527,7 @@ static void resetConf(void) masterConfig.rxConfig.rssi_channel = 0; masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; masterConfig.rxConfig.rssi_ppm_invert = 0; - masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_AUTO; + masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_OFF; masterConfig.rxConfig.rcSmoothInterval = 9; masterConfig.rxConfig.fpvCamAngleDegrees = 0; #ifdef STM32F4 From a8f1e9f530368e1b23d957319bcf4d2f1b3155f8 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 29 Jul 2016 08:25:02 +0100 Subject: [PATCH 130/456] Changed \r\n to \n in target.mk files --- src/main/target/ALIENFLIGHTF1/target.mk | 40 ++++++++-------- src/main/target/ALIENFLIGHTF3/target.mk | 22 ++++----- src/main/target/ALIENFLIGHTF4/target.mk | 26 +++++----- src/main/target/BLUEJAYF4/target.mk | 20 ++++---- src/main/target/CC3D/target.mk | 28 +++++------ src/main/target/CHEBUZZF3/target.mk | 38 +++++++-------- src/main/target/CJMCU/target.mk | 22 ++++----- src/main/target/COLIBRI_RACE/target.mk | 36 +++++++------- src/main/target/DOGE/target.mk | 24 +++++----- src/main/target/EUSTM32F103RC/target.mk | 50 +++++++++---------- src/main/target/F4BY/target.mk | 16 +++---- src/main/target/FURYF3/target.mk | 30 ++++++------ src/main/target/FURYF4/target.mk | 18 +++---- src/main/target/IRCFUSIONF3/target.mk | 18 +++---- src/main/target/KISSFC/target.mk | 16 +++---- src/main/target/LUX_RACE/target.mk | 22 ++++----- src/main/target/MOTOLAB/target.mk | 26 +++++----- src/main/target/NAZE/target.mk | 40 ++++++++-------- src/main/target/OLIMEXINO/target.mk | 26 +++++----- src/main/target/PIKOBLX/target.mk | 26 +++++----- src/main/target/PORT103R/target.mk | 56 +++++++++++----------- src/main/target/REVO/target.mk | 16 +++---- src/main/target/REVONANO/target.mk | 10 ++-- src/main/target/RMDO/target.mk | 26 +++++----- src/main/target/SINGULARITY/target.mk | 24 +++++----- src/main/target/SPARKY/target.mk | 24 +++++----- src/main/target/SPRACINGF3/target.mk | 32 ++++++------- src/main/target/SPRACINGF3EVO/target.mk | 32 ++++++------- src/main/target/SPRACINGF3MINI/target.mk | 38 +++++++-------- src/main/target/STM32F3DISCOVERY/target.mk | 44 ++++++++--------- src/main/target/ZCOREF3/target.mk | 22 ++++----- 31 files changed, 434 insertions(+), 434 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF1/target.mk b/src/main/target/ALIENFLIGHTF1/target.mk index 07d81cf447..e5fbfe8c95 100644 --- a/src/main/target/ALIENFLIGHTF1/target.mk +++ b/src/main/target/ALIENFLIGHTF1/target.mk @@ -1,20 +1,20 @@ -F1_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH HIGHEND - -TARGET_SRC = \ - drivers/accgyro_adxl345.c \ - drivers/accgyro_bma280.c \ - drivers/accgyro_l3g4200d.c \ - drivers/accgyro_mma845x.c \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu3050.c \ - drivers/accgyro_mpu6050.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/barometer_bmp085.c \ - drivers/barometer_bmp280.c \ - drivers/barometer_ms5611.c \ - drivers/compass_hmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f10x.c \ - drivers/sonar_hcsr04.c +F1_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH HIGHEND + +TARGET_SRC = \ + drivers/accgyro_adxl345.c \ + drivers/accgyro_bma280.c \ + drivers/accgyro_l3g4200d.c \ + drivers/accgyro_mma845x.c \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu3050.c \ + drivers/accgyro_mpu6050.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp085.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f10x.c \ + drivers/sonar_hcsr04.c diff --git a/src/main/target/ALIENFLIGHTF3/target.mk b/src/main/target/ALIENFLIGHTF3/target.mk index f1449b704c..90e0b83f10 100644 --- a/src/main/target/ALIENFLIGHTF3/target.mk +++ b/src/main/target/ALIENFLIGHTF3/target.mk @@ -1,11 +1,11 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6050.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/compass_ak8963.c \ - drivers/sonar_hcsr04.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6050.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/compass_ak8963.c \ + drivers/sonar_hcsr04.c + diff --git a/src/main/target/ALIENFLIGHTF4/target.mk b/src/main/target/ALIENFLIGHTF4/target.mk index 910023aa0e..edd75eefb3 100644 --- a/src/main/target/ALIENFLIGHTF4/target.mk +++ b/src/main/target/ALIENFLIGHTF4/target.mk @@ -1,13 +1,13 @@ -F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP - -TARGET_SRC = \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/barometer_bmp280.c \ - drivers/barometer_ms5611.c \ - drivers/compass_ak8963.c \ - drivers/compass_hmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f4xx.c - +F405_TARGETS += $(TARGET) +FEATURES += SDCARD VCP + +TARGET_SRC = \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_ms5611.c \ + drivers/compass_ak8963.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f4xx.c + diff --git a/src/main/target/BLUEJAYF4/target.mk b/src/main/target/BLUEJAYF4/target.mk index f523965621..23dd74507a 100644 --- a/src/main/target/BLUEJAYF4/target.mk +++ b/src/main/target/BLUEJAYF4/target.mk @@ -1,10 +1,10 @@ -F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP - -TARGET_SRC = \ - drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_mpu6500.c \ - drivers/barometer_ms5611.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f4xx.c - +F405_TARGETS += $(TARGET) +FEATURES += SDCARD VCP + +TARGET_SRC = \ + drivers/accgyro_spi_mpu6500.c \ + drivers/accgyro_mpu6500.c \ + drivers/barometer_ms5611.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f4xx.c + diff --git a/src/main/target/CC3D/target.mk b/src/main/target/CC3D/target.mk index 2718c0302d..7bf5bc80b3 100644 --- a/src/main/target/CC3D/target.mk +++ b/src/main/target/CC3D/target.mk @@ -1,14 +1,14 @@ -F1_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH HIGHEND VCP - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/barometer_bmp085.c \ - drivers/barometer_bmp280.c \ - drivers/barometer_ms5611.c \ - drivers/compass_hmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f10x.c \ - drivers/sonar_hcsr04.c - +F1_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH HIGHEND VCP + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/barometer_bmp085.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f10x.c \ + drivers/sonar_hcsr04.c + diff --git a/src/main/target/CHEBUZZF3/target.mk b/src/main/target/CHEBUZZF3/target.mk index e90db6d09f..63e6409e3a 100644 --- a/src/main/target/CHEBUZZF3/target.mk +++ b/src/main/target/CHEBUZZF3/target.mk @@ -1,19 +1,19 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD - -TARGET_SRC = \ - drivers/light_ws2811strip.c \ - drivers/accgyro_l3gd20.c \ - drivers/accgyro_lsm303dlhc.c \ - drivers/compass_hmc5883l.c \ - drivers/accgyro_adxl345.c \ - drivers/accgyro_bma280.c \ - drivers/accgyro_mma845x.c \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu3050.c \ - drivers/accgyro_mpu6050.c \ - drivers/accgyro_l3g4200d.c \ - drivers/barometer_ms5611.c \ - drivers/barometer_bmp280.c \ - drivers/compass_ak8975.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD + +TARGET_SRC = \ + drivers/light_ws2811strip.c \ + drivers/accgyro_l3gd20.c \ + drivers/accgyro_lsm303dlhc.c \ + drivers/compass_hmc5883l.c \ + drivers/accgyro_adxl345.c \ + drivers/accgyro_bma280.c \ + drivers/accgyro_mma845x.c \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu3050.c \ + drivers/accgyro_mpu6050.c \ + drivers/accgyro_l3g4200d.c \ + drivers/barometer_ms5611.c \ + drivers/barometer_bmp280.c \ + drivers/compass_ak8975.c + diff --git a/src/main/target/CJMCU/target.mk b/src/main/target/CJMCU/target.mk index 69a1ecc9f4..72f2c64b03 100644 --- a/src/main/target/CJMCU/target.mk +++ b/src/main/target/CJMCU/target.mk @@ -1,11 +1,11 @@ -F1_TARGETS += $(TARGET) -FLASH_SIZE = 64 - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6050.c \ - drivers/compass_hmc5883l.c \ - flight/gtune.c \ - blackbox/blackbox.c \ - blackbox/blackbox_io.c - +F1_TARGETS += $(TARGET) +FLASH_SIZE = 64 + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6050.c \ + drivers/compass_hmc5883l.c \ + flight/gtune.c \ + blackbox/blackbox.c \ + blackbox/blackbox_io.c + diff --git a/src/main/target/COLIBRI_RACE/target.mk b/src/main/target/COLIBRI_RACE/target.mk index b59d32d0e1..1632ce30b9 100644 --- a/src/main/target/COLIBRI_RACE/target.mk +++ b/src/main/target/COLIBRI_RACE/target.mk @@ -1,18 +1,18 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP - -TARGET_SRC = \ - i2c_bst.c \ - bus_bst_stm32f30x.c \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_mpu6500.c \ - drivers/barometer_ms5611.c \ - drivers/compass_ak8963.c \ - drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP + +TARGET_SRC = \ + i2c_bst.c \ + bus_bst_stm32f30x.c \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/accgyro_mpu6500.c \ + drivers/barometer_ms5611.c \ + drivers/compass_ak8963.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c + diff --git a/src/main/target/DOGE/target.mk b/src/main/target/DOGE/target.mk index f44e26c5bf..8699473185 100644 --- a/src/main/target/DOGE/target.mk +++ b/src/main/target/DOGE/target.mk @@ -1,12 +1,12 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP ONBOARDFLASH - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/barometer_bmp280.c \ - drivers/barometer_spi_bmp280.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_spi_bmp280.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c + diff --git a/src/main/target/EUSTM32F103RC/target.mk b/src/main/target/EUSTM32F103RC/target.mk index b315c0aacb..aa9ccbf8ef 100644 --- a/src/main/target/EUSTM32F103RC/target.mk +++ b/src/main/target/EUSTM32F103RC/target.mk @@ -1,25 +1,25 @@ -F1_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH HIGHEND - -DEVICE_FLAGS = -DSTM32F10X_HD - -TARGET_SRC = \ - drivers/accgyro_adxl345.c \ - drivers/accgyro_bma280.c \ - drivers/accgyro_l3g4200d.c \ - drivers/accgyro_mma845x.c \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu3050.c \ - drivers/accgyro_mpu6050.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/barometer_bmp085.c \ - drivers/barometer_bmp280.c \ - drivers/barometer_ms5611.c \ - drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f10x.c \ - drivers/sonar_hcsr04.c - +F1_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH HIGHEND + +DEVICE_FLAGS = -DSTM32F10X_HD + +TARGET_SRC = \ + drivers/accgyro_adxl345.c \ + drivers/accgyro_bma280.c \ + drivers/accgyro_l3g4200d.c \ + drivers/accgyro_mma845x.c \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu3050.c \ + drivers/accgyro_mpu6050.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp085.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_ms5611.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f10x.c \ + drivers/sonar_hcsr04.c + diff --git a/src/main/target/F4BY/target.mk b/src/main/target/F4BY/target.mk index 5093a8df72..12edbbb716 100644 --- a/src/main/target/F4BY/target.mk +++ b/src/main/target/F4BY/target.mk @@ -1,8 +1,8 @@ -F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP - -TARGET_SRC = \ - drivers/accgyro_spi_mpu6000.c \ - drivers/barometer_ms5611.c \ - drivers/compass_hmc5883l.c - +F405_TARGETS += $(TARGET) +FEATURES += SDCARD VCP + +TARGET_SRC = \ + drivers/accgyro_spi_mpu6000.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c + diff --git a/src/main/target/FURYF3/target.mk b/src/main/target/FURYF3/target.mk index c68651f01d..2f1639e86a 100644 --- a/src/main/target/FURYF3/target.mk +++ b/src/main/target/FURYF3/target.mk @@ -1,15 +1,15 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD ONBOARDFLASH - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/barometer_ms5611.c \ - drivers/barometer_bmp280.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/sonar_hcsr04.c \ - drivers/serial_softserial.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/barometer_ms5611.c \ + drivers/barometer_bmp280.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/sonar_hcsr04.c \ + drivers/serial_softserial.c + diff --git a/src/main/target/FURYF4/target.mk b/src/main/target/FURYF4/target.mk index e22fad504b..958e5c68de 100644 --- a/src/main/target/FURYF4/target.mk +++ b/src/main/target/FURYF4/target.mk @@ -1,9 +1,9 @@ -F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP ONBOARDFLASH - -TARGET_SRC = \ - drivers/accgyro_spi_mpu6000.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_mpu6500.c \ - drivers/barometer_ms5611.c - +F405_TARGETS += $(TARGET) +FEATURES += SDCARD VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_spi_mpu6000.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/accgyro_mpu6500.c \ + drivers/barometer_ms5611.c + diff --git a/src/main/target/IRCFUSIONF3/target.mk b/src/main/target/IRCFUSIONF3/target.mk index 5c10e856b6..1e7b96a538 100644 --- a/src/main/target/IRCFUSIONF3/target.mk +++ b/src/main/target/IRCFUSIONF3/target.mk @@ -1,9 +1,9 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP ONBOARDFLASH -TARGET_FLAGS = -DSPRACINGF3 - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6050.c \ - drivers/barometer_bmp085.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP ONBOARDFLASH +TARGET_FLAGS = -DSPRACINGF3 + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6050.c \ + drivers/barometer_bmp085.c + diff --git a/src/main/target/KISSFC/target.mk b/src/main/target/KISSFC/target.mk index 6ed94fd46d..c59af03e59 100644 --- a/src/main/target/KISSFC/target.mk +++ b/src/main/target/KISSFC/target.mk @@ -1,8 +1,8 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/display_ug2864hsweg01.c \ - drivers/accgyro_mpu6050.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/display_ug2864hsweg01.c \ + drivers/accgyro_mpu6050.c + diff --git a/src/main/target/LUX_RACE/target.mk b/src/main/target/LUX_RACE/target.mk index 1a77b54eae..2f6ba2d537 100644 --- a/src/main/target/LUX_RACE/target.mk +++ b/src/main/target/LUX_RACE/target.mk @@ -1,11 +1,11 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_mpu6500.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/accgyro_mpu6500.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c + diff --git a/src/main/target/MOTOLAB/target.mk b/src/main/target/MOTOLAB/target.mk index 9dfcb8bb80..596dedfbc3 100644 --- a/src/main/target/MOTOLAB/target.mk +++ b/src/main/target/MOTOLAB/target.mk @@ -1,13 +1,13 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP ONBOARDFLASH - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6050.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/barometer_ms5611.c \ - drivers/compass_hmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_softserial.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6050.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c + diff --git a/src/main/target/NAZE/target.mk b/src/main/target/NAZE/target.mk index 07d81cf447..e5fbfe8c95 100644 --- a/src/main/target/NAZE/target.mk +++ b/src/main/target/NAZE/target.mk @@ -1,20 +1,20 @@ -F1_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH HIGHEND - -TARGET_SRC = \ - drivers/accgyro_adxl345.c \ - drivers/accgyro_bma280.c \ - drivers/accgyro_l3g4200d.c \ - drivers/accgyro_mma845x.c \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu3050.c \ - drivers/accgyro_mpu6050.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/barometer_bmp085.c \ - drivers/barometer_bmp280.c \ - drivers/barometer_ms5611.c \ - drivers/compass_hmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f10x.c \ - drivers/sonar_hcsr04.c +F1_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH HIGHEND + +TARGET_SRC = \ + drivers/accgyro_adxl345.c \ + drivers/accgyro_bma280.c \ + drivers/accgyro_l3g4200d.c \ + drivers/accgyro_mma845x.c \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu3050.c \ + drivers/accgyro_mpu6050.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp085.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f10x.c \ + drivers/sonar_hcsr04.c diff --git a/src/main/target/OLIMEXINO/target.mk b/src/main/target/OLIMEXINO/target.mk index b299c9ffd4..6834b6126a 100644 --- a/src/main/target/OLIMEXINO/target.mk +++ b/src/main/target/OLIMEXINO/target.mk @@ -1,13 +1,13 @@ -F1_TARGETS += $(TARGET) -FEATURES = HIGHEND - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6050.c \ - drivers/barometer_bmp085.c \ - drivers/barometer_bmp280.c \ - drivers/compass_hmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f10x.c \ - drivers/sonar_hcsr04.c - +F1_TARGETS += $(TARGET) +FEATURES = HIGHEND + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6050.c \ + drivers/barometer_bmp085.c \ + drivers/barometer_bmp280.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f10x.c \ + drivers/sonar_hcsr04.c + diff --git a/src/main/target/PIKOBLX/target.mk b/src/main/target/PIKOBLX/target.mk index 3662ea997a..3192c18c4e 100644 --- a/src/main/target/PIKOBLX/target.mk +++ b/src/main/target/PIKOBLX/target.mk @@ -1,13 +1,13 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_softserial.c \ - drivers/transponder_ir.c \ - drivers/transponder_ir_stm32f30x.c \ - io/transponder_ir.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/transponder_ir.c \ + drivers/transponder_ir_stm32f30x.c \ + io/transponder_ir.c + diff --git a/src/main/target/PORT103R/target.mk b/src/main/target/PORT103R/target.mk index 2246dc6051..86cf70fb5e 100644 --- a/src/main/target/PORT103R/target.mk +++ b/src/main/target/PORT103R/target.mk @@ -1,28 +1,28 @@ -F1_TARGETS += $(TARGET) -256K_TARGETS += $(TARGET) -FLASH_SIZE = 256 - -FEATURES = ONBOARDFLASH HIGHEND - -DEVICE_FLAGS = -DSTM32F10X_HD - -TARGET_SRC = \ - drivers/accgyro_adxl345.c \ - drivers/accgyro_bma280.c \ - drivers/accgyro_l3g4200d.c \ - drivers/accgyro_mma845x.c \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu3050.c \ - drivers/accgyro_mpu6050.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6000.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/barometer_bmp085.c \ - drivers/barometer_bmp280.c \ - drivers/barometer_ms5611.c \ - drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f10x.c \ - drivers/sonar_hcsr04.c - +F1_TARGETS += $(TARGET) +256K_TARGETS += $(TARGET) +FLASH_SIZE = 256 + +FEATURES = ONBOARDFLASH HIGHEND + +DEVICE_FLAGS = -DSTM32F10X_HD + +TARGET_SRC = \ + drivers/accgyro_adxl345.c \ + drivers/accgyro_bma280.c \ + drivers/accgyro_l3g4200d.c \ + drivers/accgyro_mma845x.c \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu3050.c \ + drivers/accgyro_mpu6050.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp085.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_ms5611.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f10x.c \ + drivers/sonar_hcsr04.c + diff --git a/src/main/target/REVO/target.mk b/src/main/target/REVO/target.mk index 21c242fe4b..7347dd10f2 100644 --- a/src/main/target/REVO/target.mk +++ b/src/main/target/REVO/target.mk @@ -1,8 +1,8 @@ -F405_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH - -TARGET_SRC = \ - drivers/accgyro_spi_mpu6000.c \ - drivers/barometer_ms5611.c \ - drivers/compass_hmc5883l.c - +F405_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_spi_mpu6000.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c + diff --git a/src/main/target/REVONANO/target.mk b/src/main/target/REVONANO/target.mk index 42cce14ffd..af9876e351 100644 --- a/src/main/target/REVONANO/target.mk +++ b/src/main/target/REVONANO/target.mk @@ -1,6 +1,6 @@ -F411_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH - -TARGET_SRC = \ - drivers/accgyro_spi_mpu9250.c \ +F411_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_spi_mpu9250.c \ drivers/barometer_ms5611.c \ No newline at end of file diff --git a/src/main/target/RMDO/target.mk b/src/main/target/RMDO/target.mk index ccbbd5a8e5..ec0fc084df 100644 --- a/src/main/target/RMDO/target.mk +++ b/src/main/target/RMDO/target.mk @@ -1,13 +1,13 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP ONBOARDFLASH -TARGET_FLAGS = -DSPRACINGF3 - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6050.c \ - drivers/barometer_bmp280.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_softserial.c \ - drivers/sonar_hcsr04.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP ONBOARDFLASH +TARGET_FLAGS = -DSPRACINGF3 + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6050.c \ + drivers/barometer_bmp280.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/sonar_hcsr04.c + diff --git a/src/main/target/SINGULARITY/target.mk b/src/main/target/SINGULARITY/target.mk index 1cd4fcd942..bdae168d91 100644 --- a/src/main/target/SINGULARITY/target.mk +++ b/src/main/target/SINGULARITY/target.mk @@ -1,12 +1,12 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP ONBOARDFLASH - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6050.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_softserial.c \ - drivers/vtx_rtc6705.c \ - io/vtx.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6050.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/vtx_rtc6705.c \ + io/vtx.c + diff --git a/src/main/target/SPARKY/target.mk b/src/main/target/SPARKY/target.mk index 76ea72b120..f4de2539f2 100644 --- a/src/main/target/SPARKY/target.mk +++ b/src/main/target/SPARKY/target.mk @@ -1,12 +1,12 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6050.c \ - drivers/barometer_ms5611.c \ - drivers/barometer_bmp280.c \ - drivers/compass_ak8975.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6050.c \ + drivers/barometer_ms5611.c \ + drivers/barometer_bmp280.c \ + drivers/compass_ak8975.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c + diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk index ed7391c7a8..2d8711dfb6 100644 --- a/src/main/target/SPRACINGF3/target.mk +++ b/src/main/target/SPRACINGF3/target.mk @@ -1,16 +1,16 @@ -F3_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6050.c \ - drivers/barometer_ms5611.c \ - drivers/barometer_bmp085.c \ - drivers/barometer_bmp280.c \ - drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_softserial.c \ - drivers/sonar_hcsr04.c - +F3_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6050.c \ + drivers/barometer_ms5611.c \ + drivers/barometer_bmp085.c \ + drivers/barometer_bmp280.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/sonar_hcsr04.c + diff --git a/src/main/target/SPRACINGF3EVO/target.mk b/src/main/target/SPRACINGF3EVO/target.mk index 9b179afe4b..086683412d 100644 --- a/src/main/target/SPRACINGF3EVO/target.mk +++ b/src/main/target/SPRACINGF3EVO/target.mk @@ -1,16 +1,16 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/barometer_bmp280.c \ - drivers/compass_ak8963.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_usb_vcp.c \ - drivers/transponder_ir.c \ - drivers/transponder_ir_stm32f30x.c \ - io/transponder_ir.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/compass_ak8963.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_usb_vcp.c \ + drivers/transponder_ir.c \ + drivers/transponder_ir_stm32f30x.c \ + io/transponder_ir.c + diff --git a/src/main/target/SPRACINGF3MINI/target.mk b/src/main/target/SPRACINGF3MINI/target.mk index 1c1e3b9a52..5c16c88a7d 100644 --- a/src/main/target/SPRACINGF3MINI/target.mk +++ b/src/main/target/SPRACINGF3MINI/target.mk @@ -1,19 +1,19 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6500.c \ - drivers/barometer_bmp280.c \ - drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c \ - drivers/flash_m25p16.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c \ - drivers/serial_softserial.c \ - drivers/serial_usb_vcp.c \ - drivers/sonar_hcsr04.c \ - drivers/transponder_ir.c \ - drivers/transponder_ir_stm32f30x.c \ - io/transponder_ir.c - +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/serial_usb_vcp.c \ + drivers/sonar_hcsr04.c \ + drivers/transponder_ir.c \ + drivers/transponder_ir_stm32f30x.c \ + io/transponder_ir.c + diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index 9f3645c97c..e3eb8bffdc 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -1,22 +1,22 @@ -F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD - -TARGET_SRC = \ - drivers/light_ws2811strip.c \ - drivers/accgyro_l3gd20.c \ - drivers/accgyro_lsm303dlhc.c \ - drivers/compass_hmc5883l.c \ - drivers/accgyro_adxl345.c \ - drivers/accgyro_bma280.c \ - drivers/accgyro_mma845x.c \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu3050.c \ - drivers/accgyro_mpu6050.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_l3g4200d.c \ - drivers/barometer_ms5611.c \ - drivers/barometer_bmp280.c \ - drivers/compass_ak8975.c \ - drivers/max7456.c \ - io/osd.c +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD + +TARGET_SRC = \ + drivers/light_ws2811strip.c \ + drivers/accgyro_l3gd20.c \ + drivers/accgyro_lsm303dlhc.c \ + drivers/compass_hmc5883l.c \ + drivers/accgyro_adxl345.c \ + drivers/accgyro_bma280.c \ + drivers/accgyro_mma845x.c \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu3050.c \ + drivers/accgyro_mpu6050.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/accgyro_l3g4200d.c \ + drivers/barometer_ms5611.c \ + drivers/barometer_bmp280.c \ + drivers/compass_ak8975.c \ + drivers/max7456.c \ + io/osd.c diff --git a/src/main/target/ZCOREF3/target.mk b/src/main/target/ZCOREF3/target.mk index 4b232a7870..39b7b5d000 100644 --- a/src/main/target/ZCOREF3/target.mk +++ b/src/main/target/ZCOREF3/target.mk @@ -1,11 +1,11 @@ -F3_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH - -TARGET_SRC = \ - drivers/accgyro_mpu.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/barometer_bmp280.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f30x.c - +F3_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c + From b6c6475d4cb7a8e3d7ab8b168df942e758afc1e4 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 29 Jul 2016 10:04:54 +0200 Subject: [PATCH 131/456] Try different Timers for KISS --- src/main/target/KISSFC/target.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index f9641219f3..17799bf971 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -66,10 +66,8 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_6}, { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_2}, - { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_6}, // TIM1_CH2N - { TIM1, IO_TAG(PB15), TIM_Channel_3, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_4}, // TIM1_CH3N - //{ TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, - //{ TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, + { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED|TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_6}, + { TIM1, IO_TAG(PB15), TIM_Channel_3, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED|TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_4}, { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, From c528767778ee35cf35046f147dd4364f567ae372 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 29 Jul 2016 10:37:52 +0200 Subject: [PATCH 132/456] fixed double ACCEL activation --- src/main/main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index 15d2b6cbab..1948a72ac3 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -320,7 +320,7 @@ void init(void) bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL || masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED; // Configurator feature abused for enabling Fast PWM - pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED); + pwm_params.useFastPwm = (masterConfig.motor_pwm_protocol != PWM_TYPE_CONVENTIONAL && masterConfig.motor_pwm_protocol != PWM_TYPE_BRUSHED); pwm_params.pwmProtocolType = masterConfig.motor_pwm_protocol; pwm_params.motorPwmRate = use_unsyncedPwm ? masterConfig.motor_pwm_rate : 0; pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; @@ -716,7 +716,7 @@ void main_init(void) } rescheduleTask(TASK_ACCEL, accTargetLooptime); } - setTaskEnabled(TASK_ACCEL, sensors(SENSOR_ACC)); + setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC)); setTaskEnabled(TASK_SERIAL, true); #ifdef BEEPER From b97a2b18a25b6c1015d02b341e5e84e8e312c662 Mon Sep 17 00:00:00 2001 From: nathan Date: Fri, 29 Jul 2016 02:04:44 -0700 Subject: [PATCH 133/456] trying to fix kiss again. this should pry be a constant time implementation... --- src/main/drivers/pwm_output.c | 16 ++++++++++------ src/main/target/KISSFC/target.c | 8 ++++---- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index bcee6c70b8..44504f6801 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -193,13 +193,17 @@ void pwmEnableMotors(void) void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) { - TIM_TypeDef *lastTimerPtr = NULL; - for (int index = 0; index < motorCount; index++) { - // Force the timer to overflow if it's the first motor to output, or if we change timers - if (motors[index]->tim != lastTimerPtr) { - lastTimerPtr = motors[index]->tim; - timerForceOverflow(motors[index]->tim); + bool overflowed = false; + // If we have not already overflowed this timer + for (int j = 0; j < index; j++) { + if (motors[j]->tim == motors[index]->tim) { + overflowed = true; + break; + } + } + if (!overflowed) { + timerForceOverflow(motors[index]->tim); } // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index f9641219f3..dc7c159b8e 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -66,10 +66,10 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_6}, { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_2}, - { TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_6}, // TIM1_CH2N - { TIM1, IO_TAG(PB15), TIM_Channel_3, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_4}, // TIM1_CH3N - //{ TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, - //{ TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, + //{ TIM1, IO_TAG(PB14), TIM_Channel_2, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_6}, // TIM1_CH2N + //{ TIM1, IO_TAG(PB15), TIM_Channel_3, TIM1_CC_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_N_CHANNEL, IOCFG_AF_PP, GPIO_AF_4}, // TIM1_CH3N + { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, + { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, TIMER_OUTPUT_ENABLED|TIMER_OUTPUT_INVERTED, IOCFG_AF_PP, GPIO_AF_1}, From 07cd37468ced20c97efb8ee25d7d3a7b1798dab9 Mon Sep 17 00:00:00 2001 From: Gergely Szell Date: Fri, 29 Jul 2016 10:20:49 +0200 Subject: [PATCH 134/456] Remove SoftSerial2 from X_RACERSPI In the v3.x (SPI) version, one of the previously used soft serial ports have been sacrificed for SPI. Soft serial 1 is available on PWM 5/6. Tested with board v3.0. --- src/main/target/X_RACERSPI/target.h | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 024a3a6c28..7838b1f460 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -57,8 +57,7 @@ #define USE_UART2 #define USE_UART3 #define USE_SOFTSERIAL1 -#define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 4 #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 @@ -70,11 +69,8 @@ #define UART3_RX_PIN PB11 // PB11 (AF7) #define SOFTSERIAL_1_TIMER TIM3 -#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 -#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -#define SOFTSERIAL_2_TIMER TIM3 -#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 -#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 6 // PWM 5 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 7 // PWM 6 #define USE_I2C From 0e8d375a1c00ef2ffc7cd1c27b25b2a70b784756 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 30 Jul 2016 11:27:04 +1000 Subject: [PATCH 135/456] Update to incorporate hardware revision detection for BJF4 target --- src/main/drivers/flash_m25p16.c | 20 ++++- src/main/drivers/flash_m25p16.h | 3 +- src/main/drivers/io.h | 3 + src/main/main.c | 11 ++- src/main/target/BLUEJAYF4/hardware_revision.c | 86 +++++++++++++++++++ src/main/target/BLUEJAYF4/hardware_revision.h | 30 +++++++ src/main/target/BLUEJAYF4/target.h | 11 ++- src/main/target/BLUEJAYF4/target.mk | 2 +- 8 files changed, 153 insertions(+), 13 deletions(-) create mode 100644 src/main/target/BLUEJAYF4/hardware_revision.c create mode 100644 src/main/target/BLUEJAYF4/hardware_revision.h diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 29620fbe11..d1e62cccbe 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -196,12 +196,26 @@ static bool m25p16_readIdentification() * Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with * m25p16_getGeometry(). */ -bool m25p16_init() +bool m25p16_init(ioTag_t csTag) { - + /* + if we have already detected a flash device we can simply exit + + TODO: change the init param in favour of flash CFG when ParamGroups work is done + then cs pin can be specified in hardware_revision.c or config.c (dependent on revision). + */ + if (geometry.sectors) { + return true; + } + + if (csTag) { + m25p16CsPin = IOGetByTag(csTag); + } else { #ifdef M25P16_CS_PIN - m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN)); + m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN)); +#else return false; #endif + } IOInit(m25p16CsPin, OWNER_FLASH, RESOURCE_SPI_CS, 0); IOConfigGPIO(m25p16CsPin, SPI_IO_CS_CFG); diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h index 972fe10bbc..223efa1809 100644 --- a/src/main/drivers/flash_m25p16.h +++ b/src/main/drivers/flash_m25p16.h @@ -19,10 +19,11 @@ #include #include "flash.h" +#include "io.h" #define M25P16_PAGESIZE 256 -bool m25p16_init(); +bool m25p16_init(ioTag_t csTag); void m25p16_eraseSector(uint32_t address); void m25p16_eraseCompletely(); diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index af9deca810..d655646282 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -11,6 +11,9 @@ typedef uint8_t ioTag_t; // packet tag to specify IO pin typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change +// NONE initializer for ioTag_t variables +#define IOTAG_NONE ((ioTag_t)0) + // NONE initializer for IO_t variable #define IO_NONE ((IO_t)0) diff --git a/src/main/main.c b/src/main/main.c index 1948a72ac3..7be831d73c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -179,8 +179,7 @@ void init(void) #ifdef ALIENFLIGHTF3 if (hardwareRevision == AFF3_REV_1) { ledInit(false); - } - else { + } else { ledInit(true); } #else @@ -581,10 +580,10 @@ void init(void) #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { - m25p16_init(); + m25p16_init(IOTAG_NONE); } #elif defined(USE_FLASH_M25P16) - m25p16_init(); + m25p16_init(IOTAG_NONE); #endif flashfsInit(); @@ -599,7 +598,11 @@ void init(void) #if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL) // Ensure the SPI Tx DMA doesn't overlap with the led strip +#ifdef STM32F4 + sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_STREAM; +#else sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL; +#endif #else sdcardUseDMA = true; #endif diff --git a/src/main/target/BLUEJAYF4/hardware_revision.c b/src/main/target/BLUEJAYF4/hardware_revision.c new file mode 100644 index 0000000000..6ed07e16f9 --- /dev/null +++ b/src/main/target/BLUEJAYF4/hardware_revision.c @@ -0,0 +1,86 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" +#include "build_config.h" + +#include "drivers/system.h" +#include "drivers/bus_spi.h" +#include "drivers/io.h" +#include "drivers/flash_m25p16.h" +#include "hardware_revision.h" + +static const char * const hardwareRevisionNames[] = { + "Unknown", + "BlueJay rev1", + "BlueJay rev2", + "BlueJay rev3", + "BlueJay rev3a" +}; + +uint8_t hardwareRevision = UNKNOWN; + +void detectHardwareRevision(void) +{ + IO_t pin1 = IOGetByTag(IO_TAG(PB12)); + IOInit(pin1, OWNER_SYSTEM, RESOURCE_INPUT, 1); + IOConfigGPIO(pin1, IOCFG_IPU); + + IO_t pin2 = IOGetByTag(IO_TAG(PB13)); + IOInit(pin2, OWNER_SYSTEM, RESOURCE_INPUT, 2); + IOConfigGPIO(pin2, IOCFG_IPU); + + // Check hardware revision + delayMicroseconds(10); // allow configuration to settle + + /* + if both PB12 and 13 are tied to GND then it is Rev3A (mini) + if only PB12 is tied to GND then it is a Rev3 (full size) + */ + if (!IORead(pin1)) { + if (!IORead(pin2)) { + hardwareRevision = BJF4_REV3A; + return; + } + hardwareRevision = BJF4_REV3; + return; + } + + hardwareRevision = BJF4_REV2; +} + +void updateHardwareRevision(void) +{ + if (hardwareRevision != BJF4_REV2) { + return; + } + + /* + if flash exists on PB3 then Rev1 + */ + if (m25p16_init(IO_TAG(PB3))) { + hardwareRevision = BJF4_REV1; + } else { + IOInit(IOGetByTag(IO_TAG(PB3)), OWNER_FREE, RESOURCE_NONE, 0); + } +} + + diff --git a/src/main/target/BLUEJAYF4/hardware_revision.h b/src/main/target/BLUEJAYF4/hardware_revision.h new file mode 100644 index 0000000000..3333d86aa0 --- /dev/null +++ b/src/main/target/BLUEJAYF4/hardware_revision.h @@ -0,0 +1,30 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ +#pragma once + +typedef enum bjf4HardwareRevision_t { + UNKNOWN = 0, + BJF4_REV1, // Flash + BJF4_REV2, // SDCard + BJF4_REV3, // SDCard + Flash + BJF4_REV3A, // Flash (20x20 mini format) +} bjf4HardwareRevision_e; + +extern uint8_t hardwareRevision; + +void updateHardwareRevision(void); +void detectHardwareRevision(void); \ No newline at end of file diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 34bdcf8aa7..cea7bb5b46 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -22,6 +22,9 @@ #define USBD_PRODUCT_STRING "BlueJayF4" +#define USE_HARDWARE_REVISION_DETECTION +#define HW_PIN PB2 + #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_EXTI @@ -76,11 +79,11 @@ // Performance logging for SD card operations: // #define AFATFS_USE_INTROSPECTIVE_LOGGING -//#define M25P16_CS_PIN PB3 -//#define M25P16_SPI_INSTANCE SPI3 +#define M25P16_CS_PIN PB7 +#define M25P16_SPI_INSTANCE SPI3 -//#define USE_FLASHFS -//#define USE_FLASH_M25P16 +#define USE_FLASHFS +#define USE_FLASH_M25P16 #define USABLE_TIMER_CHANNEL_COUNT 7 diff --git a/src/main/target/BLUEJAYF4/target.mk b/src/main/target/BLUEJAYF4/target.mk index 23dd74507a..d0a982faad 100644 --- a/src/main/target/BLUEJAYF4/target.mk +++ b/src/main/target/BLUEJAYF4/target.mk @@ -1,5 +1,5 @@ F405_TARGETS += $(TARGET) -FEATURES += SDCARD VCP +FEATURES += SDCARD VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_spi_mpu6500.c \ From 0a9beee25ddef62cbe902f4d3cb3c93bfd4ba21f Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 30 Jul 2016 12:02:07 +1000 Subject: [PATCH 136/456] Added support to turn inversion off for UART1 will need to look at controlling it within configuration once param groups are in place. --- src/main/drivers/inverter.c | 9 +++++++-- src/main/target/BLUEJAYF4/hardware_revision.c | 19 ++++++++++++++++--- 2 files changed, 23 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/inverter.c b/src/main/drivers/inverter.c index 967c87680a..336f850865 100644 --- a/src/main/drivers/inverter.c +++ b/src/main/drivers/inverter.c @@ -27,11 +27,16 @@ #include "inverter.h" -static const IO_t pin = DEFIO_IO(INVERTER); +/* + TODO: move this to support multiple inverters on different UARTs etc + possibly move to put it in the UART driver itself. +*/ +static IO_t pin = IO_NONE; void initInverter(void) { - IOInit(pin, OWNER_INVERTER, RESOURCE_OUTPUT, 0); + pin = IOGetByTag(IO_TAG(INVERTER)); + IOInit(pin, OWNER_INVERTER, RESOURCE_OUTPUT, 1); IOConfigGPIO(pin, IOCFG_OUT_PP); inverterSet(false); diff --git a/src/main/target/BLUEJAYF4/hardware_revision.c b/src/main/target/BLUEJAYF4/hardware_revision.c index 6ed07e16f9..3162e9f487 100644 --- a/src/main/target/BLUEJAYF4/hardware_revision.c +++ b/src/main/target/BLUEJAYF4/hardware_revision.c @@ -58,13 +58,26 @@ void detectHardwareRevision(void) if (!IORead(pin1)) { if (!IORead(pin2)) { hardwareRevision = BJF4_REV3A; - return; } hardwareRevision = BJF4_REV3; - return; } - hardwareRevision = BJF4_REV2; + if (hardwareRevision == UNKNOWN) { + hardwareRevision = BJF4_REV2; + return; + } + + /* + enable the UART1 inversion PC9 + + TODO: once param groups are in place, inverter outputs + can be moved to be simple IO outputs, and merely set them + HI or LO in configuration. + */ + IO_t uart1invert = IOGetByTag(IO_TAG(PC9)); + IOInit(uart1invert, OWNER_INVERTER, RESOURCE_OUTPUT, 2); + IOConfigGPIO(uart1invert, IOCFG_AF_PP); + IOLo(uart1invert); } void updateHardwareRevision(void) From 552e32a287bba811bb2b3799b0f218b539685d7b Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 30 Jul 2016 13:50:09 +1000 Subject: [PATCH 137/456] minor formatting --- src/main/target/ALIENFLIGHTF1/config.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index 411f4e4154..b5bea8104a 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -76,7 +76,8 @@ #include "config/config_master.h" // alternative defaults settings for AlienFlight targets -void targetConfiguration(void) { +void targetConfiguration(void) +{ featureClear(FEATURE_ONESHOT125); masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; From 49f9eec1e9abf64ceaa8fa93c50a7ee47c28d646 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 30 Jul 2016 14:28:23 +1000 Subject: [PATCH 138/456] Gyro on Rev3 and Rev3a Bluejay are 0 aligned --- src/main/config/config.c | 6 +-- src/main/drivers/flash_m25p16.c | 3 +- src/main/target/BLUEJAYF4/config.c | 87 ++++++++++++++++++++++++++++++ src/main/target/BLUEJAYF4/target.h | 5 +- src/main/target/common.h | 2 + 5 files changed, 95 insertions(+), 8 deletions(-) create mode 100644 src/main/target/BLUEJAYF4/config.c diff --git a/src/main/config/config.c b/src/main/config/config.c index b0271ef278..4e16b8c755 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -530,11 +530,7 @@ static void resetConf(void) masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_OFF; masterConfig.rxConfig.rcSmoothInterval = 9; masterConfig.rxConfig.fpvCamAngleDegrees = 0; -#ifdef STM32F4 - masterConfig.rxConfig.max_aux_channel = 99; -#else - masterConfig.rxConfig.max_aux_channel = 6; -#endif + masterConfig.rxConfig.max_aux_channel = MAX_AUX_CHANNELS; masterConfig.rxConfig.airModeActivateThreshold = 1350; resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index d1e62cccbe..0ab3d6122b 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -213,7 +213,8 @@ bool m25p16_init(ioTag_t csTag) } else { #ifdef M25P16_CS_PIN m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN)); -#else return false; +#else + return false; #endif } IOInit(m25p16CsPin, OWNER_FLASH, RESOURCE_SPI_CS, 0); diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c new file mode 100644 index 0000000000..71007bb806 --- /dev/null +++ b/src/main/target/BLUEJAYF4/config.c @@ -0,0 +1,87 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include + +#include "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +#include "hardware_revision.h" + +// alternative defaults settings for BlueJayF4 targets +void targetConfiguration(void) +{ + if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) { + masterConfig.sensorAlignmentConfig.gyro_align = CW180_DEG; + masterConfig.sensorAlignmentConfig.acc_align = CW180_DEG; + } +} diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index cea7bb5b46..39cc723a64 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -17,6 +17,7 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "BJF4" +#define TARGET_CONFIG #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) @@ -44,12 +45,12 @@ #define ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW180_DEG +#define ACC_MPU6500_ALIGN CW0_DEG #define GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW180_DEG +#define GYRO_MPU6500_ALIGN CW0_DEG //#define MAG //#define USE_MAG_AK8963 diff --git a/src/main/target/common.h b/src/main/target/common.h index 65ab02c871..0e74b56c89 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -24,6 +24,7 @@ /* STM32F4 specific settings that apply to all F4 targets */ #ifdef STM32F4 +#define MAX_AUX_CHANNELS 99 #define TASK_GYROPID_DESIRED_PERIOD 125 #define SCHEDULER_DELAY_LIMIT 10 #define USE_SLOW_SERIAL_CLI @@ -31,6 +32,7 @@ #else /* when not an F4 */ +#define MAX_AUX_CHANNELS 6 #define TASK_GYROPID_DESIRED_PERIOD 1000 #define SCHEDULER_DELAY_LIMIT 100 From df11d398aab1dd5cab9dca3609ea76c26c128752 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 31 Jul 2016 11:06:29 +1000 Subject: [PATCH 139/456] Warning removal where present if pidBetaflight is skipped. --- src/main/flight/pid.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 16aa6e45ac..4b916911d9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -66,9 +66,10 @@ uint8_t PIDweight[3]; static int32_t errorGyroI[3]; static float errorGyroIf[3]; -#ifdef SKIP_PID_FLOAT + static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); +#ifdef SKIP_PID_FLOAT pidControllerFuncPtr pid_controller = pidLegacy; // which pid controller are we using #else static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, @@ -107,6 +108,7 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static pt1Filter_t deltaFilter[3]; static pt1Filter_t yawFilter; +#ifndef SKIP_PID_FLOAT // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab) static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) @@ -302,6 +304,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc #endif } } +#endif // Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, From 1b73a8f2aaf7ef3972520e8d5ab0d0ddd395601b Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 31 Jul 2016 11:20:27 +1000 Subject: [PATCH 140/456] Whitespace cleanup and simplification of delay for serial cli F4 support --- src/main/io/serial_cli.c | 40 ++++++++++------------------------------ 1 file changed, 10 insertions(+), 30 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9ac1946b19..87fd199559 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -826,7 +826,7 @@ const clivalue_t valueTable[] = { { "pid_max_velocity", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocity, .config.minmax = {0, 2000 } }, { "pid_max_velocity_yaw", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 2000 } }, - { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, + { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, @@ -2009,11 +2009,6 @@ static void dumpValues(uint16_t valueSection) cliPrintf("set %s = ", valueTable[i].name); cliPrintVar(value, 0); cliPrint("\r\n"); - -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif - } } @@ -2058,7 +2053,7 @@ static void cliDump(char *cmdline) cliPrint("\r\n# version\r\n"); cliVersion(NULL); - printSectionBreak(); + printSectionBreak(); cliPrint("\r\n# name\r\n"); cliName(NULL); @@ -2089,9 +2084,6 @@ static void cliDump(char *cmdline) if (yaw < 0) cliWrite(' '); cliPrintf("%s\r\n", ftoa(yaw, buf)); -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } #ifdef USE_SERVOS @@ -2113,10 +2105,6 @@ static void cliDump(char *cmdline) masterConfig.customServoMixer[i].max, masterConfig.customServoMixer[i].box ); - -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } #endif @@ -2128,18 +2116,12 @@ static void cliDump(char *cmdline) if (featureNames[i] == NULL) break; cliPrintf("feature -%s\r\n", featureNames[i]); -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } for (i = 0; ; i++) { // reenable what we want. if (featureNames[i] == NULL) break; if (mask & (1 << i)) cliPrintf("feature %s\r\n", featureNames[i]); -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } #ifdef BEEPER @@ -2194,9 +2176,6 @@ static void cliDump(char *cmdline) for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { if (servoDirection(i, channel) < 0) { cliPrintf("smix reverse %d %d r\r\n", i , channel); -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } } } @@ -2227,9 +2206,6 @@ static void cliDump(char *cmdline) cliPrint("\r\n# restore original rateprofile selection\r\n"); changeControlRateProfile(currentRateIndex); cliRateProfile(""); -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } cliPrint("\r\n# restore original profile selection\r\n"); @@ -2710,6 +2686,10 @@ static void cliPrint(const char *str) { while (*str) bufWriterAppend(cliWriter, *str++); + +#ifdef USE_SLOW_SERIAL_CLI + delay(1); +#endif } static void cliPutp(void *p, char ch) @@ -2723,6 +2703,10 @@ static void cliPrintf(const char *fmt, ...) va_start(va, fmt); tfp_format(cliWriter, cliPutp, fmt, va); va_end(va); + +#ifdef USE_SLOW_SERIAL_CLI + delay(1); +#endif } static void cliWrite(uint8_t ch) @@ -2856,10 +2840,6 @@ static void cliSet(char *cmdline) cliPrintf("%s = ", valueTable[i].name); cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui cliPrint("\r\n"); - -#ifdef USE_SLOW_SERIAL_CLI - delay(2); -#endif } } else if ((eqptr = strstr(cmdline, "=")) != NULL) { // has equals From 53cefb9d1449fd1a053b1c6790749962aacac670 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 31 Jul 2016 15:39:26 +1000 Subject: [PATCH 141/456] Fixed beeper on BJF4 rev3 --- src/main/main.c | 6 ++++++ src/main/target/BLUEJAYF4/target.h | 3 ++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/main.c b/src/main/main.c index 7be831d73c..79e415c1c2 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -362,6 +362,12 @@ void init(void) beeperConfig.isInverted = true; } #endif +/* temp until PGs are implemented. */ +#ifdef BLUEJAYF4 + if (hardwareRevision <= BJF4_REV2) { + beeperConfig.ioTag = IO_TAG(BEEPER_OPT); + } +#endif #ifdef CC3D if (masterConfig.use_buzzer_p6 == 1) beeperConfig.ioTag = IO_TAG(BEEPER_OPT); diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 39cc723a64..73f7f86c99 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -33,7 +33,8 @@ #define LED1 PB5 #define LED2 PB4 -#define BEEPER PB7 +#define BEEPER PC1 +#define BEEPER_OPT PB7 #define BEEPER_INVERTED #define INVERTER PB15 From bde49c92b1b08ccdba798491baf3d33590ebf94f Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 1 Aug 2016 06:30:31 +0100 Subject: [PATCH 142/456] Minor tidy of target.h files --- src/main/target/ALIENFLIGHTF4/target.h | 6 +-- src/main/target/BLUEJAYF4/target.h | 16 +++--- src/main/target/FURYF4/target.h | 12 ++--- src/main/target/OMNIBUS/target.h | 12 ++--- src/main/target/OMNIBUSF4/target.h | 67 +++++++++++++------------ src/main/target/SPRACINGF3EVO/target.h | 6 +-- src/main/target/SPRACINGF3MINI/target.h | 6 +-- 7 files changed, 59 insertions(+), 66 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 5bd61457fc..3469c0c113 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -32,12 +32,12 @@ #define INVERTER_USART USART2 // MPU interrupt +#define USE_EXTI +#define MPU_INT_EXTI PC14 +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready -#define MPU_INT_EXTI PC14 -#define USE_EXTI #define MPU6500_CS_PIN PA4 #define MPU6500_SPI_INSTANCE SPI1 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 73f7f86c99..737338b054 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -40,6 +40,12 @@ #define INVERTER PB15 #define INVERTER_USART USART6 +// MPU6500 interrupt +//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW +//#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready +#define MPU_INT_EXTI PC5 #define MPU6500_CS_PIN PC4 #define MPU6500_SPI_INSTANCE SPI1 @@ -87,15 +93,6 @@ #define USE_FLASHFS #define USE_FLASH_M25P16 -#define USABLE_TIMER_CHANNEL_COUNT 7 - -// MPU6500 interrupt -//#define DEBUG_MPU_DATA_READY_INTERRUPT -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW -//#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready -#define MPU_INT_EXTI PC5 - #define USE_VCP //#define VBUS_SENSING_PIN PA8 //#define VBUS_SENSING_ENABLED @@ -169,5 +166,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 7 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index c6263bcd47..073e2fb916 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -31,6 +31,12 @@ #define INVERTER PC0 // PC0 used as inverter select GPIO #define INVERTER_USART USART1 +// MPU6000 interrupts +#define USE_EXTI +#define MPU_INT_EXTI PC4 +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready (mag disabled) +#define USE_MPU_DATA_READY_SIGNAL + #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 @@ -50,12 +56,6 @@ #define USE_ACC_SPI_MPU6500 #define ACC_MPU6500_ALIGN CW180_DEG -// MPU6000 interrupts -#define USE_MPU_DATA_READY_SIGNAL -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PC4 -#define USE_EXTI - #define BARO #define USE_BARO_MS5611 #define MS5611_I2C_INSTANCE I2CDEV_1 diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 87c5c286a9..0a0ec2593d 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -26,14 +26,11 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 10 // 6 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins. - -#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect - +#define USE_EXTI +#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PC13 -#define USE_EXTI +#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define MPU6000_SPI_INSTANCE SPI1 #define MPU6000_CS_PIN PA4 @@ -71,7 +68,7 @@ #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define SERIAL_PORT_COUNT 4 +#define SERIAL_PORT_COUNT 4 #define UART1_TX_PIN PA9 #define UART1_RX_PIN PA10 @@ -192,4 +189,5 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 10 // 6 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins. #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index fccca3016b..5250a336b7 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -26,28 +26,30 @@ #define USBD_SERIALNUMBER_STRING "0x8020000" #endif -#define LED0 PB5 -#define LED1 PB4 -#define BEEPER PB4 -#define INVERTER PC0 // PC0 used as inverter select GPIO -#define INVERTER_USART USART1 +#define LED0 PB5 +#define LED1 PB4 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_INSTANCE SPI1 +#define BEEPER PB4 + +#define INVERTER PC0 // PC0 used as inverter select GPIO +#define INVERTER_USART USART1 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG +#define GYRO_MPU6000_ALIGN CW270_DEG #define GYRO #define USE_GYRO_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG +#define ACC_MPU6000_ALIGN CW270_DEG // MPU6000 interrupts +#define USE_EXTI +#define MPU_INT_EXTI PC4 #define USE_MPU_DATA_READY_SIGNAL #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PC4 -#define USE_EXTI #define MAG #define USE_MAG_HMC5883 @@ -61,8 +63,8 @@ #define OSD #define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI3 -#define MAX7456_SPI_CS_PIN PA15 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PA15 #define MAX7456_DMA_CHANNEL_TX DMA1_Stream5 #define MAX7456_DMA_CHANNEL_RX DMA1_Stream0 @@ -72,31 +74,29 @@ //#define USE_PITOT_MS4525 //#define MS4525_BUS I2C_DEVICE_EXT -#define M25P16_CS_PIN SPI3_NSS_PIN -#define M25P16_SPI_INSTANCE SPI3 +#define M25P16_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_INSTANCE SPI3 #define USE_FLASHFS #define USE_FLASH_M25P16 -#define USABLE_TIMER_CHANNEL_COUNT 12 - #define USE_VCP #define VBUS_SENSING_PIN PC5 -#define USE_USART1 -#define USART1_RX_PIN PA10 -#define USART1_TX_PIN PA9 -#define USART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 -#define USE_USART3 -#define USART3_RX_PIN PB11 -#define USART3_TX_PIN PB10 +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 -#define USE_USART6 -#define USART6_RX_PIN PC7 -#define USART6_TX_PIN PC6 +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 -#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 +#define SERIAL_PORT_COUNT 4 //VCP, USART1, USART3, USART6 #define USE_SPI @@ -112,16 +112,16 @@ #define I2C_DEVICE (I2CDEV_1) #define USE_ADC -#define CURRENT_METER_ADC_PIN PC1 -#define VBAT_ADC_PIN PC2 -#define RSSI_ADC_GPIO_PIN PA0 +#define CURRENT_METER_ADC_PIN PC1 +#define VBAT_ADC_PIN PC2 +#define RSSI_ADC_GPIO_PIN PA0 #define SENSORS_SET (SENSOR_ACC) -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125 | FEATURE_RX_SERIAL) +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125 | FEATURE_RX_SERIAL) #define USE_SERIAL_4WAY_BLHELI_INTERFACE @@ -130,4 +130,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff +#define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index 94117a125c..9ba3c4a989 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -26,12 +26,9 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 12 // PPM, 8 PWM, UART3 RX/TX, LED Strip - -#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect - #define USE_EXTI #define MPU_INT_EXTI PC13 +#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -160,5 +157,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 12 // PPM, 8 PWM, UART3 RX/TX, LED Strip #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15)) diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 799da79d47..b5ee87abf3 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -29,12 +29,9 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 12 // 8 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins. - -#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect - #define USE_EXTI #define MPU_INT_EXTI PC13 +#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW @@ -176,5 +173,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 12 // 8 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins. #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) From 168469236b512b1edd958941f0b75694c60c528c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 1 Aug 2016 06:45:59 +0100 Subject: [PATCH 143/456] Tidied serial and telemetry files --- src/main/drivers/serial_uart.c | 16 ++++++++-------- src/main/drivers/serial_usb_vcp.c | 4 ++-- src/main/io/serial.c | 4 ++-- src/main/io/serial.h | 1 + src/main/rx/jetiexbus.c | 6 ++++-- src/main/rx/jetiexbus.h | 13 ------------- src/main/telemetry/frsky.c | 1 - src/main/telemetry/frsky.h | 6 ++---- src/main/telemetry/hott.c | 5 +++-- src/main/telemetry/jetiexbus.h | 23 +++++++++++++++++++++++ src/main/telemetry/ltm.c | 4 ++-- src/main/telemetry/telemetry.c | 3 +-- 12 files changed, 48 insertions(+), 38 deletions(-) create mode 100644 src/main/telemetry/jetiexbus.h diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index ced8d97fff..4663701179 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -298,7 +298,7 @@ uint32_t uartTotalRxBytesWaiting(serialPort_t *instance) if (s->rxDMAStream) { uint32_t rxDMAHead = s->rxDMAStream->NDTR; #else - if (s->rxDMAChannel) { + if (s->rxDMAChannel) { uint32_t rxDMAHead = s->rxDMAChannel->CNDTR; #endif if (rxDMAHead >= s->rxDMAPos) { @@ -421,13 +421,13 @@ void uartWrite(serialPort_t *instance, uint8_t ch) const struct serialPortVTable uartVTable[] = { { - uartWrite, - uartTotalRxBytesWaiting, - uartTotalTxBytesFree, - uartRead, - uartSetBaudRate, - isUartTransmitBufferEmpty, - uartSetMode, + .serialWrite = uartWrite, + .serialTotalRxWaiting = uartTotalRxBytesWaiting, + .serialTotalTxFree = uartTotalTxBytesFree, + .serialRead = uartRead, + .serialSetBaudRate = uartSetBaudRate, + .isSerialTransmitBufferEmpty = isUartTransmitBufferEmpty, + .setMode = uartSetMode, .writeBuf = NULL, .beginWrite = NULL, .endWrite = NULL, diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 8360b0068f..2c2cebbac6 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -171,9 +171,9 @@ static const struct serialPortVTable usbVTable[] = { .serialSetBaudRate = usbVcpSetBaudRate, .isSerialTransmitBufferEmpty = isUsbVcpTransmitBufferEmpty, .setMode = usbVcpSetMode, + .writeBuf = usbVcpWriteBuf, .beginWrite = usbVcpBeginWrite, - .endWrite = usbVcpEndWrite, - .writeBuf = usbVcpWriteBuf + .endWrite = usbVcpEndWrite } }; diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 0f94c7bc15..2e8e2eab44 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -194,7 +194,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction #ifdef TELEMETRY #define ALL_TELEMETRY_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT) #else -#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM) +#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) #endif #define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK) @@ -270,7 +270,7 @@ serialPort_t *openSerialPort( portMode_t mode, portOptions_t options) { -#if (!defined(USE_VCP) && !defined(USE_UART1) && !defined(USE_UART2) && !defined(USE_UART3) && !defined(USE_SOFTSERIAL1) && !defined(USE_SOFTSERIAL1)) +#if (!defined(USE_VCP) && !defined(USE_UART1) && !defined(USE_UART2) && !defined(USE_UART3) && !defined(USE_UART4) && !defined(USE_UART5) && !defined(USE_UART6) && !defined(USE_SOFTSERIAL1) && !defined(USE_SOFTSERIAL2)) UNUSED(callback); UNUSED(baudRate); UNUSED(mode); diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 077ad07293..924f582323 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -34,6 +34,7 @@ typedef enum { FUNCTION_RX_SERIAL = (1 << 6), // 64 FUNCTION_BLACKBOX = (1 << 7), // 128 FUNCTION_PASSTHROUGH = (1 << 8), // 256 + FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512 } serialPortFunction_e; typedef enum { diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index d3ac730016..2836d5ebca 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -35,16 +35,17 @@ */ #include -#include #include -#include "common/utils.h" #include "platform.h" + +#include "common/utils.h" #include "build_config.h" #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/serial_uart.h" #include "io/serial.h" +#include "rx/rx.h" #include "rx/jetiexbus.h" @@ -55,6 +56,7 @@ #include "sensors/battery.h" #include "sensors/barometer.h" #include "telemetry/telemetry.h" +#include "telemetry/jetiexbus.h" #endif //TELEMETRY diff --git a/src/main/rx/jetiexbus.h b/src/main/rx/jetiexbus.h index a975e5c643..52b0bb45c4 100644 --- a/src/main/rx/jetiexbus.h +++ b/src/main/rx/jetiexbus.h @@ -17,19 +17,6 @@ #pragma once - -#include "rx/rx.h" - bool jetiExBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); uint8_t jetiExBusFrameStatus(void); - -#ifdef TELEMETRY - -#include "telemetry/telemetry.h" - -void initJetiExBusTelemetry(telemetryConfig_t *initialTelemetryConfig); -void checkJetiExBusTelemetryState(void); -void handleJetiExBusTelemetry(void); - -#endif //TELEMETRY diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 35185b9676..33bd13193c 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -21,7 +21,6 @@ */ #include #include -#include #include "platform.h" diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index 8d1b82d61c..555eb43e83 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -15,10 +15,9 @@ * along with Cleanflight. If not, see . */ -#include "rx/rx.h" +#pragma once -#ifndef TELEMETRY_FRSKY_H_ -#define TELEMETRY_FRSKY_H_ +#include "rx/rx.h" typedef enum { FRSKY_VFAS_PRECISION_LOW = 0, @@ -32,4 +31,3 @@ void initFrSkyTelemetry(telemetryConfig_t *telemetryConfig); void configureFrSkyTelemetryPort(void); void freeFrSkyTelemetryPort(void); -#endif /* TELEMETRY_FRSKY_H_ */ diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 7ccf8ea197..dcab855ae0 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -57,11 +57,12 @@ #include #include "platform.h" -#include "build_config.h" -#include "debug.h" #ifdef TELEMETRY +#include "build_config.h" +#include "debug.h" + #include "common/axis.h" #include "drivers/system.h" diff --git a/src/main/telemetry/jetiexbus.h b/src/main/telemetry/jetiexbus.h new file mode 100644 index 0000000000..79440c0df2 --- /dev/null +++ b/src/main/telemetry/jetiexbus.h @@ -0,0 +1,23 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +struct telemetryConfig_s; +void initJetiExBusTelemetry(struct telemetryConfig_s *initialTelemetryConfig); +void checkJetiExBusTelemetryState(void); +void handleJetiExBusTelemetry(void); diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index c70146f64f..fce4fbc915 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -33,10 +33,10 @@ #include "platform.h" -#include "build_config.h" - #ifdef TELEMETRY +#include "build_config.h" + #include "common/maths.h" #include "common/axis.h" #include "common/color.h" diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index e3871aead0..96e2193e9a 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -17,7 +17,6 @@ #include #include -#include #include "platform.h" @@ -40,7 +39,7 @@ #include "telemetry/hott.h" #include "telemetry/smartport.h" #include "telemetry/ltm.h" -#include "rx/jetiexbus.h" +#include "telemetry/jetiexbus.h" static telemetryConfig_t *telemetryConfig; From 69955d7b07685ab0d2104e353003b6c8ee0c54fa Mon Sep 17 00:00:00 2001 From: Evgeny Sychov Date: Mon, 1 Aug 2016 02:14:40 -0700 Subject: [PATCH 144/456] display craft name, current mesurement on OSD --- src/main/io/osd.c | 26 ++++++++++++++++++++++++++ src/main/io/osd.h | 3 +++ src/main/io/serial_cli.c | 3 +++ src/main/target/SIRINFPV/target.h | 2 ++ 4 files changed, 34 insertions(+) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ea458e66ee..c3a00bd68d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -20,6 +20,7 @@ #include #include #include +#include #include "platform.h" #include "version.h" @@ -753,6 +754,28 @@ void updateOsd(void) sprintf(line+1, "%d.%1d", vbat / 10, vbat % 10); max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]); } + + if (masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW] != -1) { + line[0] = SYM_AMP; + sprintf(line+1, "%d.%02d", amperage / 100, amperage % 100); + max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW]); + } + + if (masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] != -1) { + line[0] = SYM_MAH; + sprintf(line+1, "%d", mAhDrawn); + max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN]); + } + + if (masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME] != -1) { + for (uint8_t i = 0; i < MAX_NAME_LENGTH; i++) { + line[i] = toupper((unsigned char)masterConfig.name[i]); + if (masterConfig.name[i] == 0) + break; + } + max7456_write_string(line, masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME]); + } + if (masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] != -1) { line[0] = SYM_RSSI; sprintf(line+1, "%d", rssi / 10); @@ -823,6 +846,9 @@ void resetOsdConfig(void) masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON] = -1; masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS] = -1; + masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW] = -23; + masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] = -18; + masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME] = -77; } #endif diff --git a/src/main/io/osd.h b/src/main/io/osd.h index e348e4573c..b5f4e953db 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -50,6 +50,9 @@ typedef enum { OSD_DISARMED, OSD_ARTIFICIAL_HORIZON, OSD_HORIZON_SIDEBARS, + OSD_CURRENT_DRAW, + OSD_MAH_DRAWN, + OSD_CRAFT_NAME, OSD_MAX_ITEMS, // MUST BE LAST } osd_items_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 87fd199559..5e89ff37e0 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -893,6 +893,9 @@ const clivalue_t valueTable[] = { { "osd_disarmed_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_DISARMED], .config.minmax = { -480, 480 } }, { "osd_artificial_horizon", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], .config.minmax = { -1, 0 } }, { "osd_horizon_sidebars", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], .config.minmax = { -1, 0 } }, + { "osd_current_draw_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], .config.minmax = { -480, 480 } }, + { "osd_mah_drawn_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], .config.minmax = { -480, 480 } }, + { "osd_craft_name_pos", VAR_INT16 | MASTER_VALUE, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], .config.minmax = { -480, 480 } }, #endif }; diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 1632d2046d..cab5ad2edd 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -128,6 +128,8 @@ #define USE_ADC #define ADC_INSTANCE ADC1 #define VBAT_ADC_PIN PA0 +#define CURRENT_METER_ADC_PIN PA3 +#define RSSI_ADC_PIN PA2 //#define USE_QUAD_MIXER_ONLY #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT From bd560198f2bdd8395a6ea336849a2cf2f386d1e3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 1 Aug 2016 10:33:59 +0100 Subject: [PATCH 145/456] Removed unnecessary includes. Changed to use #pragma once --- src/main/telemetry/hott.h | 5 +---- src/main/telemetry/ltm.c | 2 -- src/main/telemetry/ltm.h | 4 +--- src/main/telemetry/smartport.c | 2 -- src/main/telemetry/smartport.h | 4 +--- 5 files changed, 3 insertions(+), 14 deletions(-) diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index 37b8e6f10f..64a43bd507 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -23,9 +23,7 @@ * Texmode add-on by Michi (mamaretti32@gmail.com) */ -#ifndef HOTT_TELEMETRY_H_ -#define HOTT_TELEMETRY_H_ - +#pragma once #define HOTTV4_RXTX 4 @@ -498,4 +496,3 @@ uint32_t getHoTTTelemetryProviderBaudRate(void); void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage); -#endif /* HOTT_TELEMETRY_H_ */ diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index fce4fbc915..a7b7556e10 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -79,8 +79,6 @@ #include "config/config.h" #include "config/runtime_config.h" -#include "config/config_profile.h" -#include "config/config_master.h" #define TELEMETRY_LTM_INITIAL_PORT_MODE MODE_TX #define LTM_CYCLETIME 100 diff --git a/src/main/telemetry/ltm.h b/src/main/telemetry/ltm.h index c78dfc22e4..6e4b80cbfa 100644 --- a/src/main/telemetry/ltm.h +++ b/src/main/telemetry/ltm.h @@ -17,8 +17,7 @@ * along with Cleanflight. If not, see . */ -#ifndef TELEMETRY_LTM_H_ -#define TELEMETRY_LTM_H_ +#pragma once void initLtmTelemetry(telemetryConfig_t *initialTelemetryConfig); void handleLtmTelemetry(void); @@ -27,4 +26,3 @@ void checkLtmTelemetryState(void); void freeLtmTelemetryPort(void); void configureLtmTelemetryPort(void); -#endif /* TELEMETRY_LTM_H_ */ diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 03f257d05f..8d167dd2ca 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -60,8 +60,6 @@ #include "config/runtime_config.h" #include "config/config.h" -#include "config/config_profile.h" -#include "config/config_master.h" enum { diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index 74643f629f..b24be90708 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -5,8 +5,7 @@ * Author: Frank26080115 */ -#ifndef TELEMETRY_SMARTPORT_H_ -#define TELEMETRY_SMARTPORT_H_ +#pragma once void initSmartPortTelemetry(telemetryConfig_t *); @@ -18,4 +17,3 @@ void freeSmartPortTelemetryPort(void); bool isSmartPortTimedOut(void); -#endif /* TELEMETRY_SMARTPORT_H_ */ From aacfe7b9911caabda8fed6968aba9110733c9e64 Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Mon, 1 Aug 2016 15:38:59 +0200 Subject: [PATCH 146/456] rearrange boxes and fixed unused boxes not disabled - rearrange boxes means first came arm and airmode after this came the flightmodes and at the end all rc modes - this makes a better overview in the configurator modes tab --- src/main/io/serial_msp.c | 86 +++++++++++++++++++++++----------------- 1 file changed, 49 insertions(+), 37 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 924610d129..4b3a5767b7 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -471,16 +471,15 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIdCount = 0; activeBoxIds[activeBoxIdCount++] = BOXARM; + if (!feature(FEATURE_AIRMODE)) { + activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; + } + if (sensors(SENSOR_ACC)) { activeBoxIds[activeBoxIdCount++] = BOXANGLE; activeBoxIds[activeBoxIdCount++] = BOXHORIZON; } - if (!feature(FEATURE_AIRMODE)) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; - activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH; - - activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; - if (sensors(SENSOR_BARO)) { activeBoxIds[activeBoxIdCount++] = BOXBARO; } @@ -491,9 +490,6 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXHEADADJ; } - if (feature(FEATURE_SERVO_TILT)) - activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; - #ifdef GPS if (feature(FEATURE_GPS)) { activeBoxIds[activeBoxIdCount++] = BOXGPSHOME; @@ -501,10 +497,21 @@ void mspInit(serialConfig_t *serialConfig) } #endif - if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) - activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU; +#ifdef SONAR + if (feature(FEATURE_SONAR)) { + activeBoxIds[activeBoxIdCount++] = BOXSONAR; + } +#endif - activeBoxIds[activeBoxIdCount++] = BOXBEEPERON; + if (feature(FEATURE_FAILSAFE)) { + activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE; + } + + if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) { + activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU; + } + + activeBoxIds[activeBoxIdCount++] = BOXBEEPERON; #ifdef LED_STRIP if (feature(FEATURE_LED_STRIP)) { @@ -512,21 +519,40 @@ void mspInit(serialConfig_t *serialConfig) } #endif - if (feature(FEATURE_INFLIGHT_ACC_CAL)) - activeBoxIds[activeBoxIdCount++] = BOXCALIB; - - activeBoxIds[activeBoxIdCount++] = BOXOSD; - -#ifdef TELEMETRY - if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch) - activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY; -#endif -#ifdef SONAR - if (feature(FEATURE_SONAR)){ - activeBoxIds[activeBoxIdCount++] = BOXSONAR; +#ifdef BLACKBOX + if (feature(FEATURE_BLACKBOX)) { + activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX; } #endif + activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; + + if (feature(FEATURE_3D)) { + activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH; + } + + if (feature(FEATURE_SERVO_TILT)) { + activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; + } + + if (feature(FEATURE_INFLIGHT_ACC_CAL)) { + activeBoxIds[activeBoxIdCount++] = BOXCALIB; + } + + if (feature(FEATURE_OSD)) { + activeBoxIds[activeBoxIdCount++] = BOXOSD; + } + +#ifdef TELEMETRY + if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch) { + activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY; + } +#endif + +#ifdef GTUNE + activeBoxIds[activeBoxIdCount++] = BOXGTUNE; +#endif + #ifdef USE_SERVOS if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) { activeBoxIds[activeBoxIdCount++] = BOXSERVO1; @@ -535,20 +561,6 @@ void mspInit(serialConfig_t *serialConfig) } #endif -#ifdef BLACKBOX - if (feature(FEATURE_BLACKBOX)){ - activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX; - } -#endif - - if (feature(FEATURE_FAILSAFE)){ - activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE; - } - -#ifdef GTUNE - activeBoxIds[activeBoxIdCount++] = BOXGTUNE; -#endif - memset(mspPorts, 0x00, sizeof(mspPorts)); mspAllocateSerialPorts(serialConfig); } From 0258edd430afeb4ceeb401b4b11b2b171731f30b Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Mon, 1 Aug 2016 15:48:37 +0200 Subject: [PATCH 147/456] cleanups --- src/main/io/serial_msp.c | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 4b3a5767b7..ee9130f0ac 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -472,8 +472,8 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXARM; if (!feature(FEATURE_AIRMODE)) { - activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; - } + activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; + } if (sensors(SENSOR_ACC)) { activeBoxIds[activeBoxIdCount++] = BOXANGLE; @@ -509,9 +509,9 @@ void mspInit(serialConfig_t *serialConfig) if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) { activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU; - } + } - activeBoxIds[activeBoxIdCount++] = BOXBEEPERON; + activeBoxIds[activeBoxIdCount++] = BOXBEEPERON; #ifdef LED_STRIP if (feature(FEATURE_LED_STRIP)) { @@ -528,25 +528,25 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXFPVANGLEMIX; if (feature(FEATURE_3D)) { - activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH; - } + activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH; + } if (feature(FEATURE_SERVO_TILT)) { activeBoxIds[activeBoxIdCount++] = BOXCAMSTAB; - } + } if (feature(FEATURE_INFLIGHT_ACC_CAL)) { activeBoxIds[activeBoxIdCount++] = BOXCALIB; - } + } - if (feature(FEATURE_OSD)) { - activeBoxIds[activeBoxIdCount++] = BOXOSD; - } + if (feature(FEATURE_OSD)) { + activeBoxIds[activeBoxIdCount++] = BOXOSD; + } #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch) { activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY; - } + } #endif #ifdef GTUNE From 188cd2649209c9055e29f00c177462471c43e00b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 1 Aug 2016 16:14:11 +0200 Subject: [PATCH 148/456] Fix motor spinning issue on save for multishot / oneshot42 --- src/main/config/config.c | 7 ++----- src/main/version.h | 2 +- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 330561d7e8..7debf0f162 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -1060,11 +1060,8 @@ void handleOneshotFeatureChangeOnRestart(void) { // Shutdown PWM on all motors prior to soft restart StopPwmAllMotors(); - delay(50); - // Apply additional delay when OneShot125 feature changed from on to off state - if (feature(FEATURE_ONESHOT125) && !featureConfigured(FEATURE_ONESHOT125)) { - delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS); - } + // Apply additional delay + delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS); } void latchActiveFeatures() diff --git a/src/main/version.h b/src/main/version.h index 5ec30cdd32..eb4d3b0450 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -17,7 +17,7 @@ #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 9 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) From 2e0433c78f9195346bec9eb434a8444374a3ec1f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 1 Aug 2016 16:45:03 +0200 Subject: [PATCH 149/456] Cleanup MSP --- src/main/io/serial_msp.c | 27 +++++++++------------------ src/main/io/serial_msp.h | 11 ++++------- 2 files changed, 13 insertions(+), 25 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index ab195fa8fd..f59a5e2882 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -859,7 +859,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16((uint16_t)targetLooptime); break; case MSP_RC_TUNING: - headSerialReply(11); + headSerialReply(12); serialize8(currentControlRateProfile->rcRate8); serialize8(currentControlRateProfile->rcExpo8); for (i = 0 ; i < 3; i++) { @@ -870,6 +870,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentControlRateProfile->thrExpo8); serialize16(currentControlRateProfile->tpa_breakpoint); serialize8(currentControlRateProfile->rcYawExpo8); + serialize8(currentControlRateProfile->rcYawRate8); break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); @@ -1244,7 +1245,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.sensorAlignmentConfig.acc_align); serialize8(masterConfig.sensorAlignmentConfig.mag_align); break; - case MSP_PID_ADVANCED_CONFIG : + case MSP_ADVANCED_CONFIG : headSerialReply(6); serialize8(masterConfig.gyro_sync_denom); serialize8(masterConfig.pid_process_denom); @@ -1258,7 +1259,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentProfile->pidProfile.dterm_lpf_hz); serialize16(currentProfile->pidProfile.yaw_lpf_hz); break; - case MSP_ADVANCED_TUNING: + case MSP_PID_ADVANCED: headSerialReply(3 * 2 + 2); serialize16(currentProfile->pidProfile.rollPitchItermIgnoreRate); serialize16(currentProfile->pidProfile.yawItermIgnoreRate); @@ -1266,13 +1267,6 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentProfile->pidProfile.deltaMethod); serialize8(masterConfig.batteryConfig.vbatPidCompensation); break; - case MSP_SPECIAL_PARAMETERS: - headSerialReply(1 + 2 + 1 + 2); - serialize8(currentControlRateProfile->rcYawRate8); - serialize16(masterConfig.rxConfig.airModeActivateThreshold); - serialize8(masterConfig.rxConfig.rcSmoothInterval); - serialize16(masterConfig.escAndServoConfig.escDesyncProtection); - break; case MSP_SENSOR_CONFIG: headSerialReply(3); serialize8(masterConfig.acc_hardware); @@ -1407,6 +1401,9 @@ static bool processInCommand(void) if (currentPort->dataSize >= 11) { currentControlRateProfile->rcYawExpo8 = read8(); } + if (currentPort->dataSize >= 12) { + currentControlRateProfile->rcYawRate8 = read8(); + } } else { headSerialError(0); } @@ -1792,7 +1789,7 @@ static bool processInCommand(void) break; #endif - case MSP_SET_PID_ADVANCED_CONFIG : + case MSP_SET_ADVANCED_CONFIG : masterConfig.gyro_sync_denom = read8(); masterConfig.pid_process_denom = read8(); masterConfig.use_unsyncedPwm = read8(); @@ -1804,19 +1801,13 @@ static bool processInCommand(void) currentProfile->pidProfile.dterm_lpf_hz = read16(); currentProfile->pidProfile.yaw_lpf_hz = read16(); break; - case MSP_SET_ADVANCED_TUNING: + case MSP_SET_PID_ADVANCED: currentProfile->pidProfile.rollPitchItermIgnoreRate = read16(); currentProfile->pidProfile.yawItermIgnoreRate = read16(); currentProfile->pidProfile.yaw_p_limit = read16(); currentProfile->pidProfile.deltaMethod = read8(); masterConfig.batteryConfig.vbatPidCompensation = read8(); break; - case MSP_SET_SPECIAL_PARAMETERS: - currentControlRateProfile->rcYawRate8 = read8(); - masterConfig.rxConfig.airModeActivateThreshold = read16(); - masterConfig.rxConfig.rcSmoothInterval = read8(); - masterConfig.escAndServoConfig.escDesyncProtection = read16(); - break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); masterConfig.baro_hardware = read8(); diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index ca165ef8ec..8946bb1c66 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -185,22 +185,19 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion // Betaflight Additional Commands -#define MSP_PID_ADVANCED_CONFIG 90 -#define MSP_SET_PID_ADVANCED_CONFIG 91 +#define MSP_ADVANCED_CONFIG 90 +#define MSP_SET_ADVANCED_CONFIG 91 #define MSP_FILTER_CONFIG 92 #define MSP_SET_FILTER_CONFIG 93 -#define MSP_ADVANCED_TUNING 94 -#define MSP_SET_ADVANCED_TUNING 95 +#define MSP_PID_ADVANCED 94 +#define MSP_SET_PID_ADVANCED 95 #define MSP_SENSOR_CONFIG 96 #define MSP_SET_SENSOR_CONFIG 97 -#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility -#define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility -// // Multwii original MSP commands // From b267a3a4722c9aaea5c0285ddd4a40fd71983981 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 27 Jul 2016 09:51:47 +0200 Subject: [PATCH 150/456] AlienFlight F1 target cleanup --- src/main/target/ALIENFLIGHTF1/config.c | 6 -- src/main/target/ALIENFLIGHTF1/target.h | 84 +------------------------ src/main/target/ALIENFLIGHTF1/target.mk | 16 +---- 3 files changed, 4 insertions(+), 102 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index b5bea8104a..083affa5b4 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -29,19 +29,16 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" -#include "drivers/compass.h" #include "drivers/system.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/serial.h" #include "drivers/pwm_output.h" -#include "drivers/max7456.h" #include "drivers/io.h" #include "drivers/pwm_mapping.h" #include "sensors/sensors.h" #include "sensors/gyro.h" -#include "sensors/compass.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/boardalignment.h" @@ -54,9 +51,6 @@ #include "io/rc_controls.h" #include "io/rc_curves.h" #include "io/ledstrip.h" -#include "io/gps.h" -#include "io/osd.h" -#include "io/vtx.h" #include "rx/rx.h" diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 41adcaf6f8..77750ca137 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -24,113 +24,33 @@ #define BEEPER PA12 -#define BARO_XCLR_PIN PC13 -#define BARO_EOC_PIN PC14 - -#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO -#define INVERTER_USART USART2 - #define USE_EXTI #define MAG_INT_EXTI PC14 -#define EXTI_CALLBACK_HANDLER_COUNT 3 // MPU data ready, MAG data ready, BMP085 EOC +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL -//#define DEBUG_MAG_DATA_READY_INTERRUPT -#define USE_MAG_DATA_READY_SIGNAL - -// SPI2 -// PB15 28 SPI2_MOSI -// PB14 27 SPI2_MISO -// PB13 26 SPI2_SCK -// PB12 25 SPI2_NSS - -#define USE_SPI -#define USE_SPI_DEVICE_2 - -#define NAZE_SPI_INSTANCE SPI2 -#define NAZE_SPI_CS_GPIO GPIOB -#define NAZE_SPI_CS_PIN PB12 -#define NAZE_CS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOB - -// We either have this 16mbit flash chip on SPI or the MPU6500 acc/gyro depending on board revision: -#define M25P16_CS_GPIO NAZE_SPI_CS_GPIO -#define M25P16_CS_PIN NAZE_SPI_CS_PIN -#define M25P16_SPI_INSTANCE NAZE_SPI_INSTANCE - -#define MPU6500_CS_GPIO_CLK_PERIPHERAL NAZE_CS_GPIO_CLK_PERIPHERAL -#define MPU6500_CS_GPIO NAZE_SPI_CS_GPIO -#define MPU6500_CS_PIN NAZE_SPI_CS_PIN -#define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE - -#define USE_FLASHFS -#define USE_FLASH_M25P16 #define GYRO -#define USE_GYRO_MPU3050 #define USE_GYRO_MPU6050 -#define USE_GYRO_MPU6500 -#define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU3050_ALIGN CW0_DEG #define GYRO_MPU6050_ALIGN CW0_DEG -#define GYRO_MPU6500_ALIGN CW0_DEG #define ACC -#define USE_ACC_ADXL345 -#define USE_ACC_BMA280 -#define USE_ACC_MMA8452 #define USE_ACC_MPU6050 -#define USE_ACC_MPU6500 -#define USE_ACC_SPI_MPU6500 -#define ACC_ADXL345_ALIGN CW270_DEG #define ACC_MPU6050_ALIGN CW0_DEG -#define ACC_MMA8452_ALIGN CW90_DEG -#define ACC_BMA280_ALIGN CW0_DEG -#define ACC_MPU6500_ALIGN CW0_DEG - -#define BARO -#define USE_BARO_MS5611 -#define USE_BARO_BMP085 -#define USE_BARO_BMP280 - -#define MAG -#define USE_MAG_HMC5883 -#define MAG_HMC5883_ALIGN CW180_DEG - -#define SONAR -#define SONAR_TRIGGER_PIN PB0 -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN_PWM PB8 -#define SONAR_ECHO_PIN_PWM PB9 - -#define DISPLAY #define USE_UART1 #define USE_UART2 #define USE_UART3 -#define USE_SOFTSERIAL1 -#define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 3 -#define SOFTSERIAL_1_TIMER TIM3 -#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 -#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 -#define SOFTSERIAL_2_TIMER TIM3 -#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 -#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 - -// USART3 only on NAZE32_SP - Flex Port #define UART3_RX_PIN PB11 #define UART3_TX_PIN PB10 #define USE_I2C #define I2C_DEVICE (I2CDEV_2) -// #define SOFT_I2C // enable to test software i2c -// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) -// #define SOFT_I2C_PB67 - #define USE_ADC #define CURRENT_METER_ADC_PIN PB1 #define VBAT_ADC_PIN PA4 diff --git a/src/main/target/ALIENFLIGHTF1/target.mk b/src/main/target/ALIENFLIGHTF1/target.mk index e5fbfe8c95..c3a0ba80e5 100644 --- a/src/main/target/ALIENFLIGHTF1/target.mk +++ b/src/main/target/ALIENFLIGHTF1/target.mk @@ -1,20 +1,8 @@ F1_TARGETS += $(TARGET) -FEATURES = ONBOARDFLASH HIGHEND +FEATURES = HIGHEND TARGET_SRC = \ - drivers/accgyro_adxl345.c \ - drivers/accgyro_bma280.c \ - drivers/accgyro_l3g4200d.c \ - drivers/accgyro_mma845x.c \ drivers/accgyro_mpu.c \ - drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu6050.c \ - drivers/accgyro_mpu6500.c \ - drivers/accgyro_spi_mpu6500.c \ - drivers/barometer_bmp085.c \ - drivers/barometer_bmp280.c \ - drivers/barometer_ms5611.c \ - drivers/compass_hmc5883l.c \ drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_stm32f10x.c \ - drivers/sonar_hcsr04.c + drivers/light_ws2811strip_stm32f10x.c From 50ce9c5be7695c0c02ed466136fbd8918643dd28 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 1 Aug 2016 21:52:45 +0200 Subject: [PATCH 151/456] Increase task priority for Telemtry --- src/main/scheduler/scheduler_tasks.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/scheduler/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c index 3699442c96..9d5062ee21 100644 --- a/src/main/scheduler/scheduler_tasks.c +++ b/src/main/scheduler/scheduler_tasks.c @@ -160,7 +160,7 @@ cfTask_t cfTasks[TASK_COUNT] = { .taskName = "TELEMETRY", .taskFunc = taskTelemetry, .desiredPeriod = 1000000 / 250, // 250 Hz - .staticPriority = TASK_PRIORITY_IDLE, + .staticPriority = TASK_PRIORITY_LOW, }, #endif From 2f18f0b8fb0aa3055e8f6a937bdd3e998e1dfd2f Mon Sep 17 00:00:00 2001 From: nathan Date: Tue, 2 Aug 2016 01:24:45 -0700 Subject: [PATCH 152/456] move the mah symbol to an nvram slot before the logo --- src/main/drivers/max7456_symbols.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h index b2e8da72b5..ba798d1d97 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/max7456_symbols.h @@ -178,7 +178,7 @@ // Voltage and amperage #define SYM_VOLT 0x06 #define SYM_AMP 0x9A -#define SYM_MAH 0xA4 +#define SYM_MAH 0x07 #define SYM_WATT 0x57 // Flying Mode From d66c9b3ee6512f57a7759820dd180c2f56139e2e Mon Sep 17 00:00:00 2001 From: nathan Date: Tue, 2 Aug 2016 01:25:06 -0700 Subject: [PATCH 153/456] set the default position on craft name so that the preview is readable in the configurator --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c3a00bd68d..abd862dd13 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -848,7 +848,7 @@ void resetOsdConfig(void) masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS] = -1; masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW] = -23; masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] = -18; - masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME] = -77; + masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME] = 1; } #endif From 1c997abaaff77c1d8aa985d00f4d5ae479fd721c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 2 Aug 2016 15:11:35 +0100 Subject: [PATCH 154/456] Moved function declarations out of main. Tidied drivers. --- src/main/common/printf.h | 2 + src/main/drivers/accgyro_lsm303dlhc.c | 5 ++- src/main/drivers/bus_i2c_stm32f30x.c | 40 +++++++++---------- src/main/drivers/compass_ak8975.c | 10 +++-- src/main/drivers/compass_ak8975.h | 2 +- src/main/drivers/compass_hmc5883l.c | 4 ++ src/main/drivers/inverter.h | 2 - src/main/drivers/io.c | 10 ++--- src/main/drivers/light_ws2811strip.c | 4 +- .../drivers/light_ws2811strip_stm32f10x.c | 4 +- .../drivers/light_ws2811strip_stm32f30x.c | 4 +- .../drivers/light_ws2811strip_stm32f4xx.c | 4 +- src/main/drivers/pwm_mapping.c | 6 +-- src/main/drivers/pwm_mapping.h | 4 +- src/main/drivers/pwm_output.c | 23 +++++------ src/main/drivers/serial_softserial.c | 16 ++++---- src/main/drivers/system.h | 2 - src/main/flight/failsafe.h | 2 + src/main/flight/imu.h | 1 + src/main/flight/mixer.h | 5 +++ src/main/flight/navigation.h | 2 + src/main/io/display.h | 2 + src/main/io/gps.h | 3 +- src/main/io/serial_cli.h | 2 + src/main/main.c | 30 +++----------- src/main/rx/rx.h | 5 ++- src/main/rx/spektrum.h | 3 ++ src/main/sensors/battery.h | 4 +- src/main/sensors/initialisation.c | 2 +- src/main/sensors/sonar.h | 4 ++ src/main/telemetry/telemetry.h | 1 + 31 files changed, 112 insertions(+), 96 deletions(-) diff --git a/src/main/common/printf.h b/src/main/common/printf.h index dff4efdd2d..ac5493590b 100644 --- a/src/main/common/printf.h +++ b/src/main/common/printf.h @@ -120,5 +120,7 @@ int tfp_format(void *putp, void (*putf) (void *, char), const char *fmt, va_list #define sprintf tfp_sprintf void setPrintfSerialPort(serialPort_t *serialPort); +void printfSupportInit(void); + #endif diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c index c3ea4968e7..80a270b9f1 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_lsm303dlhc.c @@ -19,6 +19,9 @@ #include #include "platform.h" + +#ifdef USE_ACC_LSM303DLHC + #include "debug.h" #include "common/maths.h" @@ -167,4 +170,4 @@ bool lsm303dlhcAccDetect(acc_t *acc) acc->read = lsm303dlhcAccRead; return true; } - +#endif diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 524df1bc86..88b267f111 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -132,10 +132,10 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) } } - /* Configure slave address, nbytes, reload, end mode and start or stop generation */ + /* Configure slave address, nbytes, reload, end mode and start or stop generation */ I2C_TransferHandling(I2Cx, addr_, 1, I2C_Reload_Mode, I2C_Generate_Start_Write); - /* Wait until TXIS flag is set */ + /* Wait until TXIS flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { if ((i2cTimeout--) == 0) { @@ -143,10 +143,10 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) } } - /* Send Register address */ + /* Send Register address */ I2C_SendData(I2Cx, (uint8_t) reg); - /* Wait until TCR flag is set */ + /* Wait until TCR flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TCR) == RESET) { @@ -155,10 +155,10 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) } } - /* Configure slave address, nbytes, reload, end mode and start or stop generation */ + /* Configure slave address, nbytes, reload, end mode and start or stop generation */ I2C_TransferHandling(I2Cx, addr_, 1, I2C_AutoEnd_Mode, I2C_No_StartStop); - /* Wait until TXIS flag is set */ + /* Wait until TXIS flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { if ((i2cTimeout--) == 0) { @@ -166,10 +166,10 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) } } - /* Write data to TXDR */ + /* Write data to TXDR */ I2C_SendData(I2Cx, data); - /* Wait until STOPF flag is set */ + /* Wait until STOPF flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; while (I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) { if ((i2cTimeout--) == 0) { @@ -177,7 +177,7 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data) } } - /* Clear STOPF flag */ + /* Clear STOPF flag */ I2C_ClearFlag(I2Cx, I2C_ICR_STOPCF); return true; @@ -198,10 +198,10 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* } } - /* Configure slave address, nbytes, reload, end mode and start or stop generation */ + /* Configure slave address, nbytes, reload, end mode and start or stop generation */ I2C_TransferHandling(I2Cx, addr_, 1, I2C_SoftEnd_Mode, I2C_Generate_Start_Write); - /* Wait until TXIS flag is set */ + /* Wait until TXIS flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) { if ((i2cTimeout--) == 0) { @@ -209,10 +209,10 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* } } - /* Send Register address */ + /* Send Register address */ I2C_SendData(I2Cx, (uint8_t) reg); - /* Wait until TC flag is set */ + /* Wait until TC flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; while (I2C_GetFlagStatus(I2Cx, I2C_ISR_TC) == RESET) { if ((i2cTimeout--) == 0) { @@ -220,10 +220,10 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* } } - /* Configure slave address, nbytes, reload, end mode and start or stop generation */ + /* Configure slave address, nbytes, reload, end mode and start or stop generation */ I2C_TransferHandling(I2Cx, addr_, len, I2C_AutoEnd_Mode, I2C_Generate_Start_Read); - /* Wait until all data are received */ + /* Wait until all data are received */ while (len) { /* Wait until RXNE flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; @@ -233,16 +233,16 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* } } - /* Read data from RXDR */ + /* Read data from RXDR */ *buf = I2C_ReceiveData(I2Cx); /* Point to the next location where the byte read will be saved */ buf++; - /* Decrement the read bytes counter */ + /* Decrement the read bytes counter */ len--; } - /* Wait until STOPF flag is set */ + /* Wait until STOPF flag is set */ i2cTimeout = I2C_LONG_TIMEOUT; while (I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) { if ((i2cTimeout--) == 0) { @@ -250,10 +250,10 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* } } - /* Clear STOPF flag */ + /* Clear STOPF flag */ I2C_ClearFlag(I2Cx, I2C_ICR_STOPCF); - /* If all operations OK */ + /* If all operations OK */ return true; } diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index c35db1d1f6..0d648ecc58 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -20,10 +20,12 @@ #include -#include "build_config.h" - #include "platform.h" +#ifdef USE_MAG_AK8975 + +#include "build_config.h" + #include "common/axis.h" #include "common/maths.h" @@ -31,7 +33,6 @@ #include "gpio.h" #include "bus_i2c.h" -#include "sensors/boardalignment.h" #include "sensors/sensors.h" #include "sensor.h" @@ -59,7 +60,7 @@ #define AK8975_MAG_REG_CNTL 0x0a #define AK8975_MAG_REG_ASCT 0x0c // self test -bool ak8975detect(mag_t *mag) +bool ak8975Detect(mag_t *mag) { bool ack = false; uint8_t sig = 0; @@ -155,3 +156,4 @@ bool ak8975Read(int16_t *magData) ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again return true; } +#endif diff --git a/src/main/drivers/compass_ak8975.h b/src/main/drivers/compass_ak8975.h index f80e646130..493114cb3d 100644 --- a/src/main/drivers/compass_ak8975.h +++ b/src/main/drivers/compass_ak8975.h @@ -17,6 +17,6 @@ #pragma once -bool ak8975detect(mag_t *mag); +bool ak8975Detect(mag_t *mag); void ak8975Init(void); bool ak8975Read(int16_t *magData); diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 68c404d804..3709cc510e 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -21,6 +21,9 @@ #include #include "platform.h" + +#ifdef USE_MAG_HMC5883 + #include "debug.h" #include "common/axis.h" @@ -271,3 +274,4 @@ bool hmc5883lRead(int16_t *magData) return true; } +#endif diff --git a/src/main/drivers/inverter.h b/src/main/drivers/inverter.h index 6ec27ef83c..4b9c3fe274 100644 --- a/src/main/drivers/inverter.h +++ b/src/main/drivers/inverter.h @@ -28,5 +28,3 @@ void inverterSet(bool on); void initInverter(void); - - diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index 980168abba..0ba9d9169b 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -54,14 +54,14 @@ const struct ioPortDef_s ioPortDefs[] = { # endif const char * const ownerNames[OWNER_TOTAL_COUNT] = { - "FREE", "PWM", "PPM", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "DEBUG", "TIMER", - "SONAR", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD", - "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER" + "FREE", "PWM", "PPM", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "DEBUG", "TIMER", + "SONAR", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD", + "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER" }; const char * const resourceNames[RESOURCE_TOTAL_COUNT] = { - "", // NONE - "IN", "OUT", "IN / OUT", "TIMER","UART TX","UART RX","UART TX/RX","EXTI","SCL", + "", // NONE + "IN", "OUT", "IN / OUT", "TIMER","UART TX","UART RX","UART TX/RX","EXTI","SCL", "SDA", "SCK","MOSI","MISO","CS","BATTERY","RSSI","EXT","CURRENT" }; diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 05f0998927..6c8f2bb18a 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -30,6 +30,8 @@ #include +#ifdef LED_STRIP + #include "build_config.h" #include "common/color.h" @@ -37,8 +39,6 @@ #include "drivers/dma.h" #include "drivers/light_ws2811strip.h" -#ifdef LED_STRIP - uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; volatile uint8_t ws2811LedDataTransferInProgress = 0; diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index b4a9b54268..4b588a0c9f 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -20,6 +20,8 @@ #include +#ifdef LED_STRIP + #include "common/color.h" #include "drivers/light_ws2811strip.h" #include "nvic.h" @@ -27,8 +29,6 @@ #include "dma.h" #include "timer.h" -#ifdef LED_STRIP - static IO_t ws2811IO = IO_NONE; bool ws2811Initialised = false; diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 722dd9a2e4..8379188f99 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -20,6 +20,8 @@ #include +#ifdef LED_STRIP + #include "io.h" #include "nvic.h" @@ -29,8 +31,6 @@ #include "rcc.h" #include "timer.h" -#ifdef LED_STRIP - #ifndef WS2811_PIN #define WS2811_PIN PB8 // TIM16_CH1 #define WS2811_TIMER TIM16 diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index e8676e2cdf..63bcd9cee6 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -20,6 +20,8 @@ #include "platform.h" +#ifdef LED_STRIP + #include "common/color.h" #include "light_ws2811strip.h" #include "nvic.h" @@ -29,8 +31,6 @@ #include "rcc.h" #include "timer.h" -#ifdef LED_STRIP - #if !defined(WS2811_PIN) #define WS2811_PIN PA0 #define WS2811_TIMER TIM5 diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 3cec1ab3ef..3f88e3a472 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -89,7 +89,6 @@ pwmOutputConfiguration_t *pwmGetOutputConfiguration(void) pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) { - int i = 0; const uint16_t *setup; #ifndef SKIP_RX_PWM_PPM @@ -99,6 +98,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) memset(&pwmOutputConfiguration, 0, sizeof(pwmOutputConfiguration)); // this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ] + int i = 0; if (init->airplane) i = 2; // switch to air hardware config if (init->usePPM || init->useSerialRx) @@ -177,8 +177,8 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) #ifdef SONAR if (init->useSonar && ( - timerHardwarePtr->tag == init->sonarConfig.triggerTag || - timerHardwarePtr->tag == init->sonarConfig.echoTag + timerHardwarePtr->tag == init->sonarIOConfig.triggerTag || + timerHardwarePtr->tag == init->sonarIOConfig.echoTag )) { continue; } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 5a3f402e97..29a5d02f15 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -68,9 +68,10 @@ typedef struct drv_pwm_config_s { uint16_t motorPwmRate; uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), // some higher value (used by 3d mode), or 0, for brushed pwm drivers. - sonarIOConfig_t sonarConfig; + sonarIOConfig_t sonarIOConfig; } drv_pwm_config_t; + enum { MAP_TO_PPM_INPUT = 1, MAP_TO_PWM_INPUT, @@ -136,4 +137,5 @@ extern const uint16_t airPPM_BP6[]; extern const uint16_t airPWM_BP6[]; #endif +pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init); pwmOutputConfiguration_t *pwmGetOutputConfiguration(void); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 44504f6801..f470137a7a 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -71,12 +71,11 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 TIM_OCStructInit(&TIM_OCInitStructure); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; if (output & TIMER_OUTPUT_N_CHANNEL) { - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; - } - else { - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable; + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; + } else { + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; } TIM_OCInitStructure.TIM_Pulse = value; TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_High : TIM_OCPolarity_Low; @@ -175,7 +174,7 @@ void pwmWriteMotor(uint8_t index, uint16_t value) void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { - for(int index = 0; index < motorCount; index++){ + for (int index = 0; index < motorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows *motors[index]->ccr = 0; } @@ -197,13 +196,13 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) bool overflowed = false; // If we have not already overflowed this timer for (int j = 0; j < index; j++) { - if (motors[j]->tim == motors[index]->tim) { - overflowed = true; - break; - } + if (motors[j]->tim == motors[index]->tim) { + overflowed = true; + break; + } } if (!overflowed) { - timerForceOverflow(motors[index]->tim); + timerForceOverflow(motors[index]->tim); } // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 51cdeb2936..d8fafdf78e 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -470,14 +470,16 @@ bool isSoftSerialTransmitBufferEmpty(serialPort_t *instance) const struct serialPortVTable softSerialVTable[] = { { - softSerialWriteByte, - softSerialRxBytesWaiting, - softSerialTxBytesFree, - softSerialReadByte, - softSerialSetBaudRate, - isSoftSerialTransmitBufferEmpty, - softSerialSetMode, + .serialWrite = softSerialWriteByte, + .serialTotalRxWaiting = softSerialRxBytesWaiting, + .serialTotalTxFree = softSerialTxBytesFree, + .serialRead = softSerialReadByte, + .serialSetBaudRate = softSerialSetBaudRate, + .isSerialTransmitBufferEmpty = isSoftSerialTransmitBufferEmpty, + .setMode = softSerialSetMode, .writeBuf = NULL, + .beginWrite = NULL, + .endWrite = NULL } }; diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index c6d58aa744..105a584c44 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -67,5 +67,3 @@ void unregisterExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn); extern uint32_t cachedRccCsrValue; - - diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 501c168456..d166e1c360 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -70,6 +70,8 @@ typedef struct failsafeState_s { failsafeRxLinkState_e rxLinkState; } failsafeState_t; +struct rxConfig_s; +void failsafeInit(struct rxConfig_s *intialRxConfig, uint16_t deadband3d_throttle); void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse); void failsafeStartMonitoring(void); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 6dc0a1f6b7..94b361fcf6 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -88,3 +88,4 @@ int16_t imuCalculateHeading(t_fp_vector *vec); void imuResetAccelerationSum(void); void imuUpdateAcc(rollAndPitchTrims_t *accelerometerTrims); +void imuInit(void); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index b76aafa1bd..bf1293e732 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -206,10 +206,15 @@ void mixerUseConfigs( void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); #ifdef USE_SERVOS +void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixer_t *customServoMixers); void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); void loadCustomServoMixer(void); int servoDirection(int servoIndex, int fromChannel); +#else +void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); #endif +struct pwmOutputConfiguration_s; +void mixerUsePWMOutputConfiguration(struct pwmOutputConfiguration_s *pwmOutputConfiguration, bool use_unsyncedPwm); void mixerResetDisarmedMotors(void); void mixTable(void *pidProfilePtr); void syncMotors(bool enabled); diff --git a/src/main/flight/navigation.h b/src/main/flight/navigation.h index 673cf5cfbf..8adb9918d4 100644 --- a/src/main/flight/navigation.h +++ b/src/main/flight/navigation.h @@ -46,6 +46,8 @@ extern int16_t GPS_directionToHome; // direction to home or hol point in extern navigationMode_e nav_mode; // Navigation mode +struct pidProfile_s; +void navigationInit(gpsProfile_t *initialGpsProfile, struct pidProfile_s *pidProfile); void GPS_reset_home_position(void); void GPS_reset_nav(void); void GPS_set_next_wp(int32_t* lat, int32_t* lon); diff --git a/src/main/io/display.h b/src/main/io/display.h index 8149a35ade..5582dc5126 100644 --- a/src/main/io/display.h +++ b/src/main/io/display.h @@ -35,6 +35,8 @@ typedef enum { #endif } pageId_e; +struct rxConfig_s; +void displayInit(struct rxConfig_s *intialRxConfig); void updateDisplay(void); void displayShowFixedPage(pageId_e pageId); diff --git a/src/main/io/gps.h b/src/main/io/gps.h index cdb9f66f6f..efa0ec32c2 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -117,7 +117,8 @@ extern uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Str #define GPS_DBHZ_MIN 0 #define GPS_DBHZ_MAX 55 - +struct serialConfig_s; +void gpsInit(struct serialConfig_s *serialConfig, gpsConfig_t *initialGpsConfig); void gpsThread(void); bool gpsNewFrame(uint8_t c); void updateGpsIndicator(uint32_t currentTime); diff --git a/src/main/io/serial_cli.h b/src/main/io/serial_cli.h index d54b04cecf..f2ec866a6c 100644 --- a/src/main/io/serial_cli.h +++ b/src/main/io/serial_cli.h @@ -20,6 +20,8 @@ extern uint8_t cliMode; +struct serialConfig_s; +void cliInit(struct serialConfig_s *serialConfig); void cliProcess(void); bool cliIsActiveOnPort(serialPort_t *serialPort); diff --git a/src/main/main.c b/src/main/main.c index 79e415c1c2..6597ea7673 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -25,6 +25,7 @@ #include "common/axis.h" #include "common/color.h" #include "common/maths.h" +#include "common/printf.h" #include "drivers/nvic.h" @@ -61,6 +62,7 @@ #endif #include "rx/rx.h" +#include "rx/spektrum.h" #include "io/beeper.h" #include "io/serial.h" @@ -72,6 +74,8 @@ #include "io/ledstrip.h" #include "io/display.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "io/serial_cli.h" +#include "io/serial_msp.h" #include "io/transponder_ir.h" #include "io/osd.h" #include "io/vtx.h" @@ -115,28 +119,6 @@ extern uint8_t motorControlEnable; serialPort_t *loopbackPort; #endif -void printfSupportInit(void); -void timerInit(void); -void telemetryInit(void); -void mspInit(serialConfig_t *serialConfig); -void cliInit(serialConfig_t *serialConfig); -void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle); -pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init); -#ifdef USE_SERVOS -void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixer_t *customServoMixers); -#else -void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); -#endif -void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration, bool use_unsyncedPwm); -void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions); -void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); -void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); -void imuInit(void); -void displayInit(rxConfig_t *intialRxConfig); -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); -void spektrumBind(rxConfig_t *rxConfig); -const sonarHardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor); -void osdInit(void); typedef enum { SYSTEM_STATE_INITIALISING = 0, @@ -270,8 +252,8 @@ void init(void) const sonarHardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType); if (sonarHardware) { pwm_params.useSonar = true; - pwm_params.sonarConfig.triggerTag = sonarHardware->triggerTag; - pwm_params.sonarConfig.echoTag = sonarHardware->echoTag; + pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag; + pwm_params.sonarIOConfig.echoTag = sonarHardware->echoTag; } } #endif diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 60625f608c..1200e5e8f5 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -143,10 +143,11 @@ typedef struct rxRuntimeConfig_s { extern rxRuntimeConfig_t rxRuntimeConfig; -void useRxConfig(rxConfig_t *rxConfigToUse); - typedef uint16_t (*rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data +struct modeActivationCondition_s; +void rxInit(rxConfig_t *rxConfig, struct modeActivationCondition_s *modeActivationConditions); +void useRxConfig(rxConfig_t *rxConfigToUse); void updateRx(uint32_t currentTime); bool rxIsReceivingSignal(void); bool rxAreFlightChannelsValid(void); diff --git a/src/main/rx/spektrum.h b/src/main/rx/spektrum.h index 82989ea55a..5481bdebc0 100644 --- a/src/main/rx/spektrum.h +++ b/src/main/rx/spektrum.h @@ -21,3 +21,6 @@ #define SPEKTRUM_SAT_BIND_MAX 10 uint8_t spektrumFrameStatus(void); +struct rxConfig_s; +void spektrumBind(struct rxConfig_s *rxConfig); + diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 9d6afab5dc..f459a3bcc4 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -17,7 +17,6 @@ #pragma once -#include "rx/rx.h" #include "common/maths.h" #ifndef VBAT_SCALE_DEFAULT @@ -75,7 +74,8 @@ void updateBattery(void); void batteryInit(batteryConfig_t *initialBatteryConfig); batteryConfig_t *batteryConfig; -void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle); +struct rxConfig_s; +void updateCurrentMeter(int32_t lastUpdateAt, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); int32_t currentMeterToCentiamps(uint16_t src); fix12_t calculateVbatPidCompensation(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 38d81a1f4f..bb46059ac1 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -537,7 +537,7 @@ retry: case MAG_AK8975: #ifdef USE_MAG_AK8975 - if (ak8975detect(&mag)) { + if (ak8975Detect(&mag)) { #ifdef MAG_AK8975_ALIGN magAlign = MAG_AK8975_ALIGN; #endif diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index be49afb7b8..0c0849e32b 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -17,12 +17,16 @@ #pragma once +#include "sensors/battery.h" + #define SONAR_OUT_OF_RANGE (-1) extern int16_t sonarMaxRangeCm; extern int16_t sonarCfAltCm; extern int16_t sonarMaxAltWithTiltCm; +struct sonarHardware_s; +const struct sonarHardware_s *sonarGetHardwareConfiguration(currentSensor_e currentSensor); void sonarInit(void); void sonarUpdate(void); int32_t sonarRead(void); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index b4585e5fce..b61aa68bd4 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -46,6 +46,7 @@ typedef struct telemetryConfig_s { uint8_t hottAlarmSoundInterval; } telemetryConfig_t; +void telemetryInit(void); bool telemetryCheckRxPortShared(serialPortConfig_t *portConfig); extern serialPort_t *telemetrySharedPort; From 918e4e8f276f7314659b41d7d0d1a8ebe04a54c4 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 1 Aug 2016 10:45:13 +0100 Subject: [PATCH 155/456] Added Micro SciSky target --- src/main/target/MICROSCISKY/target.c | 106 ++++++++++++++++++++++++++ src/main/target/MICROSCISKY/target.h | 100 ++++++++++++++++++++++++ src/main/target/MICROSCISKY/target.mk | 12 +++ 3 files changed, 218 insertions(+) create mode 100644 src/main/target/MICROSCISKY/target.c create mode 100644 src/main/target/MICROSCISKY/target.h create mode 100644 src/main/target/MICROSCISKY/target.mk diff --git a/src/main/target/MICROSCISKY/target.c b/src/main/target/MICROSCISKY/target.c new file mode 100644 index 0000000000..7116b53551 --- /dev/null +++ b/src/main/target/MICROSCISKY/target.c @@ -0,0 +1,106 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD }, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD }, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_IPD }, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD }, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_IPD }, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_IPD }, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_IPD }, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_IPD }, // PWM8 - RC8 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_IPD }, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_IPD }, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_IPD }, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_IPD }, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_IPD } // PWM14 - OUT6 +}; + diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h new file mode 100644 index 0000000000..80d4f04856 --- /dev/null +++ b/src/main/target/MICROSCISKY/target.h @@ -0,0 +1,100 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "MSKY" // Micro sciSKY + +#define LED0 PB3 +#define LED1 PB4 + +#define BARO_XCLR_PIN PC13 +#define BARO_EOC_PIN PC14 + +#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO +#define INVERTER_USART USART2 + +#define USE_EXTI +#define MAG_INT_EXTI PC14 +#define EXTI_CALLBACK_HANDLER_COUNT 3 // MPU data ready, MAG data ready, BMP085 EOC +//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define USE_MPU_DATA_READY_SIGNAL +//#define DEBUG_MAG_DATA_READY_INTERRUPT +#define USE_MAG_DATA_READY_SIGNAL + +// SPI2 +// PB15 28 SPI2_MOSI +// PB14 27 SPI2_MISO +// PB13 26 SPI2_SCK +// PB12 25 SPI2_NSS + +#define USE_SPI +#define USE_SPI_DEVICE_2 + +#define GYRO +#define USE_GYRO_MPU6050 +#define GYRO_MPU6050_ALIGN CW0_DEG + +#define ACC +#define USE_ACC_MPU6050 +#define ACC_MPU6050_ALIGN CW0_DEG + +#define BARO +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_BMP280 + +#define MAG +#define USE_MAG_HMC5883 +#define MAG_HMC5883_ALIGN CW180_DEG + +#define USE_UART1 +#define USE_UART2 +#define SERIAL_PORT_COUNT 2 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_2) + +#define LED_STRIP +#define WS2811_TIMER TIM3 +#define WS2811_PIN PA6 +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER + +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PIN PA3 + +#define BRUSHED_MOTORS +#define DEFAULT_FEATURES FEATURE_MOTOR_STOP +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SPEKTRUM1024 +#define SERIALRX_UART SERIAL_PORT_USART2 +#define RX_CHANNELS_TAER + +#undef GPS +#undef USE_SERVOS +#define USE_QUAD_MIXER_ONLY +#define DISPLAY + + +// IO - assuming all IOs on 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) + +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/MICROSCISKY/target.mk b/src/main/target/MICROSCISKY/target.mk new file mode 100644 index 0000000000..02299eedd0 --- /dev/null +++ b/src/main/target/MICROSCISKY/target.mk @@ -0,0 +1,12 @@ +F1_TARGETS += $(TARGET) +FEATURES = HIGHEND + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6050.c \ + drivers/barometer_bmp085.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f10x.c From 2a645f1058864a1a18677e8c63f3b329d6b89187 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 2 Aug 2016 16:23:55 +0100 Subject: [PATCH 156/456] Added RX_CHANNELS_TAER build flag --- src/main/config/config.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/config/config.c b/src/main/config/config.c index 4e16b8c755..20792f6246 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -591,7 +591,11 @@ static void resetConf(void) #endif // Radio +#ifdef RX_CHANNELS_TAER + parseRcChannels("TAER1234", &masterConfig.rxConfig); +#else parseRcChannels("AETR1234", &masterConfig.rxConfig); +#endif resetRcControlsConfig(&masterConfig.rcControlsConfig); From 5c3e65782dec80332edd442cd677f6fee84445b3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 2 Aug 2016 16:48:22 +0100 Subject: [PATCH 157/456] Tidied ALIENFLIGHTF3 ledInit --- src/main/main.c | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index 6597ea7673..7ec478feae 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -159,11 +159,7 @@ void init(void) latchActiveFeatures(); #ifdef ALIENFLIGHTF3 - if (hardwareRevision == AFF3_REV_1) { - ledInit(false); - } else { - ledInit(true); - } + ledInit(hardwareRevision == AFF3_REV_1 ? false : true); #else ledInit(false); #endif From fee5aa7e7cc6bd3508ff917e4e579416cdb3a47a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 2 Aug 2016 17:58:33 +0100 Subject: [PATCH 158/456] Fixed ALIENFLIGHT targets to use RX_CHANNELS_TAER build flag --- src/main/target/ALIENFLIGHTF1/config.c | 1 - src/main/target/ALIENFLIGHTF1/target.h | 1 + src/main/target/ALIENFLIGHTF3/config.c | 1 - src/main/target/ALIENFLIGHTF3/target.h | 1 + src/main/target/ALIENFLIGHTF4/config.c | 1 - src/main/target/ALIENFLIGHTF4/target.h | 1 + 6 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index 083affa5b4..7dd16e808f 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -78,7 +78,6 @@ void targetConfiguration(void) masterConfig.motor_pwm_rate = 32000; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; - parseRcChannels("TAER1234", &masterConfig.rxConfig); masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 77750ca137..6d945008f7 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -78,6 +78,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 #define SERIALRX_UART SERIAL_PORT_USART2 +#define RX_CHANNELS_TAER #define HARDWARE_BIND_PLUG // Hardware bind plug at PB5 (Pin 41) diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 271c9292c4..be0a9bfea3 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -92,7 +92,6 @@ void targetConfiguration(void) { currentProfile->pidProfile.P8[PITCH] = 90; currentProfile->pidProfile.I8[PITCH] = 44; currentProfile->pidProfile.D8[PITCH] = 60; - parseRcChannels("TAER1234", &masterConfig.rxConfig); masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index ce9704fdf3..144cc3074d 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -120,6 +120,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 #define SERIALRX_UART SERIAL_PORT_USART3 +#define RX_CHANNELS_TAER #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 56086358c1..ecce870a48 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -92,7 +92,6 @@ void targetConfiguration(void) { currentProfile->pidProfile.P8[PITCH] = 90; currentProfile->pidProfile.I8[PITCH] = 44; currentProfile->pidProfile.D8[PITCH] = 60; - parseRcChannels("TAER1234", &masterConfig.rxConfig); masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 3469c0c113..a6fc694e6a 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -165,6 +165,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 #define SERIALRX_UART SERIAL_PORT_USART3 +#define RX_CHANNELS_TAER #define USE_SERIAL_4WAY_BLHELI_INTERFACE From 571b9ba99187c7bef5b5bec21bdd41fe420f84e4 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 2 Aug 2016 21:01:34 +0200 Subject: [PATCH 159/456] Add CPU load in MSP_STATUS_EX --- src/main/io/serial_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index f59a5e2882..e2157be0bf 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -740,7 +740,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_STATUS_EX: - headSerialReply(12); + headSerialReply(13); serialize16(cycleTime); #ifdef USE_I2C serialize16(i2cGetErrorCounter()); @@ -750,7 +750,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); serialize32(packFlightModeFlags()); serialize8(masterConfig.current_profile_index); - //serialize16(averageSystemLoadPercent); + serialize16(constrain(averageSystemLoadPercent, 0, 100)); break; case MSP_STATUS: From 47c7409ec477f40341dbdb5a3c91527a593ca44e Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Tue, 2 Aug 2016 23:07:59 +0200 Subject: [PATCH 160/456] MSP hack when gyro_lpf != 0 --- src/main/io/serial_msp.c | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index ee9130f0ac..9401a96287 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1239,9 +1239,14 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_ADVANCED_CONFIG : headSerialReply(6); - serialize8(masterConfig.gyro_sync_denom); - serialize8(masterConfig.pid_process_denom); - serialize8(masterConfig.use_unsyncedPwm); + if (masterConfig.gyro_lpf == 0) { + serialize8(masterConfig.gyro_sync_denom); + serialize8(masterConfig.pid_process_denom); + } else { + serialize8(8); // If gyro_lpf != OFF then looptime is set to 1000 + serialize8(1); + } + serialize8(masterConfig.use_unsyncedPwm); serialize8(masterConfig.motor_pwm_protocol); serialize16(masterConfig.motor_pwm_rate); break; From 930a9b9b4e22fc19677e7fe214bc7b66d4dc2b1c Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Tue, 2 Aug 2016 23:13:13 +0200 Subject: [PATCH 161/456] Additional code style cleanup --- src/main/io/serial_msp.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 9401a96287..9a53b56e43 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1240,13 +1240,13 @@ static bool processOutCommand(uint8_t cmdMSP) case MSP_ADVANCED_CONFIG : headSerialReply(6); if (masterConfig.gyro_lpf == 0) { - serialize8(masterConfig.gyro_sync_denom); - serialize8(masterConfig.pid_process_denom); + serialize8(masterConfig.gyro_sync_denom); + serialize8(masterConfig.pid_process_denom); } else { - serialize8(8); // If gyro_lpf != OFF then looptime is set to 1000 - serialize8(1); + serialize8(8); // If gyro_lpf != OFF then looptime is set to 1000 + serialize8(1); } - serialize8(masterConfig.use_unsyncedPwm); + serialize8(masterConfig.use_unsyncedPwm); serialize8(masterConfig.motor_pwm_protocol); serialize16(masterConfig.motor_pwm_rate); break; From 3cd6ed42452392bada7b9aab321f6a1afc250b89 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 2 Aug 2016 23:13:59 +0200 Subject: [PATCH 162/456] Fix oneshot timer overflow by @nathansoi --- src/main/drivers/pwm_output.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 9a49661a51..7f928891e8 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -170,18 +170,18 @@ void pwmEnableMotors(void) void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) { - uint8_t index; - TIM_TypeDef *lastTimerPtr = NULL; - - for(index = 0; index < motorCount; index++){ - - // Force the timer to overflow if it's the first motor to output, or if we change timers - if(motors[index]->tim != lastTimerPtr){ - lastTimerPtr = motors[index]->tim; - - timerForceOverflow(motors[index]->tim); + for (int index = 0; index < motorCount; index++) { + bool overflowed = false; + // If we have not already overflowed this timer + for (int j = 0; j < index; j++) { + if (motors[j]->tim == motors[index]->tim) { + overflowed = true; + break; + } + } + if (!overflowed) { + timerForceOverflow(motors[index]->tim); } - // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. *motors[index]->ccr = 0; From 0ef6c3d6f66b6d371041d56b9a0bd6ed81dbfa53 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 2 Aug 2016 23:30:33 +0200 Subject: [PATCH 163/456] Fix gyro_lpf visualisation in configurator --- src/main/io/serial_msp.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index e2157be0bf..63a18299d4 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1247,8 +1247,13 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_ADVANCED_CONFIG : headSerialReply(6); - serialize8(masterConfig.gyro_sync_denom); - serialize8(masterConfig.pid_process_denom); + if (masterConfig.gyro_lpf) { + serialize8(8); // If gyro_lpf != OFF then looptime is set to 1000 + serialize8(1); + } else { + serialize8(masterConfig.gyro_sync_denom); + serialize8(masterConfig.pid_process_denom); + } serialize8(masterConfig.use_unsyncedPwm); serialize8(masterConfig.fast_pwm_protocol); serialize16(masterConfig.motor_pwm_rate); From 3a91c690fa1df43a69bfc394514e06148179568c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 2 Aug 2016 23:32:19 +0200 Subject: [PATCH 164/456] Invert Logic --- src/main/io/serial_msp.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 9a53b56e43..0098db8133 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1239,12 +1239,12 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_ADVANCED_CONFIG : headSerialReply(6); - if (masterConfig.gyro_lpf == 0) { + if (masterConfig.gyro_lpf) { + serialize8(8); // If gyro_lpf != OFF then looptime is set to 1000 + serialize8(1); + } else { serialize8(masterConfig.gyro_sync_denom); serialize8(masterConfig.pid_process_denom); - } else { - serialize8(8); // If gyro_lpf != OFF then looptime is set to 1000 - serialize8(1); } serialize8(masterConfig.use_unsyncedPwm); serialize8(masterConfig.motor_pwm_protocol); From c35675d258cc3e0188e4969eb29480da96bdac1d Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Mon, 25 Jul 2016 10:34:36 +0200 Subject: [PATCH 165/456] Fix AlienFlight F3 and Sparky PPM RX --- src/main/drivers/pwm_mapping.c | 6 +++--- src/main/drivers/pwm_output.c | 12 ------------ src/main/drivers/pwm_output.h | 12 ++++++++++++ src/main/drivers/pwm_rx.c | 33 ++++++++++++++++++++++++--------- src/main/drivers/pwm_rx.h | 2 +- 5 files changed, 40 insertions(+), 25 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 3cec1ab3ef..1ddd6f878d 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -293,8 +293,8 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) if (type == MAP_TO_PPM_INPUT) { #ifndef SKIP_RX_PWM_PPM #if defined(SPARKY) || defined(ALIENFLIGHTF3) - if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2); + if (!(init->pwmProtocolType == PWM_TYPE_CONVENTIONAL)) { + ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2, init->pwmProtocolType); } #endif ppmInConfig(timerHardwarePtr); @@ -307,7 +307,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } else if (type == MAP_TO_MOTOR_OUTPUT) { #ifdef CC3D - if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) { + if (!(init->pwmProtocolType == PWM_TYPE_CONVENTIONAL)) { // Skip it if it would cause PPM capture timer to be reconfigured or manually overflowed if (timerHardwarePtr->tim == TIM2) continue; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 44504f6801..4cc9edede3 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -26,18 +26,6 @@ #include "pwm_mapping.h" #include "pwm_output.h" -#if defined(STM32F40_41xxx) // must be multiples of timer clock -#define ONESHOT125_TIMER_MHZ 12 -#define ONESHOT42_TIMER_MHZ 21 -#define MULTISHOT_TIMER_MHZ 84 -#define PWM_BRUSHED_TIMER_MHZ 21 -#else -#define ONESHOT125_TIMER_MHZ 8 -#define ONESHOT42_TIMER_MHZ 24 -#define MULTISHOT_TIMER_MHZ 72 -#define PWM_BRUSHED_TIMER_MHZ 24 -#endif - #define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index b9bc62c68d..b8443ac406 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -25,6 +25,18 @@ typedef enum { PWM_TYPE_BRUSHED } motorPwmProtocolTypes_e; +#if defined(STM32F40_41xxx) // must be multiples of timer clock +#define ONESHOT125_TIMER_MHZ 12 +#define ONESHOT42_TIMER_MHZ 21 +#define MULTISHOT_TIMER_MHZ 84 +#define PWM_BRUSHED_TIMER_MHZ 21 +#else +#define ONESHOT125_TIMER_MHZ 8 +#define ONESHOT42_TIMER_MHZ 24 +#define MULTISHOT_TIMER_MHZ 72 +#define PWM_BRUSHED_TIMER_MHZ 24 +#endif + struct timerHardware_s; void pwmBrushedMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate); void pwmBrushlessMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 42f0a2fed0..3c72d104dc 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -33,6 +33,7 @@ #include "gpio.h" #include "timer.h" +#include "pwm_output.h" #include "pwm_mapping.h" #include "pwm_rx.h" @@ -85,7 +86,7 @@ static uint16_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT]; static uint8_t ppmFrameCount = 0; static uint8_t lastPPMFrameCount = 0; -static uint8_t ppmCountShift = 0; +static uint8_t ppmCountDivisor = 1; typedef struct ppmDevice_s { uint8_t pulseIndex; @@ -204,14 +205,14 @@ static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture } } - // Divide by 8 if Oneshot125 is active and this is a CC3D board - currentTime = currentTime >> ppmCountShift; + // Divide value if Oneshot, Multishot or brushed motors are active and the timer is shared + currentTime = currentTime / ppmCountDivisor; /* Capture computation */ if (currentTime > previousTime) { - ppmDev.deltaTime = currentTime - (previousTime + (ppmDev.overflowed ? (PPM_TIMER_PERIOD >> ppmCountShift) : 0)); + ppmDev.deltaTime = currentTime - (previousTime + (ppmDev.overflowed ? (PPM_TIMER_PERIOD / ppmCountDivisor) : 0)); } else { - ppmDev.deltaTime = (PPM_TIMER_PERIOD >> ppmCountShift) + currentTime - previousTime; + ppmDev.deltaTime = (PPM_TIMER_PERIOD / ppmCountDivisor) + currentTime - previousTime; } ppmDev.overflowed = false; @@ -383,11 +384,25 @@ void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel) #define UNUSED_PPM_TIMER_REFERENCE 0 #define FIRST_PWM_PORT 0 -void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer) +void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer, uint8_t pwmProtocol) { - if (timerHardwarePtr->tim == sharedPwmTimer) { - ppmCountShift = 3; // Divide by 8 if the timer is running at 8 MHz - } + if (timerHardwarePtr->tim == sharedPwmTimer) { + switch (pwmProtocol) + { + case PWM_TYPE_ONESHOT125: + ppmCountDivisor = ONESHOT125_TIMER_MHZ; + break; + case PWM_TYPE_ONESHOT42: + ppmCountDivisor = ONESHOT42_TIMER_MHZ; + break; + case PWM_TYPE_MULTISHOT: + ppmCountDivisor = MULTISHOT_TIMER_MHZ; + break; + case PWM_TYPE_BRUSHED: + ppmCountDivisor = PWM_BRUSHED_TIMER_MHZ; + break; + } + } } void ppmInConfig(const timerHardware_t *timerHardwarePtr) diff --git a/src/main/drivers/pwm_rx.h b/src/main/drivers/pwm_rx.h index 46afd388c5..1eb792b7fe 100644 --- a/src/main/drivers/pwm_rx.h +++ b/src/main/drivers/pwm_rx.h @@ -26,7 +26,7 @@ typedef enum { void ppmInConfig(const timerHardware_t *timerHardwarePtr); -void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer); +void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer, uint8_t pwmProtocol); void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel); uint16_t pwmRead(uint8_t channel); From 4ba7c4582a67310f3cdadd0ab2e5bbbff7b3e101 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 28 Jul 2016 16:16:07 +0200 Subject: [PATCH 166/456] Correct PPM port for Sparky and AlienFlight F3 targets --- src/main/target/ALIENFLIGHTF3/target.h | 2 +- src/main/target/SPARKY/target.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 144cc3074d..28a896ff01 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -72,7 +72,7 @@ #define USE_UART2 // Receiver - RX (PA3) #define USE_UART3 // Not connected - 10/RX (PB11) 11/TX (PB10) #define SERIAL_PORT_COUNT 4 -#define AVOID_UART3_FOR_PWM_PPM +#define AVOID_UART2_FOR_PWM_PPM #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 9dbce83c11..582b82bf76 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -56,7 +56,7 @@ #define USE_UART2 // Input - RX (PA3) #define USE_UART3 // Servo out - 10/RX (PB11) 11/TX (PB10) #define SERIAL_PORT_COUNT 4 -#define AVOID_UART3_FOR_PWM_PPM +#define AVOID_UART2_FOR_PWM_PPM #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 From 56162d72166b1c2f70199cc1b3bf56ed79e0e776 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 2 Aug 2016 22:59:16 +0200 Subject: [PATCH 167/456] AlienFlight F3 PPM timer initialization fix --- src/main/target/ALIENFLIGHTF3/target.c | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index 2ced9687ac..c1946aa17c 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -63,28 +63,19 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - // 6 x 3 pin headers + // up to 10 Motor Outputs { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - - // 6 pin header - // PWM7-10 { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N // PPM PORT - Also USART2 RX (AF5) - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 - //{ TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, IOCFG_IPD, GPIO_PinSource3, GPIO_AF_9 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 - - // USART3 RX/TX - // RX conflicts with PPM port - //{ TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_PinSource11, GPIO_AF_1 } // RX - PB11 - *TIM2_CH4, UART3_RX (AF7) - PWM11 - //{ TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_PinSource10, GPIO_AF_1 } // TX - PB10 - *TIM2_CH3, UART3_TX (AF7) - PWM12 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP_PD, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 }; From 98c560697b21135be61543a1833b9e620eb850dc Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 3 Aug 2016 08:06:11 +0200 Subject: [PATCH 168/456] Apply default settings also to the AlienFlight F1 target (flight tested with an 110mm brushed quadcopter) --- src/main/target/ALIENFLIGHTF1/config.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index 7dd16e808f..3369b17635 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -78,6 +78,12 @@ void targetConfiguration(void) masterConfig.motor_pwm_rate = 32000; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; + currentProfile->pidProfile.P8[ROLL] = 90; + currentProfile->pidProfile.I8[ROLL] = 44; + currentProfile->pidProfile.D8[ROLL] = 60; + currentProfile->pidProfile.P8[PITCH] = 90; + currentProfile->pidProfile.I8[PITCH] = 44; + currentProfile->pidProfile.D8[PITCH] = 60; masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R From ca703b1ff151c4bff7313024bc40141c4b489db6 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 1 Aug 2016 23:37:52 +0200 Subject: [PATCH 169/456] Change defaults // Add optional filter config --- src/main/config/config.c | 15 ++++++++------- src/main/config/config_master.h | 1 + src/main/io/serial_cli.c | 7 +++++++ src/main/sensors/gyro.c | 17 ++++++++++++++--- src/main/sensors/gyro.h | 7 ++++++- 5 files changed, 36 insertions(+), 11 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 20792f6246..1c1b5d221d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -240,13 +240,13 @@ static void resetPidProfile(pidProfile_t *pidProfile) // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; - pidProfile->dtermSetpointWeight = 0; + pidProfile->dtermSetpointWeight = 200; pidProfile->pidMaxVelocity = 1000; pidProfile->pidMaxVelocityYaw = 50; - pidProfile->toleranceBand = 15; + pidProfile->toleranceBand = 20; pidProfile->toleranceBandReduction = 40; pidProfile->zeroCrossAllowanceCount = 2; - pidProfile->itermThrottleGain = 10; + pidProfile->itermThrottleGain = 0; #ifdef GTUNE pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune. @@ -475,11 +475,12 @@ static void resetConf(void) masterConfig.gyro_sync_denom = 4; masterConfig.pid_process_denom = 2; #endif - masterConfig.gyro_soft_lpf_hz = 100; + masterConfig.gyro_soft_type = GYRO_FILTER_PT1; + masterConfig.gyro_soft_lpf_hz = 80; masterConfig.gyro_soft_notch_hz = 0; masterConfig.gyro_soft_notch_q = 5; - masterConfig.debug_mode = 0; + masterConfig.debug_mode = DEBUG_NONE; resetAccelerometerTrims(&masterConfig.accZero); @@ -527,7 +528,7 @@ static void resetConf(void) masterConfig.rxConfig.rssi_channel = 0; masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; masterConfig.rxConfig.rssi_ppm_invert = 0; - masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_OFF; + masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_AUTO; masterConfig.rxConfig.rcSmoothInterval = 9; masterConfig.rxConfig.fpvCamAngleDegrees = 0; masterConfig.rxConfig.max_aux_channel = MAX_AUX_CHANNELS; @@ -742,7 +743,7 @@ void activateConfig(void) ¤tProfile->pidProfile ); - gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q); + gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q, masterConfig.gyro_soft_type); #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 29a236e565..e0bf2f7690 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -58,6 +58,7 @@ typedef struct master_t { uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes. uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_sync_denom; // Gyro sample divider + uint8_t gyro_soft_type; // Gyro Filter Type uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz uint16_t gyro_soft_notch_hz; // Biquad gyro notch hz uint8_t gyro_soft_notch_q; // Biquad gyro notch quality diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5e89ff37e0..ecb422f348 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -497,6 +497,10 @@ static const char * const lookupTableRcSmoothing[] = { "OFF", "DEFAULT", "AUTO", "MANUAL" }; +static const char * const lookupTableGyroSoftLowpassType[] = { + "NORMAL", "HIGH" +}; + typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -534,6 +538,7 @@ typedef enum { TABLE_MOTOR_PWM_PROTOCOL, TABLE_DELTA_METHOD, TABLE_RC_SMOOTHING, + TABLE_GYRO_LOWPASS_TYPE, #ifdef OSD TABLE_OSD, #endif @@ -571,6 +576,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) }, { lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) }, { lookupTableRcSmoothing, sizeof(lookupTableRcSmoothing) / sizeof(char *) }, + { lookupTableGyroSoftLowpassType, sizeof(lookupTableGyroSoftLowpassType) / sizeof(char *) }, #ifdef OSD { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, #endif @@ -742,6 +748,7 @@ const clivalue_t valueTable[] = { { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, + { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_GYRO_LOWPASS_TYPE } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, { "gyro_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } }, { "gyro_notch_q", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_notch_q, .config.minmax = { 1, 100 } }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 40a30ebb7c..5d8de9fc72 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -47,17 +47,21 @@ static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT]; static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT]; +static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; +static uint8_t gyroSoftLpfType; static uint16_t gyroSoftNotchHz; static uint8_t gyroSoftNotchQ; static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; +static float gyroDt; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q) +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q, uint8_t gyro_soft_lpf_type) { gyroConfig = gyroConfigToUse; gyroSoftLpfHz = gyro_soft_lpf_hz; gyroSoftNotchHz = gyro_soft_notch_hz; gyroSoftNotchQ = gyro_soft_notch_q; + gyroSoftLpfType = gyro_soft_lpf_type; } void gyroInit(void) @@ -65,7 +69,10 @@ void gyroInit(void) if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH); - biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); + if (gyroSoftLpfType == GYRO_FILTER_BIQUAD) + biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); + else + gyroDt = gyro.targetLooptime / 1000.0f; } } } @@ -175,7 +182,11 @@ void gyroUpdate(void) debug[axis*2 + 1] = lrintf(sample); } - gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);; + if (gyroSoftLpfType == GYRO_FILTER_BIQUAD) { + gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample); + } else { + gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], sample, gyroSoftLpfHz, gyroDt); + } gyroADC[axis] = lrintf(gyroADCf[axis]); } } else { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 4d2153fa0d..403f54ca90 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -31,6 +31,11 @@ typedef enum { GYRO_MAX = GYRO_FAKE } gyroSensor_e; +typedef enum { + GYRO_FILTER_PT1 = 0, + GYRO_FILTER_BIQUAD, +} gyroFilter_e; + extern gyro_t gyro; extern int32_t gyroADC[XYZ_AXIS_COUNT]; @@ -40,7 +45,7 @@ typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. } gyroConfig_t; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q); +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q, uint8_t gyro_soft_lpf_type); void gyroSetCalibrationCycles(void); void gyroInit(void); void gyroUpdate(void); From 771feb8fdec4a90dd51c57efe83ec44d842637b4 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 3 Aug 2016 11:51:12 +0200 Subject: [PATCH 170/456] Add more filter freedom --- src/main/common/filter.c | 2 +- src/main/common/filter.h | 11 ++++++++--- src/main/config/config.c | 8 ++++++-- src/main/flight/pid.c | 42 +++++++++++++++++++++++++++++++++++++--- src/main/flight/pid.h | 3 +++ src/main/io/serial_cli.c | 11 +++++++---- src/main/sensors/gyro.c | 4 ++-- src/main/sensors/gyro.h | 5 ----- 8 files changed, 66 insertions(+), 20 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 90c3decdae..04bd90823c 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -60,7 +60,7 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr { biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); } -void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType) +void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) { // setup variables const float sampleRate = 1 / ((float)refreshRate * 0.000001f); diff --git a/src/main/common/filter.h b/src/main/common/filter.h index d97becc015..5f9c34ba61 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -30,12 +30,17 @@ typedef struct biquadFilter_s { } biquadFilter_t; typedef enum { - FILTER_LPF, - FILTER_NOTCH + FILTER_PT1 = 0, + FILTER_BIQUAD, } filterType_e; +typedef enum { + FILTER_LPF, + FILTER_NOTCH +} biquadFilterType_e; + void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); -void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, filterType_e filterType); +void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); float biquadFilterApply(biquadFilter_t *filter, float input); void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT); diff --git a/src/main/config/config.c b/src/main/config/config.c index 1c1b5d221d..7e535ee366 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -20,6 +20,7 @@ #include #include "platform.h" +#include "debug.h" #include "build_config.h" @@ -171,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 141; +static const uint8_t EEPROM_CONF_VERSION = 142; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -233,7 +234,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_lpf_hz = 80; pidProfile->rollPitchItermIgnoreRate = 200; pidProfile->yawItermIgnoreRate = 50; + pidProfile->dterm_filter_type = FILTER_PT1; pidProfile->dterm_lpf_hz = 100; // filtering ON by default + pidProfile->dterm_notch_hz = 0; + pidProfile->dterm_notch_q = 5; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; @@ -475,7 +479,7 @@ static void resetConf(void) masterConfig.gyro_sync_denom = 4; masterConfig.pid_process_denom = 2; #endif - masterConfig.gyro_soft_type = GYRO_FILTER_PT1; + masterConfig.gyro_soft_type = FILTER_PT1; masterConfig.gyro_soft_lpf_hz = 80; masterConfig.gyro_soft_notch_hz = 0; masterConfig.gyro_soft_notch_q = 5; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4b916911d9..db7165bf86 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -66,7 +66,6 @@ uint8_t PIDweight[3]; static int32_t errorGyroI[3]; static float errorGyroIf[3]; - static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); #ifdef SKIP_PID_FLOAT @@ -107,6 +106,23 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static pt1Filter_t deltaFilter[3]; static pt1Filter_t yawFilter; +static biquadFilter_t dtermFilterLpf[3]; +static biquadFilter_t dtermFilterNotch[3]; +static bool dtermNotchInitialised, dtermBiquadLpfInitialised; + +void initFilters(const pidProfile_t *pidProfile) { + int axis; + + if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { + for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, ((float) pidProfile->dterm_notch_q) / 10, FILTER_NOTCH); + } + + if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { + if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) { + for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, gyro.targetLooptime); + } + } +} #ifndef SKIP_PID_FLOAT // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab) @@ -126,6 +142,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float + initFilters(pidProfile); + if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); @@ -265,7 +283,15 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd * delta * dynReduction; // Filter delta - if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); + + if (pidProfile->dterm_lpf_hz) { + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + } DTerm = constrainf(Kd * delta * dynReduction, -300.0f, 300.0f); @@ -317,6 +343,8 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina int8_t horizonLevelStrength = 100; + initFilters(pidProfile); + if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); @@ -417,7 +445,15 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; // Filter delta - if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT()); + if (pidProfile->dterm_lpf_hz) { + float deltaf = delta; // single conversion + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + delta = lrintf(deltaf); + } DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e4fe96ce89..7209f193d3 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -80,8 +80,11 @@ typedef struct pidProfile_s { uint8_t I8[PID_ITEM_COUNT]; uint8_t D8[PID_ITEM_COUNT]; + uint8_t dterm_filter_type; // Filter selection for dterm uint16_t dterm_lpf_hz; // Delta Filter in hz uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy + uint16_t dterm_notch_hz; // Biquad dterm notch hz + uint8_t dterm_notch_q; // Biquad dterm notch quality uint8_t deltaMethod; // Alternative delta Calculation uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ecb422f348..8bb22a9534 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -497,7 +497,7 @@ static const char * const lookupTableRcSmoothing[] = { "OFF", "DEFAULT", "AUTO", "MANUAL" }; -static const char * const lookupTableGyroSoftLowpassType[] = { +static const char * const lookupTableLowpassType[] = { "NORMAL", "HIGH" }; @@ -538,7 +538,7 @@ typedef enum { TABLE_MOTOR_PWM_PROTOCOL, TABLE_DELTA_METHOD, TABLE_RC_SMOOTHING, - TABLE_GYRO_LOWPASS_TYPE, + TABLE_LOWPASS_TYPE, #ifdef OSD TABLE_OSD, #endif @@ -576,7 +576,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) }, { lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) }, { lookupTableRcSmoothing, sizeof(lookupTableRcSmoothing) / sizeof(char *) }, - { lookupTableGyroSoftLowpassType, sizeof(lookupTableGyroSoftLowpassType) / sizeof(char *) }, + { lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) }, #ifdef OSD { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, #endif @@ -748,7 +748,7 @@ const clivalue_t valueTable[] = { { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, - { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_GYRO_LOWPASS_TYPE } }, + { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, { "gyro_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } }, { "gyro_notch_q", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_notch_q, .config.minmax = { 1, 100 } }, @@ -821,7 +821,10 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, #endif + { "dterm_filter_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, + { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 500 } }, + { "dterm_notch_q", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_q, .config.minmax = { 1, 100 } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } }, { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5d8de9fc72..2905b9a297 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -69,7 +69,7 @@ void gyroInit(void) if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH); - if (gyroSoftLpfType == GYRO_FILTER_BIQUAD) + if (gyroSoftLpfType == FILTER_BIQUAD) biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); else gyroDt = gyro.targetLooptime / 1000.0f; @@ -182,7 +182,7 @@ void gyroUpdate(void) debug[axis*2 + 1] = lrintf(sample); } - if (gyroSoftLpfType == GYRO_FILTER_BIQUAD) { + if (gyroSoftLpfType == FILTER_BIQUAD) { gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample); } else { gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], sample, gyroSoftLpfHz, gyroDt); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 403f54ca90..63c99f1a6e 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -31,11 +31,6 @@ typedef enum { GYRO_MAX = GYRO_FAKE } gyroSensor_e; -typedef enum { - GYRO_FILTER_PT1 = 0, - GYRO_FILTER_BIQUAD, -} gyroFilter_e; - extern gyro_t gyro; extern int32_t gyroADC[XYZ_AXIS_COUNT]; From 69ab04f87fdd6d0f383cc585baa40f1859b2f7b4 Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Wed, 3 Aug 2016 13:48:44 +0100 Subject: [PATCH 171/456] Additional Header Fields Add gyro and PID denoms to log header. --- src/main/blackbox/blackbox.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 96281b05fe..46e01ce9ad 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1168,6 +1168,8 @@ static bool blackboxWriteSysinfo() ); BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime); + BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyro_sync_denom); + BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", masterConfig.pid_process_denom); BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8); BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8); BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8); @@ -1228,6 +1230,10 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); BLACKBOX_PRINT_HEADER_LINE("rc_smooth_interval:%d", masterConfig.rxConfig.rcSmoothInterval); BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold); + BLACKBOX_PRINT_HEADER_LINE("serialrx_provider:%d", masterConfig.rxConfig.serialrx_provider); + BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", masterConfig.use_unsyncedPwm); + BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", masterConfig.motor_pwm_protocol); + BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", masterConfig.motor_pwm_rate); BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures); default: From cc7ea13e66ef7abcf22b430fdabc5a1be656d1ea Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 3 Aug 2016 20:56:37 +0200 Subject: [PATCH 172/456] Convert Notch Q to BW --- src/main/blackbox/blackbox.c | 4 +++- src/main/config/config.c | 6 +++--- src/main/config/config_master.h | 2 +- src/main/flight/pid.c | 5 ++++- src/main/flight/pid.h | 2 +- src/main/io/serial_cli.c | 4 ++-- src/main/sensors/gyro.c | 11 +++++++---- src/main/sensors/gyro.h | 2 +- 8 files changed, 22 insertions(+), 14 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 96281b05fe..fc4a7ceb85 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1215,12 +1215,14 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("dterm_notch_bw:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_bw * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband); BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d", (int)(masterConfig.gyro_soft_notch_hz * 100.0f)); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_q:%d", (int)(masterConfig.gyro_soft_notch_q * 10.0f)); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_bw:%d", (int)(masterConfig.gyro_soft_notch_bw * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.acc_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware); diff --git a/src/main/config/config.c b/src/main/config/config.c index 7e535ee366..c18c1ed343 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -237,7 +237,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dterm_filter_type = FILTER_PT1; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 0; - pidProfile->dterm_notch_q = 5; + pidProfile->dterm_notch_bw = 200; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; @@ -482,7 +482,7 @@ static void resetConf(void) masterConfig.gyro_soft_type = FILTER_PT1; masterConfig.gyro_soft_lpf_hz = 80; masterConfig.gyro_soft_notch_hz = 0; - masterConfig.gyro_soft_notch_q = 5; + masterConfig.gyro_soft_notch_bw = 200; masterConfig.debug_mode = DEBUG_NONE; @@ -747,7 +747,7 @@ void activateConfig(void) ¤tProfile->pidProfile ); - gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q, masterConfig.gyro_soft_type); + gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_bw, masterConfig.gyro_soft_type); #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index e0bf2f7690..48046bd22f 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -61,7 +61,7 @@ typedef struct master_t { uint8_t gyro_soft_type; // Gyro Filter Type uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz uint16_t gyro_soft_notch_hz; // Biquad gyro notch hz - uint8_t gyro_soft_notch_q; // Biquad gyro notch quality + uint16_t gyro_soft_notch_bw; // Biquad gyro notch quality uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index db7165bf86..4b19dbe18d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -114,7 +114,10 @@ void initFilters(const pidProfile_t *pidProfile) { int axis; if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { - for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, ((float) pidProfile->dterm_notch_q) / 10, FILTER_NOTCH); + float octaves = log2f(((float) pidProfile->dterm_notch_hz + pidProfile->dterm_notch_bw / 2) / ((float) pidProfile->dterm_notch_hz - pidProfile->dterm_notch_bw / 2)); + float dtermNotchQ = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1); + + for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, dtermNotchQ, FILTER_NOTCH); } if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7209f193d3..9316af5e01 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -84,7 +84,7 @@ typedef struct pidProfile_s { uint16_t dterm_lpf_hz; // Delta Filter in hz uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy uint16_t dterm_notch_hz; // Biquad dterm notch hz - uint8_t dterm_notch_q; // Biquad dterm notch quality + uint16_t dterm_notch_bw; // Biquad dterm notch quality uint8_t deltaMethod; // Alternative delta Calculation uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 8bb22a9534..edb7f43927 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -751,7 +751,7 @@ const clivalue_t valueTable[] = { { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, { "gyro_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } }, - { "gyro_notch_q", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_notch_q, .config.minmax = { 1, 100 } }, + { "gyro_notch_bw", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_bw, .config.minmax = { 1, 500 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, @@ -824,7 +824,7 @@ const clivalue_t valueTable[] = { { "dterm_filter_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 500 } }, - { "dterm_notch_q", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_q, .config.minmax = { 1, 100 } }, + { "dterm_notch_bw", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_bw, .config.minmax = { 1, 500 } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } }, { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 2905b9a297..7833ccc7c1 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -50,25 +50,28 @@ static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT]; static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfType; static uint16_t gyroSoftNotchHz; -static uint8_t gyroSoftNotchQ; +static float gyroSoftNotchQ; static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; static float gyroDt; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q, uint8_t gyro_soft_lpf_type) +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_bw, uint8_t gyro_soft_lpf_type) { gyroConfig = gyroConfigToUse; gyroSoftLpfHz = gyro_soft_lpf_hz; gyroSoftNotchHz = gyro_soft_notch_hz; - gyroSoftNotchQ = gyro_soft_notch_q; + gyroSoftNotchQ = gyro_soft_notch_bw; gyroSoftLpfType = gyro_soft_lpf_type; + + float octaves = log2f(((float) gyro_soft_notch_hz + gyro_soft_notch_bw / 2) / ((float) gyro_soft_notch_hz - gyro_soft_notch_bw / 2)); + gyroSoftNotchQ = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1); } void gyroInit(void) { if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH); + biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, gyroSoftNotchQ, FILTER_NOTCH); if (gyroSoftLpfType == FILTER_BIQUAD) biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); else diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 63c99f1a6e..385ae4a98c 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -40,7 +40,7 @@ typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. } gyroConfig_t; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q, uint8_t gyro_soft_lpf_type); +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_bw, uint8_t gyro_soft_lpf_type); void gyroSetCalibrationCycles(void); void gyroInit(void); void gyroUpdate(void); From f6194f0eb779492e1c9a223e3d16bdb1ed1a0f2e Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 3 Aug 2016 21:19:40 +0100 Subject: [PATCH 173/456] Used forward references to remove #includes from header files --- src/main/blackbox/blackbox.c | 1 + src/main/blackbox/blackbox_io.c | 1 + src/main/config/config.c | 1 + src/main/drivers/bus_i2c.h | 4 ++-- src/main/drivers/exti.h | 2 +- src/main/drivers/sound_beeper.h | 2 +- src/main/flight/altitudehold.h | 16 +++++++++------- src/main/flight/imu.h | 3 ++- src/main/flight/pid.h | 1 - src/main/io/display.c | 2 ++ src/main/io/rc_controls.h | 9 ++++----- src/main/io/rc_curves.c | 7 ++++--- src/main/rx/rx.c | 4 ++-- src/main/rx/xbus.h | 2 -- src/main/sensors/battery.c | 2 ++ src/main/telemetry/frsky.h | 5 ++--- src/main/telemetry/hott.h | 3 ++- src/main/telemetry/ltm.h | 3 ++- src/main/telemetry/smartport.h | 3 ++- 19 files changed, 40 insertions(+), 31 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 96281b05fe..410a2c3bc4 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -72,6 +72,7 @@ #include "flight/altitudehold.h" #include "flight/failsafe.h" #include "flight/imu.h" +#include "flight/pid.h" #include "flight/navigation.h" #include "config/runtime_config.h" diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index cd4f05d980..88306767c6 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -53,6 +53,7 @@ #include "flight/altitudehold.h" #include "flight/failsafe.h" #include "flight/imu.h" +#include "flight/pid.h" #include "flight/navigation.h" #include "config/runtime_config.h" diff --git a/src/main/config/config.c b/src/main/config/config.c index 1c1b5d221d..ffff6578a8 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -22,6 +22,7 @@ #include "platform.h" #include "build_config.h" +#include "debug.h" #include "blackbox/blackbox_io.h" diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index 86f7a66181..d31d806225 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -21,8 +21,8 @@ #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) #define I2C_DEFAULT_TIMEOUT I2C_SHORT_TIMEOUT -#include "drivers/io.h" -#include "drivers/rcc.h" +#include "io.h" +#include "rcc.h" #ifndef I2C_DEVICE #define I2C_DEVICE I2CINVALID diff --git a/src/main/drivers/exti.h b/src/main/drivers/exti.h index 8071c65e33..713ec50aec 100644 --- a/src/main/drivers/exti.h +++ b/src/main/drivers/exti.h @@ -18,7 +18,7 @@ #pragma once -#include "drivers/io.h" +#include "io.h" // old EXTI interface, to be replaced typedef struct extiConfig_s { diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index ab7a7c3dfc..4fad6422cc 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -17,7 +17,7 @@ #pragma once -#include "drivers/io.h" +#include "io.h" #ifdef BEEPER #define BEEP_TOGGLE systemBeepToggle() diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index 3677fbb287..da16978814 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -15,19 +15,21 @@ * along with Cleanflight. If not, see . */ -#include "io/escservo.h" -#include "io/rc_controls.h" -#include "flight/pid.h" - -#include "sensors/barometer.h" +#pragma once extern int32_t AltHold; extern int32_t vario; void calculateEstimatedAltitude(uint32_t currentTime); -void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig); -void applyAltHold(airplaneConfig_t *airplaneConfig); +struct pidProfile_s; +struct barometerConfig_s; +struct rcControlsConfig_s; +struct escAndServoConfig_s; +void configureAltitudeHold(struct pidProfile_s *initialPidProfile, struct barometerConfig_s *intialBarometerConfig, struct rcControlsConfig_s *initialRcControlsConfig, struct escAndServoConfig_s *initialEscAndServoConfig); + +struct airplaneConfig_t; +void applyAltHold(struct airplaneConfig_s *airplaneConfig); void updateAltHoldState(void); void updateSonarAltHoldState(void); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 94b361fcf6..e5229a2ead 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -69,9 +69,10 @@ typedef struct accProcessor_s { accProcessorState_e state; } accProcessor_t; +struct pidProfile_s; void imuConfigure( imuRuntimeConfig_t *initialImuRuntimeConfig, - pidProfile_t *initialPidProfile, + struct pidProfile_s *initialPidProfile, accDeadband_t *initialAccDeadband, uint16_t throttle_correction_angle ); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e4fe96ce89..320eab9f18 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -14,7 +14,6 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ -#include "rx/rx.h" #pragma once diff --git a/src/main/io/display.c b/src/main/io/display.c index 1e5eee9746..a22ca88023 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -63,6 +63,8 @@ #include "io/display.h" +#include "rx/rx.h" + #include "scheduler/scheduler.h" extern profile_t *currentProfile; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index aace839d9b..a3622aacab 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -17,8 +17,6 @@ #pragma once -#include "rx/rx.h" - typedef enum { BOXARM = 0, BOXANGLE, @@ -167,8 +165,9 @@ typedef struct rcControlsConfig_s { bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); -throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); -void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); +struct rxConfig_s; +throttleStatus_e calculateThrottleStatus(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void processRcStickPositions(struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); @@ -257,7 +256,7 @@ bool isAirmodeActive(void); bool isSuperExpoActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); -void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); +void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig); bool isUsingSticksForArming(void); diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index 89b46522f2..4a2b20bdde 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -18,12 +18,13 @@ #include #include -#include "io/rc_controls.h" -#include "io/escservo.h" +#include "config/config.h" +#include "io/escservo.h" +#include "io/rc_controls.h" #include "io/rc_curves.h" -#include "config/config.h" +#include "rx/rx.h" #define THROTTLE_LOOKUP_LENGTH 12 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index a7a09a44b7..f5c0c9a9d7 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -41,6 +41,8 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/system.h" + +#include "rx/rx.h" #include "rx/pwm.h" #include "rx/sbus.h" #include "rx/spektrum.h" @@ -51,8 +53,6 @@ #include "rx/ibus.h" #include "rx/jetiexbus.h" -#include "rx/rx.h" - //#define DEBUG_RX_SIGNAL_LOSS diff --git a/src/main/rx/xbus.h b/src/main/rx/xbus.h index bc76f56786..19dc6d5735 100644 --- a/src/main/rx/xbus.h +++ b/src/main/rx/xbus.h @@ -17,7 +17,5 @@ #pragma once -#include "rx/rx.h" - bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); uint8_t xBusFrameStatus(void); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 75aac6cf0a..6d165cbc57 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -35,6 +35,8 @@ #include "io/rc_controls.h" #include "io/beeper.h" +#include "rx/rx.h" + #define VBATT_PRESENT_THRESHOLD_MV 10 #define VBATT_LPF_FREQ 0.4f diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index 555eb43e83..c057f22376 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -17,14 +17,13 @@ #pragma once -#include "rx/rx.h" - typedef enum { FRSKY_VFAS_PRECISION_LOW = 0, FRSKY_VFAS_PRECISION_HIGH } frskyVFasPrecision_e; -void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); +struct rxConfig_s; +void handleFrSkyTelemetry(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); void checkFrSkyTelemetryState(void); void initFrSkyTelemetry(telemetryConfig_t *telemetryConfig); diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index 64a43bd507..19d4c5369a 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -488,7 +488,8 @@ typedef struct HOTT_AIRESC_MSG_s { void handleHoTTTelemetry(void); void checkHoTTTelemetryState(void); -void initHoTTTelemetry(telemetryConfig_t *telemetryConfig); +struct telemetryConfig_s; +void initHoTTTelemetry(struct telemetryConfig_s *telemetryConfig); void configureHoTTTelemetryPort(void); void freeHoTTTelemetryPort(void); diff --git a/src/main/telemetry/ltm.h b/src/main/telemetry/ltm.h index 6e4b80cbfa..e5b4790c5a 100644 --- a/src/main/telemetry/ltm.h +++ b/src/main/telemetry/ltm.h @@ -19,7 +19,8 @@ #pragma once -void initLtmTelemetry(telemetryConfig_t *initialTelemetryConfig); +struct telemetryConfig_s; +void initLtmTelemetry(struct telemetryConfig_s *initialTelemetryConfig); void handleLtmTelemetry(void); void checkLtmTelemetryState(void); diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index b24be90708..932ba1a929 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -7,7 +7,8 @@ #pragma once -void initSmartPortTelemetry(telemetryConfig_t *); +struct telemetryConfig_s; +void initSmartPortTelemetry(struct telemetryConfig_s *); void handleSmartPortTelemetry(void); void checkSmartPortTelemetryState(void); From 91db1fc442e46410cab8966bb1bed5e329d00e2e Mon Sep 17 00:00:00 2001 From: tommyleo Date: Wed, 3 Aug 2016 22:51:45 +0200 Subject: [PATCH 174/456] Add target for VRRACE --- src/main/target/VRRACE/target.c | 103 ++++++++++++++++++ src/main/target/VRRACE/target.h | 180 +++++++++++++++++++++++++++++++ src/main/target/VRRACE/target.mk | 7 ++ 3 files changed, 290 insertions(+) create mode 100644 src/main/target/VRRACE/target.c create mode 100644 src/main/target/VRRACE/target.h create mode 100644 src/main/target/VRRACE/target.mk diff --git a/src/main/target/VRRACE/target.c b/src/main/target/VRRACE/target.c new file mode 100644 index 0000000000..094ade421e --- /dev/null +++ b/src/main/target/VRRACE/target.c @@ -0,0 +1,103 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + 0xFFFF +}; + + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // PPM + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S2_IN + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S3_IN + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S4_IN + { TIM9, IO_TAG(PE6), TIM_Channel_1, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S5_IN + { TIM9, IO_TAG(PE7), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S6_IN + + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S1_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S2_OUT + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S3_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S4_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S6_OUT +}; diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h new file mode 100644 index 0000000000..da1581e9f3 --- /dev/null +++ b/src/main/target/VRRACE/target.h @@ -0,0 +1,180 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "VRRA" +#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) + +#define USBD_PRODUCT_STRING "VRRACE" + +#define LED0 PD14 +#define LED1 PD15 +#define BEEPER PA0 +#define BEEPER_INVERTED + +#define INVERTER PD7 +#define INVERTER_USART USART6 + +#define MPU6500_CS_PIN PE10 +#define MPU6500_SPI_INSTANCE SPI2 + +#define ACC +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW270_DEG + +#define GYRO +#define USE_GYRO_SPI_MPU6500 +#define GYRO_MPU6500_ALIGN CW270_DEG + +// MPU6500 interrupts +#define USE_MPU_DATA_READY_SIGNAL +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) +#define MPU_INT_EXTI PD10 +#define USE_EXTI + +/* +#define BARO +#define USE_BARO_MS5611 +#define MS5611_I2C_INSTANCE I2CDEV_1 + +#define USE_SDCARD + +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PD2 +#define SDCARD_SPI_INSTANCE SPI2 +#define SDCARD_SPI_CS_PIN PB12 +*/ + +/* +#define SDCARD_DETECT_PIN PD2 +#define SDCARD_DETECT_EXTI_LINE EXTI_Line2 +#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource2 +#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOD +#define SDCARD_DETECT_EXTI_IRQn EXTI2_IRQn + +#define SDCARD_SPI_INSTANCE SPI3 +#define SDCARD_SPI_CS_PIN PB3 +*/ + +// SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: +/* +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz +*/ +// Divide to under 25MHz for normal operation: +/* +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz +*/ + +/* +#define SDCARD_DMA_CHANNEL_TX DMA1_Stream4 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4 +#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 +#define SDCARD_DMA_CHANNEL DMA_Channel_0 +*/ + + +/* +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PB3 +#define M25P16_SPI_INSTANCE SPI3 +*/ + +#define USE_VCP +#define VBUS_SENSING_PIN PA9 +//#define VBUS_SENSING_ENABLED + +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 + +#define USE_UART2 +#define UART2_RX_PIN PD6 +#define UART2_TX_PIN PD5 + +#define USE_UART3 +#define UART3_RX_PIN PD9 +#define UART3_TX_PIN PD8 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define USE_SOFTSERIAL1 + +#define SOFTSERIAL_1_TIMER TIM1 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 2 // PWM 3 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 1 // PWM 2 + + +#define SERIAL_PORT_COUNT 6 //VCP, USART1, USART2, USART3, USART6, SOFTSERIAL1 + + +#define USE_SPI + +#define USE_SPI_DEVICE_1 + +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PE10 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +/* +#define USE_SPI_DEVICE_3 +#define SPI3_NSS_PIN PB3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB8-SCL, PB8-SDA +#define USE_I2C_PULLUP +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +*/ + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER +#define VBAT_ADC_PIN PC0 +#define RSSI_ADC_GPIO_PIN PB1 +#define CURRENT_METER_ADC_PIN PA5 + +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_ONESHOT125 | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +//#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +/* +#define SPEKTRUM_BIND +// USART3 Rx, PB11 +#define BIND_PIN PB11 +*/ + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define USABLE_TIMER_CHANNEL_COUNT 12 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) ) diff --git a/src/main/target/VRRACE/target.mk b/src/main/target/VRRACE/target.mk new file mode 100644 index 0000000000..ed853fd62b --- /dev/null +++ b/src/main/target/VRRACE/target.mk @@ -0,0 +1,7 @@ +F405_TARGETS += $(TARGET) +FEATURES += VCP + +TARGET_SRC = \ + drivers/accgyro_spi_mpu6000.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/accgyro_mpu6500.c From 0e905e01d3ec15b8055c7b9e599d6b4a91812046 Mon Sep 17 00:00:00 2001 From: rav Date: Wed, 3 Aug 2016 23:45:52 +0200 Subject: [PATCH 175/456] set lower filter cutoff instead of Q fix error in gyro pt1 filter --- src/main/blackbox/blackbox.c | 4 ++-- src/main/common/filter.c | 5 +++++ src/main/common/filter.h | 1 + src/main/config/config.c | 6 +++--- src/main/config/config_master.h | 2 +- src/main/flight/pid.c | 6 ++---- src/main/flight/pid.h | 2 +- src/main/io/serial_cli.c | 4 ++-- src/main/sensors/gyro.c | 9 +++------ src/main/sensors/gyro.h | 2 +- 10 files changed, 21 insertions(+), 20 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ae2932a0e9..a59cc8d78f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1218,13 +1218,13 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_hz * 100.0f)); - BLACKBOX_PRINT_HEADER_LINE("dterm_notch_bw:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_bw * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_cutoff * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband); BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d", (int)(masterConfig.gyro_soft_notch_hz * 100.0f)); - BLACKBOX_PRINT_HEADER_LINE("gyro_notch_bw:%d", (int)(masterConfig.gyro_soft_notch_bw * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d", (int)(masterConfig.gyro_soft_notch_cutoff * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.acc_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware); diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 04bd90823c..70b77ad289 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -55,6 +55,11 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT) return filter->state; } +float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff) { + float octaves = log2f((float) centerFreq / (float) cutoff) * 2; + return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1); +} + /* sets up a biquad Filter */ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate) { diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 5f9c34ba61..c584a23c86 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -42,6 +42,7 @@ typedef enum { void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); float biquadFilterApply(biquadFilter_t *filter, float input); +float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff); void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT); float pt1FilterApply(pt1Filter_t *filter, float input); diff --git a/src/main/config/config.c b/src/main/config/config.c index c18c1ed343..991d6f5c74 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -237,7 +237,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dterm_filter_type = FILTER_PT1; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 0; - pidProfile->dterm_notch_bw = 200; + pidProfile->dterm_notch_cutoff = 150; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; @@ -482,7 +482,7 @@ static void resetConf(void) masterConfig.gyro_soft_type = FILTER_PT1; masterConfig.gyro_soft_lpf_hz = 80; masterConfig.gyro_soft_notch_hz = 0; - masterConfig.gyro_soft_notch_bw = 200; + masterConfig.gyro_soft_notch_cutoff = 150; masterConfig.debug_mode = DEBUG_NONE; @@ -747,7 +747,7 @@ void activateConfig(void) ¤tProfile->pidProfile ); - gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_bw, masterConfig.gyro_soft_type); + gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_cutoff, masterConfig.gyro_soft_type); #ifdef TELEMETRY telemetryUseConfig(&masterConfig.telemetryConfig); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 48046bd22f..9e37154d1b 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -61,7 +61,7 @@ typedef struct master_t { uint8_t gyro_soft_type; // Gyro Filter Type uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz uint16_t gyro_soft_notch_hz; // Biquad gyro notch hz - uint16_t gyro_soft_notch_bw; // Biquad gyro notch quality + uint16_t gyro_soft_notch_cutoff; // Biquad gyro notch low cutoff uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4b19dbe18d..b6244aa5c9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -114,10 +114,8 @@ void initFilters(const pidProfile_t *pidProfile) { int axis; if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { - float octaves = log2f(((float) pidProfile->dterm_notch_hz + pidProfile->dterm_notch_bw / 2) / ((float) pidProfile->dterm_notch_hz - pidProfile->dterm_notch_bw / 2)); - float dtermNotchQ = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1); - - for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, dtermNotchQ, FILTER_NOTCH); + float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); + for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, notchQ, FILTER_NOTCH); } if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9316af5e01..9d158c3838 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -84,7 +84,7 @@ typedef struct pidProfile_s { uint16_t dterm_lpf_hz; // Delta Filter in hz uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy uint16_t dterm_notch_hz; // Biquad dterm notch hz - uint16_t dterm_notch_bw; // Biquad dterm notch quality + uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff uint8_t deltaMethod; // Alternative delta Calculation uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index edb7f43927..9d854f9321 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -751,7 +751,7 @@ const clivalue_t valueTable[] = { { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, { "gyro_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } }, - { "gyro_notch_bw", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_bw, .config.minmax = { 1, 500 } }, + { "gyro_notch_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff, .config.minmax = { 1, 500 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, @@ -824,7 +824,7 @@ const clivalue_t valueTable[] = { { "dterm_filter_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 500 } }, - { "dterm_notch_bw", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_bw, .config.minmax = { 1, 500 } }, + { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 500 } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } }, { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 7833ccc7c1..a17b61df20 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -55,16 +55,13 @@ static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; static float gyroDt; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_bw, uint8_t gyro_soft_lpf_type) +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_cutoff, uint8_t gyro_soft_lpf_type) { gyroConfig = gyroConfigToUse; gyroSoftLpfHz = gyro_soft_lpf_hz; gyroSoftNotchHz = gyro_soft_notch_hz; - gyroSoftNotchQ = gyro_soft_notch_bw; gyroSoftLpfType = gyro_soft_lpf_type; - - float octaves = log2f(((float) gyro_soft_notch_hz + gyro_soft_notch_bw / 2) / ((float) gyro_soft_notch_hz - gyro_soft_notch_bw / 2)); - gyroSoftNotchQ = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1); + gyroSoftNotchQ = filterGetNotchQ(gyro_soft_notch_hz, gyro_soft_notch_cutoff); } void gyroInit(void) @@ -75,7 +72,7 @@ void gyroInit(void) if (gyroSoftLpfType == FILTER_BIQUAD) biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); else - gyroDt = gyro.targetLooptime / 1000.0f; + gyroDt = (float) gyro.targetLooptime * 0.000001f; } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 385ae4a98c..bae1805e50 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -40,7 +40,7 @@ typedef struct gyroConfig_s { uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. } gyroConfig_t; -void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_bw, uint8_t gyro_soft_lpf_type); +void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_cutoff, uint8_t gyro_soft_lpf_type); void gyroSetCalibrationCycles(void); void gyroInit(void); void gyroUpdate(void); From b4e61d49c9e3087baf3232a3ef981f2d73bd9acd Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 00:50:11 +0200 Subject: [PATCH 176/456] More efficiency in PID code // add yaw velocity to setpoint --- src/main/config/config.c | 3 +- src/main/flight/pid.c | 68 ++++++++++++++++++++-------------------- src/main/flight/pid.h | 3 +- src/main/io/serial_cli.c | 5 ++- src/main/io/serial_msp.c | 4 +-- src/main/mw.c | 7 +++-- 6 files changed, 44 insertions(+), 46 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 991d6f5c74..c1ddd2d281 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -245,8 +245,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; pidProfile->dtermSetpointWeight = 200; - pidProfile->pidMaxVelocity = 1000; - pidProfile->pidMaxVelocityYaw = 50; + pidProfile->pidMaxVelocityYaw = 200; pidProfile->toleranceBand = 20; pidProfile->toleranceBandReduction = 40; pidProfile->zeroCrossAllowanceCount = 2; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b6244aa5c9..ae735be925 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -133,13 +133,10 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; float delta; int axis; float horizonLevelStrength = 1; - static int16_t axisPIDState[3]; - static float velocityWindupFactor[3] = { 1.0f, 1.0f, 1.0f }; - - const float velocityFactor = getdT() * 1000.0f; float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float @@ -180,13 +177,31 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { + static uint8_t configP[3], configI[3], configD[3]; + + // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now // Prepare all parameters for PID controller - float Kp = PTERM_SCALE * pidProfile->P8[axis]; - float Ki = ITERM_SCALE * pidProfile->I8[axis]; - float Kd = DTERM_SCALE * pidProfile->D8[axis]; - float b = pidProfile->ptermSetpointWeight / 100.0f; - float c = pidProfile->dtermSetpointWeight / 100.0f; - float velocityMax = (axis == YAW) ? (float)pidProfile->pidMaxVelocityYaw * velocityFactor : (float)pidProfile->pidMaxVelocity * velocityFactor; + if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { + Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; + Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; + Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; + b[axis] = pidProfile->ptermSetpointWeight / 100.0f; + c[axis] = pidProfile->dtermSetpointWeight / 100.0f; + yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); + + configP[axis] = pidProfile->P8[axis]; + configI[axis] = pidProfile->I8[axis]; + configD[axis] = pidProfile->D8[axis]; + } + + // Limit abrupt yaw inputs + if (axis == YAW && pidProfile->pidMaxVelocityYaw) { + float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; + if (ABS(yawCurrentVelocity) > yawMaxVelocity) { + setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; + } + yawPreviousRate = setpointRate[axis]; + } // Yaw control is GYRO based, direct sticks control is applied to rate PID if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { @@ -214,9 +229,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // ----- calculate error / angle rates ---------- - errorRate = setpointRate[axis] - PVRate; // r - y - rP = b * setpointRate[axis] - PVRate; // br - y - rD = c * setpointRate[axis] - PVRate; // cr - y + errorRate = setpointRate[axis] - PVRate; // r - y + rP = b[axis] * setpointRate[axis] - PVRate; // br - y // Slowly restore original setpoint with more stick input float diffRate = errorRate - rP; @@ -251,7 +265,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc } // -----calculate P component - PTerm = Kp * rP * dynReduction; + PTerm = Kp[axis] * rP * dynReduction; // -----calculate I component. // Reduce strong Iterm accumulation during higher stick inputs @@ -260,9 +274,9 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // Handle All windup Scenarios // limit maximum integrator value to prevent WindUp - float itermScaler = setpointRateScaler * kiThrottleGain * velocityWindupFactor[axis]; + float itermScaler = setpointRateScaler * kiThrottleGain; - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki * errorRate * getdT() * itermScaler, -250.0f, 250.0f); + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); // I coefficient (I8) moved before integration to make limiting independent from PID settings ITerm = errorGyroIf[axis]; @@ -275,13 +289,14 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc DTerm = 0.0f; // needed for blackbox } else { + rD = c[axis] * setpointRate[axis] - PVRate; // cr - y delta = rD - lastRateError[axis]; lastRateError[axis] = rD; // Divide delta by targetLooptime to get differential (ie dr/dt) delta *= (1.0f / getdT()); - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd * delta * dynReduction; + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; // Filter delta if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); @@ -294,30 +309,15 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc } } - DTerm = constrainf(Kd * delta * dynReduction, -300.0f, 300.0f); + DTerm = Kd[axis] * delta * dynReduction; // -----calculate total PID output - axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); + axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); } // Disable PID control at zero throttle if (!pidStabilisationEnabled) axisPID[axis] = 0; - // Velocity limit only active below 1000 - if (velocityMax < 1000) { - int16_t currentVelocity = axisPID[axis] - axisPIDState[axis]; - if (debugMode == DEBUG_VELOCITY) debug[axis] = currentVelocity; - if (ABS(currentVelocity) > velocityMax) { - axisPID[axis] = (currentVelocity > 0) ? axisPIDState[axis] + velocityMax : axisPIDState[axis] - velocityMax; - velocityWindupFactor[axis] = ABS(currentVelocity) / velocityMax; - } - else { - velocityWindupFactor[axis] = 1.0f; - } - - axisPIDState[axis] = axisPID[axis]; - } - #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { calculate_Gtune(axis); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9d158c3838..4f3623c319 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -100,8 +100,7 @@ typedef struct pidProfile_s { uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm uint8_t ptermSetpointWeight; // Setpoint weight for Pterm (lower means more PV tracking) uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) - uint16_t pidMaxVelocity; // velocity limiter for pid controller (per ms) - uint16_t pidMaxVelocityYaw; // velocity limiter for pid controller (per ms) yaw + uint16_t pidMaxVelocityYaw; // velocity limiter for pid controller deg/sec/ms #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9d854f9321..860874af7c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -832,9 +832,8 @@ const clivalue_t valueTable[] = { { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } }, - { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 200 } }, - { "pid_max_velocity", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocity, .config.minmax = {0, 2000 } }, - { "pid_max_velocity_yaw", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 2000 } }, + { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 300 } }, + { "max_yaw_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 1000 } }, { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 0098db8133..b5d170cfbc 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1268,8 +1268,8 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentProfile->pidProfile.toleranceBand); serialize8(currentProfile->pidProfile.toleranceBandReduction); serialize8(currentProfile->pidProfile.itermThrottleGain); - serialize16(currentProfile->pidProfile.pidMaxVelocity); serialize16(currentProfile->pidProfile.pidMaxVelocityYaw); + serialize16(1000); // Reserved for future break; case MSP_SENSOR_CONFIG: headSerialReply(3); @@ -1858,8 +1858,8 @@ static bool processInCommand(void) currentProfile->pidProfile.toleranceBand = read8(); currentProfile->pidProfile.toleranceBandReduction = read8(); currentProfile->pidProfile.itermThrottleGain = read8(); - currentProfile->pidProfile.pidMaxVelocity = read16(); currentProfile->pidProfile.pidMaxVelocityYaw = read16(); + read16(); // reserved for future purposes break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); diff --git a/src/main/mw.c b/src/main/mw.c index 073e795017..c3ebdf8299 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -240,7 +240,6 @@ void processRcCommand(void) debug[3] = rxRefreshRate; } - isRXDataNew = false; for (int channel=0; channel < 4; channel++) { deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); lastCommand[channel] = rcCommand[channel]; @@ -264,9 +263,11 @@ void processRcCommand(void) } if (readyToCalculateRate || isRXDataNew) { - isRXDataNew = false; + // Don't smooth yaw axis + int axisToCalculate = (isRXDataNew) ? 3 : 2; + for (int axis = 0; axis < axisToCalculate; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); - for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); + isRXDataNew = false; // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) From f4d623c5e771228d6f5924ef1416d508fc8ea07b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 01:31:18 +0200 Subject: [PATCH 177/456] Fix CC3D_OPBL by removing LED_STRIP --- src/main/target/CC3D/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 03d10272e4..3e86856965 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -116,6 +116,7 @@ #undef BARO #undef MAG #undef SONAR +#undef LED_STRIP #endif #define DEFAULT_RX_FEATURE FEATURE_RX_PPM From b581f4c39aed606c5cd382809a7d8912b5ef4a70 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 01:41:58 +0200 Subject: [PATCH 178/456] Revert "Used forward references to remove #includes from header files" --- src/main/blackbox/blackbox.c | 1 - src/main/blackbox/blackbox_io.c | 1 - src/main/config/config.c | 1 - src/main/drivers/bus_i2c.h | 4 ++-- src/main/drivers/exti.h | 2 +- src/main/drivers/sound_beeper.h | 2 +- src/main/flight/altitudehold.h | 16 +++++++--------- src/main/flight/imu.h | 3 +-- src/main/flight/pid.h | 1 + src/main/io/display.c | 2 -- src/main/io/rc_controls.h | 9 +++++---- src/main/io/rc_curves.c | 7 +++---- src/main/rx/rx.c | 4 ++-- src/main/rx/xbus.h | 2 ++ src/main/sensors/battery.c | 2 -- src/main/telemetry/frsky.h | 5 +++-- src/main/telemetry/hott.h | 3 +-- src/main/telemetry/ltm.h | 3 +-- src/main/telemetry/smartport.h | 3 +-- 19 files changed, 31 insertions(+), 40 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 96dedade74..a59cc8d78f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -72,7 +72,6 @@ #include "flight/altitudehold.h" #include "flight/failsafe.h" #include "flight/imu.h" -#include "flight/pid.h" #include "flight/navigation.h" #include "config/runtime_config.h" diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 88306767c6..cd4f05d980 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -53,7 +53,6 @@ #include "flight/altitudehold.h" #include "flight/failsafe.h" #include "flight/imu.h" -#include "flight/pid.h" #include "flight/navigation.h" #include "config/runtime_config.h" diff --git a/src/main/config/config.c b/src/main/config/config.c index 48ad7fa563..c1ddd2d281 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -23,7 +23,6 @@ #include "debug.h" #include "build_config.h" -#include "debug.h" #include "blackbox/blackbox_io.h" diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index d31d806225..86f7a66181 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -21,8 +21,8 @@ #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) #define I2C_DEFAULT_TIMEOUT I2C_SHORT_TIMEOUT -#include "io.h" -#include "rcc.h" +#include "drivers/io.h" +#include "drivers/rcc.h" #ifndef I2C_DEVICE #define I2C_DEVICE I2CINVALID diff --git a/src/main/drivers/exti.h b/src/main/drivers/exti.h index 713ec50aec..8071c65e33 100644 --- a/src/main/drivers/exti.h +++ b/src/main/drivers/exti.h @@ -18,7 +18,7 @@ #pragma once -#include "io.h" +#include "drivers/io.h" // old EXTI interface, to be replaced typedef struct extiConfig_s { diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index 4fad6422cc..ab7a7c3dfc 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -17,7 +17,7 @@ #pragma once -#include "io.h" +#include "drivers/io.h" #ifdef BEEPER #define BEEP_TOGGLE systemBeepToggle() diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index da16978814..3677fbb287 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -15,21 +15,19 @@ * along with Cleanflight. If not, see . */ -#pragma once +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "flight/pid.h" + +#include "sensors/barometer.h" extern int32_t AltHold; extern int32_t vario; void calculateEstimatedAltitude(uint32_t currentTime); -struct pidProfile_s; -struct barometerConfig_s; -struct rcControlsConfig_s; -struct escAndServoConfig_s; -void configureAltitudeHold(struct pidProfile_s *initialPidProfile, struct barometerConfig_s *intialBarometerConfig, struct rcControlsConfig_s *initialRcControlsConfig, struct escAndServoConfig_s *initialEscAndServoConfig); - -struct airplaneConfig_t; -void applyAltHold(struct airplaneConfig_s *airplaneConfig); +void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig); +void applyAltHold(airplaneConfig_t *airplaneConfig); void updateAltHoldState(void); void updateSonarAltHoldState(void); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index e5229a2ead..94b361fcf6 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -69,10 +69,9 @@ typedef struct accProcessor_s { accProcessorState_e state; } accProcessor_t; -struct pidProfile_s; void imuConfigure( imuRuntimeConfig_t *initialImuRuntimeConfig, - struct pidProfile_s *initialPidProfile, + pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband, uint16_t throttle_correction_angle ); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3adbe31671..4f3623c319 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -14,6 +14,7 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ +#include "rx/rx.h" #pragma once diff --git a/src/main/io/display.c b/src/main/io/display.c index a22ca88023..1e5eee9746 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -63,8 +63,6 @@ #include "io/display.h" -#include "rx/rx.h" - #include "scheduler/scheduler.h" extern profile_t *currentProfile; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index a3622aacab..aace839d9b 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -17,6 +17,8 @@ #pragma once +#include "rx/rx.h" + typedef enum { BOXARM = 0, BOXANGLE, @@ -165,9 +167,8 @@ typedef struct rcControlsConfig_s { bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); -struct rxConfig_s; -throttleStatus_e calculateThrottleStatus(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); -void processRcStickPositions(struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); +throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); +void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); @@ -256,7 +257,7 @@ bool isAirmodeActive(void); bool isSuperExpoActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); -void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig); +void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); bool isUsingSticksForArming(void); diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index 4a2b20bdde..89b46522f2 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -18,13 +18,12 @@ #include #include -#include "config/config.h" - -#include "io/escservo.h" #include "io/rc_controls.h" +#include "io/escservo.h" + #include "io/rc_curves.h" -#include "rx/rx.h" +#include "config/config.h" #define THROTTLE_LOOKUP_LENGTH 12 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index f5c0c9a9d7..a7a09a44b7 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -41,8 +41,6 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/system.h" - -#include "rx/rx.h" #include "rx/pwm.h" #include "rx/sbus.h" #include "rx/spektrum.h" @@ -53,6 +51,8 @@ #include "rx/ibus.h" #include "rx/jetiexbus.h" +#include "rx/rx.h" + //#define DEBUG_RX_SIGNAL_LOSS diff --git a/src/main/rx/xbus.h b/src/main/rx/xbus.h index 19dc6d5735..bc76f56786 100644 --- a/src/main/rx/xbus.h +++ b/src/main/rx/xbus.h @@ -17,5 +17,7 @@ #pragma once +#include "rx/rx.h" + bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); uint8_t xBusFrameStatus(void); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 6d165cbc57..75aac6cf0a 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -35,8 +35,6 @@ #include "io/rc_controls.h" #include "io/beeper.h" -#include "rx/rx.h" - #define VBATT_PRESENT_THRESHOLD_MV 10 #define VBATT_LPF_FREQ 0.4f diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index c057f22376..555eb43e83 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -17,13 +17,14 @@ #pragma once +#include "rx/rx.h" + typedef enum { FRSKY_VFAS_PRECISION_LOW = 0, FRSKY_VFAS_PRECISION_HIGH } frskyVFasPrecision_e; -struct rxConfig_s; -void handleFrSkyTelemetry(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); void checkFrSkyTelemetryState(void); void initFrSkyTelemetry(telemetryConfig_t *telemetryConfig); diff --git a/src/main/telemetry/hott.h b/src/main/telemetry/hott.h index 19d4c5369a..64a43bd507 100644 --- a/src/main/telemetry/hott.h +++ b/src/main/telemetry/hott.h @@ -488,8 +488,7 @@ typedef struct HOTT_AIRESC_MSG_s { void handleHoTTTelemetry(void); void checkHoTTTelemetryState(void); -struct telemetryConfig_s; -void initHoTTTelemetry(struct telemetryConfig_s *telemetryConfig); +void initHoTTTelemetry(telemetryConfig_t *telemetryConfig); void configureHoTTTelemetryPort(void); void freeHoTTTelemetryPort(void); diff --git a/src/main/telemetry/ltm.h b/src/main/telemetry/ltm.h index e5b4790c5a..6e4b80cbfa 100644 --- a/src/main/telemetry/ltm.h +++ b/src/main/telemetry/ltm.h @@ -19,8 +19,7 @@ #pragma once -struct telemetryConfig_s; -void initLtmTelemetry(struct telemetryConfig_s *initialTelemetryConfig); +void initLtmTelemetry(telemetryConfig_t *initialTelemetryConfig); void handleLtmTelemetry(void); void checkLtmTelemetryState(void); diff --git a/src/main/telemetry/smartport.h b/src/main/telemetry/smartport.h index 932ba1a929..b24be90708 100644 --- a/src/main/telemetry/smartport.h +++ b/src/main/telemetry/smartport.h @@ -7,8 +7,7 @@ #pragma once -struct telemetryConfig_s; -void initSmartPortTelemetry(struct telemetryConfig_s *); +void initSmartPortTelemetry(telemetryConfig_t *); void handleSmartPortTelemetry(void); void checkSmartPortTelemetryState(void); From c0c64deff57672d0ee48e46ffcc1e1ac7808d18a Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 01:51:02 +0200 Subject: [PATCH 179/456] More appropriate cli names for filter types --- src/main/io/serial_cli.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 860874af7c..dcf8828457 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -748,7 +748,7 @@ const clivalue_t valueTable[] = { { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, - { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, + { "gyro_lowpass_level", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } }, { "gyro_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } }, { "gyro_notch_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff, .config.minmax = { 1, 500 } }, @@ -821,7 +821,7 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, #endif - { "dterm_filter_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, + { "dterm_lowpass_level", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 500 } }, { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 500 } }, From 1f40cd664cd518c10ac658f2c815e771799869f0 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 01:37:59 +0200 Subject: [PATCH 180/456] Start on version 3.1.0 --- src/main/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/version.h b/src/main/version.h index 0f770674c1..c3c6529ed3 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,7 +16,7 @@ */ #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 0 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x From 2cabb9f3a0c19b4d94f04c9664df3a5872bf317a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 00:55:16 +0100 Subject: [PATCH 181/456] Optimisation of driver header files --- src/main/config/config.c | 1 + src/main/drivers/accgyro_mma845x.c | 2 +- src/main/drivers/adc.c | 2 + src/main/drivers/adc.h | 2 +- src/main/drivers/adc_impl.h | 6 +-- src/main/drivers/barometer_spi_bmp280.c | 2 + src/main/drivers/bus_i2c.h | 4 +- src/main/drivers/bus_i2c_soft.c | 2 +- src/main/drivers/bus_i2c_stm32f30x.c | 5 +- src/main/drivers/bus_spi.h | 5 +- src/main/drivers/compass_hmc5883l.h | 2 +- src/main/drivers/dma.c | 4 +- src/main/drivers/dma_stm32f4xx.c | 4 +- src/main/drivers/flash_m25p16.c | 7 +-- src/main/drivers/flash_m25p16.h | 2 +- src/main/drivers/gyro_sync.c | 6 +-- src/main/drivers/io.h | 24 +-------- src/main/drivers/io_types.h | 28 +++++++++++ src/main/drivers/light_ws2811strip.c | 4 +- .../drivers/light_ws2811strip_stm32f10x.c | 3 +- .../drivers/light_ws2811strip_stm32f30x.c | 2 +- src/main/drivers/max7456.c | 15 +++--- src/main/drivers/pwm_mapping.h | 5 +- src/main/drivers/pwm_rx.c | 2 +- src/main/drivers/rcc.h | 4 +- src/main/drivers/rcc_types.h | 4 ++ src/main/drivers/sdcard.c | 4 +- src/main/drivers/serial_usb_vcp.c | 4 +- src/main/drivers/sonar_hcsr04.h | 3 +- src/main/drivers/sound_beeper.h | 2 +- src/main/drivers/timer.c | 2 +- src/main/drivers/timer.h | 9 ++-- src/main/drivers/timer_stm32f10x.c | 5 ++ src/main/drivers/timer_stm32f30x.c | 5 ++ src/main/drivers/timer_stm32f4xx.c | 7 ++- src/main/drivers/transponder_ir.c | 6 +-- src/main/drivers/transponder_ir_stm32f30x.c | 6 +-- src/main/drivers/vtx_rtc6705.c | 6 +-- src/main/drivers/vtx_soft_spi_rtc6705.c | 7 +-- src/main/io/serial_4way_avrootloader.c | 2 + src/main/main.c | 1 + src/main/sensors/initialisation.c | 2 +- src/main/target/AIORACERF3/target.c | 1 + src/main/target/AIR32/target.c | 1 + src/main/target/ALIENFLIGHTF1/target.c | 1 + src/main/target/ALIENFLIGHTF1/target.h | 1 + src/main/target/ALIENFLIGHTF3/target.c | 1 + src/main/target/ALIENFLIGHTF3/target.h | 3 +- src/main/target/ALIENFLIGHTF4/target.c | 1 + src/main/target/BLUEJAYF4/target.c | 1 + src/main/target/CC3D/target.c | 1 + src/main/target/CHEBUZZF3/target.c | 1 + src/main/target/CJMCU/target.c | 1 + src/main/target/CJMCU/target.h | 1 + src/main/target/COLIBRI_RACE/target.c | 1 + src/main/target/DOGE/target.c | 1 + src/main/target/DOGE/target.h | 9 ++-- src/main/target/EUSTM32F103RC/target.c | 1 + src/main/target/EUSTM32F103RC/target.h | 1 + src/main/target/F4BY/target.c | 49 ++++++++++--------- src/main/target/F4BY/target.h | 3 +- src/main/target/FURYF3/target.c | 1 + src/main/target/FURYF4/target.c | 1 + src/main/target/FURYF4/target.h | 3 +- src/main/target/IRCFUSIONF3/target.c | 1 + src/main/target/IRCFUSIONF3/target.h | 3 +- src/main/target/KISSFC/target.c | 1 + src/main/target/KISSFC/target.h | 3 +- src/main/target/LUX_RACE/target.c | 1 + src/main/target/MICROSCISKY/target.c | 1 + src/main/target/MICROSCISKY/target.h | 1 + src/main/target/MOTOLAB/target.c | 1 + src/main/target/NAZE/target.c | 1 + src/main/target/NAZE/target.h | 1 + src/main/target/OLIMEXINO/target.c | 1 + src/main/target/OLIMEXINO/target.h | 1 + src/main/target/OMNIBUS/target.c | 1 + src/main/target/OMNIBUSF4/target.c | 1 + src/main/target/PIKOBLX/target.c | 1 + src/main/target/PIKOBLX/target.h | 3 +- src/main/target/PORT103R/target.c | 1 + src/main/target/PORT103R/target.h | 1 + src/main/target/REVO/target.c | 1 + src/main/target/REVONANO/target.c | 1 + src/main/target/RMDO/target.c | 1 + src/main/target/SINGULARITY/target.c | 1 + src/main/target/SINGULARITY/target.h | 3 +- src/main/target/SIRINFPV/target.c | 1 + src/main/target/SPARKY/target.c | 1 + src/main/target/SPRACINGF3/target.c | 1 + src/main/target/SPRACINGF3EVO/target.c | 1 + src/main/target/SPRACINGF3MINI/target.c | 1 + src/main/target/STM32F3DISCOVERY/target.c | 1 + src/main/target/STM32F3DISCOVERY/target.h | 1 + src/main/target/X_RACERSPI/target.c | 1 + src/main/target/X_RACERSPI/target.h | 3 +- src/main/target/ZCOREF3/target.c | 1 + src/main/target/ZCOREF3/target.h | 1 - src/main/telemetry/frsky.c | 2 + src/main/telemetry/telemetry.c | 2 +- 100 files changed, 212 insertions(+), 138 deletions(-) create mode 100644 src/main/drivers/io_types.h create mode 100644 src/main/drivers/rcc_types.h diff --git a/src/main/config/config.c b/src/main/config/config.c index 1c1b5d221d..ffff6578a8 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -22,6 +22,7 @@ #include "platform.h" #include "build_config.h" +#include "debug.h" #include "blackbox/blackbox_io.h" diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index 202b90e804..69a82a0adb 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -21,7 +21,7 @@ #include "platform.h" #include "system.h" -#include "gpio.h" +#include "io.h" #include "bus_i2c.h" #include "sensor.h" diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index a65abf9128..6bba109396 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -27,6 +27,8 @@ #include "adc.h" #include "adc_impl.h" +#include "common/utils.h" + //#define DEBUG_ADC_CHANNELS #ifdef USE_ADC diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 55914a3365..47a4b52a0e 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -17,7 +17,7 @@ #pragma once -#include "io.h" +#include "io_types.h" typedef enum { ADC_BATTERY = 0, diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 146ada62be..c89dc1bf53 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -17,8 +17,8 @@ #pragma once -#include "io.h" -#include "rcc.h" +#include "io_types.h" +#include "rcc_types.h" #if defined(STM32F4) #define ADC_TAG_MAP_COUNT 16 @@ -65,4 +65,4 @@ extern const adcTagMap_t adcTagMap[ADC_TAG_MAP_COUNT]; extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; -uint8_t adcChannelByTag(ioTag_t ioTag); \ No newline at end of file +uint8_t adcChannelByTag(ioTag_t ioTag); diff --git a/src/main/drivers/barometer_spi_bmp280.c b/src/main/drivers/barometer_spi_bmp280.c index 9d97343e11..0c97684972 100644 --- a/src/main/drivers/barometer_spi_bmp280.c +++ b/src/main/drivers/barometer_spi_bmp280.c @@ -15,6 +15,7 @@ * along with Betaflight. If not, see . */ +#include #include #include @@ -24,6 +25,7 @@ #include "barometer.h" #include "barometer_bmp280.h" +#include "io.h" #ifdef USE_BARO_SPI_BMP280 #define DISABLE_BMP280 IOHi(bmp280CsPin) diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index 86f7a66181..26cce3792f 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -21,8 +21,8 @@ #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) #define I2C_DEFAULT_TIMEOUT I2C_SHORT_TIMEOUT -#include "drivers/io.h" -#include "drivers/rcc.h" +#include "io_types.h" +#include "rcc_types.h" #ifndef I2C_DEVICE #define I2C_DEVICE I2CINVALID diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c index 104c77c379..42ea8cadc9 100644 --- a/src/main/drivers/bus_i2c_soft.c +++ b/src/main/drivers/bus_i2c_soft.c @@ -22,7 +22,7 @@ #include "build_config.h" #include "bus_i2c.h" -#include "drivers/io.h" +#include "io.h" // Software I2C driver, using same pins as hardware I2C, with hw i2c module disabled. // Can be configured for I2C2 pinout (SCL: PB10, SDA: PB11) or I2C1 pinout (SCL: PB6, SDA: PB7) diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 88b267f111..f8c5b23697 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -20,9 +20,10 @@ #include -#include "gpio.h" #include "system.h" -#include "drivers/io_impl.h" +#include "io.h" +#include "io_impl.h" +#include "rcc.h" #include "bus_i2c.h" diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 5a6ea7dd37..58c76c9d78 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -17,9 +17,8 @@ #pragma once -#include -#include "io.h" -#include "rcc.h" +#include "io_types.h" +#include "rcc_types.h" #if defined(STM32F4) || defined(STM32F3) #define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index 035a2c936f..e2f71fd28d 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -17,7 +17,7 @@ #pragma once -#include "io.h" +#include "io_types.h" typedef struct hmc5883Config_s { ioTag_t intTag; diff --git a/src/main/drivers/dma.c b/src/main/drivers/dma.c index 0ad411312e..7e4942fbe1 100644 --- a/src/main/drivers/dma.c +++ b/src/main/drivers/dma.c @@ -21,8 +21,8 @@ #include -#include "drivers/nvic.h" -#include "drivers/dma.h" +#include "nvic.h" +#include "dma.h" /* * DMA descriptors. diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index ec6e7908d3..8372ad0bcb 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -21,8 +21,8 @@ #include -#include "drivers/nvic.h" -#include "drivers/dma.h" +#include "nvic.h" +#include "dma.h" /* * DMA descriptors. diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 0ab3d6122b..bf7cf6f0cb 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -22,9 +22,10 @@ #ifdef USE_FLASH_M25P16 -#include "drivers/flash_m25p16.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "flash_m25p16.h" +#include "io.h" +#include "bus_spi.h" +#include "system.h" #define M25P16_INSTRUCTION_RDID 0x9F #define M25P16_INSTRUCTION_READ_BYTES 0x03 diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h index 223efa1809..6cad8a8ea6 100644 --- a/src/main/drivers/flash_m25p16.h +++ b/src/main/drivers/flash_m25p16.h @@ -19,7 +19,7 @@ #include #include "flash.h" -#include "io.h" +#include "io_types.h" #define M25P16_PAGESIZE 256 diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index cd95e186e1..dbdc2244c9 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -10,9 +10,9 @@ #include "platform.h" -#include "drivers/sensor.h" -#include "drivers/accgyro.h" -#include "drivers/gyro_sync.h" +#include "sensor.h" +#include "accgyro.h" +#include "gyro_sync.h" static uint8_t mpuDividerDrops; diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index d655646282..5afa4abb76 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -6,16 +6,7 @@ #include #include "resource.h" -// IO pin identification -// make sure that ioTag_t can't be assigned into IO_t without warning -typedef uint8_t ioTag_t; // packet tag to specify IO pin -typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change - -// NONE initializer for ioTag_t variables -#define IOTAG_NONE ((ioTag_t)0) - -// NONE initializer for IO_t variable -#define IO_NONE ((IO_t)0) +#include "io_types.h" // preprocessor is used to convert pinid to requested C data value // compile-time error is generated if requested pin is not available (not set in TARGET_IO_PORTx) @@ -24,19 +15,6 @@ typedef void* IO_t; // type specifying IO pin. Currently ioRec_t poin // expand pinid to to ioTag_t #define IO_TAG(pinid) DEFIO_TAG(pinid) -// both ioTag_t and IO_t are guarantied to be zero if pinid is NONE (no pin) -// this simplifies initialization (globals are zeroed on start) and allows -// omitting unused fields in structure initializers. -// it is also possible to use IO_t and ioTag_t as boolean value -// TODO - this may conflict with requirement to generate warning/error on IO_t - ioTag_t assignment -// IO_t being pointer is only possibility I know of .. - -// pin config handling -// pin config is packed into ioConfig_t to decrease memory requirements -// IOCFG_x macros are defined for common combinations for all CPUs; this -// helps masking CPU differences - -typedef uint8_t ioConfig_t; // packed IO configuration #if defined(STM32F1) // mode is using only bits 6-2 diff --git a/src/main/drivers/io_types.h b/src/main/drivers/io_types.h new file mode 100644 index 0000000000..ba80cd3bc7 --- /dev/null +++ b/src/main/drivers/io_types.h @@ -0,0 +1,28 @@ +#pragma once + +#include + +// IO pin identification +// make sure that ioTag_t can't be assigned into IO_t without warning +typedef uint8_t ioTag_t; // packet tag to specify IO pin +typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change + +// NONE initializer for ioTag_t variables +#define IOTAG_NONE ((ioTag_t)0) + +// NONE initializer for IO_t variable +#define IO_NONE ((IO_t)0) + +// both ioTag_t and IO_t are guarantied to be zero if pinid is NONE (no pin) +// this simplifies initialization (globals are zeroed on start) and allows +// omitting unused fields in structure initializers. +// it is also possible to use IO_t and ioTag_t as boolean value +// TODO - this may conflict with requirement to generate warning/error on IO_t - ioTag_t assignment +// IO_t being pointer is only possibility I know of .. + +// pin config handling +// pin config is packed into ioConfig_t to decrease memory requirements +// IOCFG_x macros are defined for common combinations for all CPUs; this +// helps masking CPU differences + +typedef uint8_t ioConfig_t; // packed IO configuration diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 6c8f2bb18a..d9c29af370 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -36,8 +36,8 @@ #include "common/color.h" #include "common/colorconversion.h" -#include "drivers/dma.h" -#include "drivers/light_ws2811strip.h" +#include "dma.h" +#include "light_ws2811strip.h" uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; volatile uint8_t ws2811LedDataTransferInProgress = 0; diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 4b588a0c9f..911b7cdc66 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -23,10 +23,11 @@ #ifdef LED_STRIP #include "common/color.h" -#include "drivers/light_ws2811strip.h" +#include "light_ws2811strip.h" #include "nvic.h" #include "io.h" #include "dma.h" +#include "rcc.h" #include "timer.h" static IO_t ws2811IO = IO_NONE; diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 8379188f99..1fadfb1f03 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -26,7 +26,7 @@ #include "nvic.h" #include "common/color.h" -#include "drivers/light_ws2811strip.h" +#include "light_ws2811strip.h" #include "dma.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 82f3f29af9..7ddc2a058b 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -19,16 +19,19 @@ #include #include "platform.h" -#include "version.h" #ifdef USE_MAX7456 +#include "version.h" + #include "common/printf.h" -#include "drivers/bus_spi.h" -#include "drivers/light_led.h" -#include "drivers/system.h" -#include "drivers/nvic.h" -#include "drivers/dma.h" + +#include "bus_spi.h" +#include "light_led.h" +#include "io.h" +#include "system.h" +#include "nvic.h" +#include "dma.h" #include "max7456.h" #include "max7456_symbols.h" diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 29a5d02f15..c017061835 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -17,7 +17,7 @@ #pragma once -#include "timer.h" +#include "io_types.h" #define MAX_PWM_MOTORS 12 #define MAX_PWM_SERVOS 8 @@ -88,10 +88,11 @@ typedef enum { PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4) } pwmPortFlags_e; +struct timerHardware_s; typedef struct pwmPortConfiguration_s { uint8_t index; pwmPortFlags_e flags; - const timerHardware_t *timerHardware; + const struct timerHardware_s *timerHardware; } pwmPortConfiguration_t; typedef struct pwmOutputConfiguration_s { diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 3c72d104dc..60af85e97b 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -30,7 +30,7 @@ #include "system.h" #include "nvic.h" -#include "gpio.h" +#include "io.h" #include "timer.h" #include "pwm_output.h" diff --git a/src/main/drivers/rcc.h b/src/main/drivers/rcc.h index fb04ec7e2c..69dce965f9 100644 --- a/src/main/drivers/rcc.h +++ b/src/main/drivers/rcc.h @@ -2,6 +2,7 @@ #include "platform.h" #include "common/utils.h" +#include "rcc_types.h" enum rcc_reg { RCC_EMPTY = 0, // make sure that default value (0) does not enable anything @@ -17,9 +18,6 @@ enum rcc_reg { #define RCC_APB1(periph) RCC_ENCODE(RCC_APB1, RCC_APB1ENR_ ## periph ## EN) #define RCC_AHB1(periph) RCC_ENCODE(RCC_AHB1, RCC_AHB1ENR_ ## periph ## EN) -typedef uint8_t rccPeriphTag_t; - void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState); void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState); - diff --git a/src/main/drivers/rcc_types.h b/src/main/drivers/rcc_types.h new file mode 100644 index 0000000000..1b7d7b9f2f --- /dev/null +++ b/src/main/drivers/rcc_types.h @@ -0,0 +1,4 @@ +#pragma once + +typedef uint8_t rccPeriphTag_t; + diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 878d5eacf6..a86fc74577 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -25,8 +25,8 @@ #include "nvic.h" #include "io.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "bus_spi.h" +#include "system.h" #include "sdcard.h" #include "sdcard_standard.h" diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 2c2cebbac6..50c6055ddd 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -22,7 +22,7 @@ #include "build_config.h" #include "common/utils.h" -#include "drivers/io.h" +#include "io.h" #include "usb_core.h" #ifdef STM32F4 @@ -32,7 +32,7 @@ #include "hw_config.h" #endif -#include "drivers/system.h" +#include "system.h" #include "serial.h" #include "serial_usb_vcp.h" diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h index d734f62f4f..cb3f38c989 100644 --- a/src/main/drivers/sonar_hcsr04.h +++ b/src/main/drivers/sonar_hcsr04.h @@ -17,8 +17,7 @@ #pragma once -#include "platform.h" -#include "io.h" +#include "io_types.h" typedef struct sonarHardware_s { ioTag_t triggerTag; diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index ab7a7c3dfc..9caad4f554 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -17,7 +17,7 @@ #pragma once -#include "drivers/io.h" +#include "io_types.h" #ifdef BEEPER #define BEEP_TOGGLE systemBeepToggle() diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 15e36cce3f..5051588c99 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -25,7 +25,7 @@ #include "nvic.h" -#include "gpio.h" +#include "io.h" #include "rcc.h" #include "system.h" diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 7050bbabda..00410a9c68 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -17,12 +17,11 @@ #pragma once -#include "io.h" -#include "rcc.h" +#include +#include -#if !defined(USABLE_TIMER_CHANNEL_COUNT) -#define USABLE_TIMER_CHANNEL_COUNT 14 -#endif +#include "io_types.h" +#include "rcc_types.h" typedef uint16_t captureCompare_t; // 16 bit on both 103 and 303, just register access must be 32bit sometimes (use timCCR_t) diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index 1a5085e63a..49106818f0 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -5,6 +5,11 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f10x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 04069b20c3..99111bad77 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -5,6 +5,11 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f30x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index af09f85e1b..ccce10350a 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -5,9 +5,14 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f4xx.h" -#include "timer.h" #include "rcc.h" +#include "timer.h" /** * @brief Selects the TIM Output Compare Mode. diff --git a/src/main/drivers/transponder_ir.c b/src/main/drivers/transponder_ir.c index 626c60097c..dbc6749af7 100644 --- a/src/main/drivers/transponder_ir.c +++ b/src/main/drivers/transponder_ir.c @@ -21,9 +21,9 @@ #include -#include "drivers/dma.h" -#include "drivers/nvic.h" -#include "drivers/transponder_ir.h" +#include "dma.h" +#include "nvic.h" +#include "transponder_ir.h" /* * Implementation note: diff --git a/src/main/drivers/transponder_ir_stm32f30x.c b/src/main/drivers/transponder_ir_stm32f30x.c index 3962e1c999..0bff2e112c 100644 --- a/src/main/drivers/transponder_ir_stm32f30x.c +++ b/src/main/drivers/transponder_ir_stm32f30x.c @@ -20,9 +20,9 @@ #include -#include "drivers/gpio.h" -#include "drivers/transponder_ir.h" -#include "drivers/nvic.h" +#include "gpio.h" +#include "transponder_ir.h" +#include "nvic.h" #ifndef TRANSPONDER_GPIO #define USE_TRANSPONDER_ON_DMA1_CHANNEL3 diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index 93ffa83ecb..08ec758e8d 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -31,9 +31,9 @@ #include "common/maths.h" -#include "drivers/vtx_rtc6705.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "vtx_rtc6705.h" +#include "bus_spi.h" +#include "system.h" #define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400 #define RTC6705_SET_A1 0x8F3031 //5865 diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.c b/src/main/drivers/vtx_soft_spi_rtc6705.c index 4ceba50df9..e6340680fa 100644 --- a/src/main/drivers/vtx_soft_spi_rtc6705.c +++ b/src/main/drivers/vtx_soft_spi_rtc6705.c @@ -22,9 +22,10 @@ #ifdef USE_RTC6705 -#include "drivers/bus_spi.h" -#include "drivers/system.h" -#include "drivers/light_led.h" +#include "bus_spi.h" +#include "io.h" +#include "system.h" +#include "light_led.h" #include "vtx_soft_spi_rtc6705.h" diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index f0f5525813..223d3e10cd 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -25,8 +25,10 @@ #include "platform.h" #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +#include "drivers/io_types.h" #include "drivers/system.h" #include "drivers/serial.h" +#include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "io/serial.h" #include "io/serial_msp.h" diff --git a/src/main/main.c b/src/main/main.c index 7ec478feae..10cc950d82 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -33,6 +33,7 @@ #include "drivers/system.h" #include "drivers/dma.h" #include "drivers/gpio.h" +#include "drivers/io.h" #include "drivers/light_led.h" #include "drivers/sound_beeper.h" #include "drivers/timer.h" diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index bb46059ac1..c3465b3934 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,7 +24,7 @@ #include "common/axis.h" -#include "drivers/gpio.h" +#include "drivers/io.h" #include "drivers/system.h" #include "drivers/exti.h" diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index e780364ec2..f950c67844 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index 378627daaf..b5b9889379 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c index 7116b53551..dec8739ba0 100644 --- a/src/main/target/ALIENFLIGHTF1/target.c +++ b/src/main/target/ALIENFLIGHTF1/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 6d945008f7..04984912cd 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -90,4 +90,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index c1946aa17c..8169e9e912 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 28a896ff01..d2e9e92ba4 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -35,8 +35,6 @@ #define BEEPER PA5 -#define USABLE_TIMER_CHANNEL_COUNT 11 - #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_EXTI @@ -131,5 +129,6 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 11 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index efedb56689..dafcdf86da 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index b85e50f539..83e0700cd5 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index a870de9e24..589c6af30d 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 631d83e6aa..c2aca46716 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index 0f8228559e..3562e3234d 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 973e0984f9..beb4be9657 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -71,4 +71,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 91b21c8b36..099f9b62e7 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index 05b642f088..ca13ef7d0b 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 2399ede8d1..3f40c4ab43 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -67,11 +67,6 @@ #define M25P16_CS_PIN PC15 #define M25P16_SPI_INSTANCE SPI2 -// timer definitions in drivers/timer.c -// channel mapping in drivers/pwm_mapping.c -// only 6 outputs available on hardware -#define USABLE_TIMER_CHANNEL_COUNT 9 - #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 @@ -147,5 +142,9 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +// timer definitions in drivers/timer.c +// channel mapping in drivers/pwm_mapping.c +// only 6 outputs available on hardware +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/EUSTM32F103RC/target.c b/src/main/target/EUSTM32F103RC/target.c index b2a0db9a16..0dc2d66044 100644 --- a/src/main/target/EUSTM32F103RC/target.c +++ b/src/main/target/EUSTM32F103RC/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 12ec85f371..d883641d4f 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -113,5 +113,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index a8d80e823d..e9befde0ab 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -3,17 +3,18 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM7 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed @@ -47,13 +48,13 @@ const uint16_t multiPWM[] = { const uint16_t airPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), PWM12 | (MAP_TO_SERVO_OUTPUT << 8), PWM13 | (MAP_TO_SERVO_OUTPUT << 8), PWM14 | (MAP_TO_SERVO_OUTPUT << 8), - PWM15 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed - PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM7 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed @@ -84,23 +85,23 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT - { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT - { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT + { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT + { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // sonar echo if needed }; diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 8b4b846c40..a922eac603 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -74,8 +74,6 @@ #define SDCARD_DMA_CHANNEL DMA_Channel_0 -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define USE_VCP #define VBUS_SENSING_PIN PA9 @@ -156,5 +154,6 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) |TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index bf4ff7b20a..db28609d97 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index ac600ea439..a53aa4d447 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 073e2fb916..9a6c89434c 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -101,8 +101,6 @@ #define M25P16_CS_PIN PB3 #define M25P16_SPI_INSTANCE SPI3 -#define USABLE_TIMER_CHANNEL_COUNT 5 - #define USE_VCP #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED @@ -167,5 +165,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 5 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index dd87a86410..2e4e12fb0a 100644 --- a/src/main/target/IRCFUSIONF3/target.c +++ b/src/main/target/IRCFUSIONF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 8dbd6e55ba..9ceabb35d2 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -23,8 +23,6 @@ #define LED0 PB3 -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready, no MAG #define USE_MPU_DATA_READY_SIGNAL @@ -105,5 +103,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 167265ed78..f105e9aaf7 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM7 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 05346145d6..e532b9d782 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -28,8 +28,6 @@ #define BEEPER PB13 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 12 - #define USE_EXTI #define MPU_INT_EXTI PB2 #define USE_MPU_DATA_READY_SIGNAL @@ -84,4 +82,5 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTF (BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 91b21c8b36..099f9b62e7 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/MICROSCISKY/target.c b/src/main/target/MICROSCISKY/target.c index 7116b53551..dec8739ba0 100644 --- a/src/main/target/MICROSCISKY/target.c +++ b/src/main/target/MICROSCISKY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 80d4f04856..9138f74e86 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -97,4 +97,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index a89063cc9a..cc898ba834 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index 7116b53551..dec8739ba0 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 0e8ba62be2..7e3705b47a 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -163,4 +163,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/OLIMEXINO/target.c b/src/main/target/OLIMEXINO/target.c index ea20267b3c..5d0a271c5b 100644 --- a/src/main/target/OLIMEXINO/target.c +++ b/src/main/target/OLIMEXINO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index bb270cb4fc..ae8ac3b38e 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -91,4 +91,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 977d623f85..a3cd3e54d3 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 7dd021055e..2ed3eab8fb 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index a89063cc9a..cc898ba834 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 111f6fc476..7a104b7466 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -28,8 +28,6 @@ #define BEEPER PA0 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 9 - // MPU6000 interrupts #define USE_EXTI #define MPU_INT_EXTI PA15 @@ -136,5 +134,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/PORT103R/target.c b/src/main/target/PORT103R/target.c index 8f21b7cbb7..093fa6ee27 100644 --- a/src/main/target/PORT103R/target.c +++ b/src/main/target/PORT103R/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 66e83fa3d5..9f0239d572 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -123,5 +123,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 67f7327285..a253a0b31f 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index 9a579f2bc9..bf5023fc24 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index ee96272c11..d344cda357 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index 8131579519..e48d0c33b4 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 09a5678174..ed23489a3a 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -25,8 +25,6 @@ #define BEEPER PC15 -#define USABLE_TIMER_CHANNEL_COUNT 10 - #define USE_EXTI #define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL @@ -115,5 +113,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index b52fbd00e4..7d602c74b1 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM7 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index 389617278b..20b1f185d3 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index ec9176a27f..99f79ec9b7 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 843ad3d9aa..87eb5db35f 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index d1e3a0d8b2..bf8776aa92 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index 1712bebae2..d2c17db5a2 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 592833bc48..a75e952171 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -165,5 +165,6 @@ #define TARGET_IO_PORTE 0xffff #define TARGET_IO_PORTF 0x00ff +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index 85db79093c..a63258100e 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -5,6 +5,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 7838b1f460..9cc822ebee 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -26,8 +26,6 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH @@ -128,5 +126,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/ZCOREF3/target.c b/src/main/target/ZCOREF3/target.c index b72400e6fd..48a6ce60da 100644 --- a/src/main/target/ZCOREF3/target.c +++ b/src/main/target/ZCOREF3/target.c @@ -5,6 +5,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h index c65abb7d92..5c07c78190 100644 --- a/src/main/target/ZCOREF3/target.h +++ b/src/main/target/ZCOREF3/target.h @@ -101,6 +101,5 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 17 // PPM, 8 PWM, UART3 RX/TX, LED Strip - #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 33bd13193c..d698b958a0 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -19,6 +19,8 @@ * Initial FrSky Telemetry implementation by silpstream @ rcgroups. * Addition protocol work by airmamaf @ github. */ + +#include #include #include diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 96e2193e9a..6373d166eb 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#include #include #include @@ -22,7 +23,6 @@ #ifdef TELEMETRY -#include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" From f0ec4b487471a3c6882f44415b5258e7b4d15b4a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 01:10:46 +0100 Subject: [PATCH 182/456] Added #pragma once to debug.h --- src/main/debug.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/debug.h b/src/main/debug.h index 2e2a90e89c..6ebbba735a 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -15,6 +15,8 @@ * along with Cleanflight. If not, see . */ +#pragma once + #define DEBUG16_VALUE_COUNT 4 extern int16_t debug[DEBUG16_VALUE_COUNT]; extern uint8_t debugMode; From 9c420ea212de200869442b96839205bb8ff0004c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 09:02:31 +0100 Subject: [PATCH 183/456] Put each PID in its own .c file --- Makefile | 2 + src/main/flight/pid.c | 384 +------------------------------ src/main/flight/pid.h | 5 + src/main/flight/pid_betaflight.c | 277 ++++++++++++++++++++++ src/main/flight/pid_legacy.c | 210 +++++++++++++++++ 5 files changed, 506 insertions(+), 372 deletions(-) create mode 100644 src/main/flight/pid_betaflight.c create mode 100644 src/main/flight/pid_legacy.c diff --git a/Makefile b/Makefile index 281b169c63..68364d7b7c 100644 --- a/Makefile +++ b/Makefile @@ -392,6 +392,8 @@ COMMON_SRC = \ flight/imu.c \ flight/mixer.c \ flight/pid.c \ + flight/pid_legacy.c \ + flight/pid_betaflight.c \ io/beeper.c \ io/rc_controls.c \ io/rc_curves.c \ diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ae735be925..5ca8e312a7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -47,12 +47,9 @@ #include "config/runtime_config.h" -extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float setpointRate[3]; -extern float rcInput[3]; -static bool pidStabilisationEnabled; +bool pidStabilisationEnabled; int16_t axisPID[3]; @@ -60,19 +57,12 @@ int16_t axisPID[3]; int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; +int32_t errorGyroI[3]; +float errorGyroIf[3]; -static int32_t errorGyroI[3]; -static float errorGyroIf[3]; - -static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); #ifdef SKIP_PID_FLOAT pidControllerFuncPtr pid_controller = pidLegacy; // which pid controller are we using #else -static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using #endif @@ -94,7 +84,7 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false; } -float getdT (void) +float getdT(void) { static float dT; if (!dT) dT = (float)targetPidLooptime * 0.000001f; @@ -104,13 +94,15 @@ float getdT (void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static pt1Filter_t deltaFilter[3]; -static pt1Filter_t yawFilter; -static biquadFilter_t dtermFilterLpf[3]; -static biquadFilter_t dtermFilterNotch[3]; -static bool dtermNotchInitialised, dtermBiquadLpfInitialised; +pt1Filter_t deltaFilter[3]; +pt1Filter_t yawFilter; +biquadFilter_t dtermFilterLpf[3]; +biquadFilter_t dtermFilterNotch[3]; +bool dtermNotchInitialised; +bool dtermBiquadLpfInitialised; -void initFilters(const pidProfile_t *pidProfile) { +void initFilters(const pidProfile_t *pidProfile) +{ int axis; if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { @@ -125,358 +117,6 @@ void initFilters(const pidProfile_t *pidProfile) { } } -#ifndef SKIP_PID_FLOAT -// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab) -static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - float errorRate = 0, rP = 0, rD = 0, PVRate = 0; - float ITerm,PTerm,DTerm; - static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; - float delta; - int axis; - float horizonLevelStrength = 1; - - float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection - if(pidProfile->D8[PIDLEVEL] == 0){ - horizonLevelStrength = 0; - } else { - horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); - } - } - - // Yet Highly experimental and under test and development - // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) - static float kiThrottleGain = 1.0f; - if (pidProfile->itermThrottleGain) { - const uint16_t maxLoopCount = 20000 / targetPidLooptime; - const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; - static int16_t previousThrottle; - static uint16_t loopIncrement; - - if (loopIncrement >= maxLoopCount) { - kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 - previousThrottle = rcCommand[THROTTLE]; - loopIncrement = 0; - } else { - loopIncrement++; - } - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - static uint8_t configP[3], configI[3], configD[3]; - - // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now - // Prepare all parameters for PID controller - if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { - Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; - Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; - Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; - b[axis] = pidProfile->ptermSetpointWeight / 100.0f; - c[axis] = pidProfile->dtermSetpointWeight / 100.0f; - yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); - - configP[axis] = pidProfile->P8[axis]; - configI[axis] = pidProfile->I8[axis]; - configD[axis] = pidProfile->D8[axis]; - } - - // Limit abrupt yaw inputs - if (axis == YAW && pidProfile->pidMaxVelocityYaw) { - float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; - if (ABS(yawCurrentVelocity) > yawMaxVelocity) { - setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; - } - yawPreviousRate = setpointRate[axis]; - } - - // Yaw control is GYRO based, direct sticks control is applied to rate PID - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to the max inclination -#ifdef GPS - const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#else - const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; - } else { - // HORIZON mode - direct sticks control is applied to rate PID - // mix up angle error to desired AngleRate to add a little auto-level feel - setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); - } - } - - PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec - - // --------low-level gyro-based PID based on 2DOF PID controller. ---------- - // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // ----- calculate error / angle rates ---------- - errorRate = setpointRate[axis] - PVRate; // r - y - rP = b[axis] * setpointRate[axis] - PVRate; // br - y - - // Slowly restore original setpoint with more stick input - float diffRate = errorRate - rP; - rP += diffRate * rcInput[axis]; - - // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount - float dynReduction = tpaFactor; - if (pidProfile->toleranceBand) { - const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; - static uint8_t zeroCrossCount[3]; - static uint8_t currentErrorPolarity[3]; - if (ABS(errorRate) < pidProfile->toleranceBand) { - if (zeroCrossCount[axis]) { - if (currentErrorPolarity[axis] == POSITIVE_ERROR) { - if (errorRate < 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = NEGATIVE_ERROR; - } - } else { - if (errorRate > 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = POSITIVE_ERROR; - } - } - } else { - dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); - } - } else { - zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; - currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; - } - } - - // -----calculate P component - PTerm = Kp[axis] * rP * dynReduction; - - // -----calculate I component. - // Reduce strong Iterm accumulation during higher stick inputs - float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); - - // Handle All windup Scenarios - // limit maximum integrator value to prevent WindUp - float itermScaler = setpointRateScaler * kiThrottleGain; - - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); - - // I coefficient (I8) moved before integration to make limiting independent from PID settings - ITerm = errorGyroIf[axis]; - - //-----calculate D-term (Yaw D not yet supported) - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = lrintf(PTerm + ITerm); - - DTerm = 0.0f; // needed for blackbox - } else { - rD = c[axis] * setpointRate[axis] - PVRate; // cr - y - delta = rD - lastRateError[axis]; - lastRateError[axis] = rD; - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta *= (1.0f / getdT()); - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; - - // Filter delta - if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); - - if (pidProfile->dterm_lpf_hz) { - if (dtermBiquadLpfInitialised) { - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } - } - - DTerm = Kd[axis] * delta * dynReduction; - - // -----calculate total PID output - axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); - } - - // Disable PID control at zero throttle - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} -#endif - -// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future -static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - int axis; - int32_t PTerm, ITerm, DTerm, delta; - static int32_t lastRateError[3]; - int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; - - int8_t horizonLevelStrength = 100; - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection - // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. - // For more rate mode increase D and slower flips and rolls will be possible - horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - // -----Get the desired angle rate depending on flight mode - AngleRateTmp = (int32_t)setpointRate[axis]; - - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to max configured inclination -#ifdef GPS - const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#else - const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; - } else { - // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, - // horizonLevelStrength is scaled to the stick input - AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); - } - } - - // --------low-level gyro-based PID. ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // -----calculate scaled error.AngleRates - // multiplication of rcCommand corresponds to changing the sticks scaling here - gyroRate = gyroADC[axis] / 4; - - RateError = AngleRateTmp - gyroRate; - - // -----calculate P component - PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; - - // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply - if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { - PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); - } - - // -----calculate I component - // there should be no division before accumulating the error to integrator, because the precision would be reduced. - // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. - // Time correction (to avoid different I scaling for different builds based on average cycle time) - // is normalized to cycle time = 2048. - // Prevent Accumulation - uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); - uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - - errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; - - // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. - // I coefficient (I8) moved before integration to make limiting independent from PID settings - errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - - ITerm = errorGyroI[axis] >> 13; - - //-----calculate D-term - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = PTerm + ITerm; - - if (motorCount >= 4) { - int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); - - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); - } - - DTerm = 0; // needed for blackbox - } else { - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastRateError[axis]; - lastRateError[axis] = RateError; - } else { - delta = -(gyroRate - lastRateError[axis]); - lastRateError[axis] = gyroRate; - } - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // Filter delta - if (pidProfile->dterm_lpf_hz) { - float deltaf = delta; // single conversion - if (dtermBiquadLpfInitialised) { - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } - delta = lrintf(deltaf); - } - - DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // -----calculate total PID output - axisPID[axis] = PTerm + ITerm + DTerm; - } - - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} void pidSetController(pidControllerType_e type) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4f3623c319..79e83a2c04 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -117,6 +117,11 @@ struct rxConfig_s; typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype +void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); +void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); + extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool airmodeWasActivated; diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c new file mode 100644 index 0000000000..72cdd4ea4c --- /dev/null +++ b/src/main/flight/pid_betaflight.c @@ -0,0 +1,277 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#ifndef SKIP_PID_FLOAT + +#include "build_config.h" +#include "debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" + +#include "drivers/accgyro.h" +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/gtune.h" + +#include "config/runtime_config.h" + +extern float rcInput[3]; +extern float setpointRate[3]; + +extern float errorGyroIf[3]; +extern bool pidStabilisationEnabled; + +extern pt1Filter_t deltaFilter[3]; +extern pt1Filter_t yawFilter; +extern biquadFilter_t dtermFilterLpf[3]; +extern biquadFilter_t dtermFilterNotch[3]; +extern bool dtermNotchInitialised; +extern bool dtermBiquadLpfInitialised; + +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + +void initFilters(const pidProfile_t *pidProfile); +float getdT(void); + +// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. +// Based on 2DOF reference design (matlab) +void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) +{ + float errorRate = 0, rP = 0, rD = 0, PVRate = 0; + float ITerm,PTerm,DTerm; + static float lastRateError[2]; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; + float delta; + int axis; + float horizonLevelStrength = 1; + + float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection + if(pidProfile->D8[PIDLEVEL] == 0){ + horizonLevelStrength = 0; + } else { + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); + } + } + + // Yet Highly experimental and under test and development + // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) + static float kiThrottleGain = 1.0f; + if (pidProfile->itermThrottleGain) { + const uint16_t maxLoopCount = 20000 / targetPidLooptime; + const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; + static int16_t previousThrottle; + static uint16_t loopIncrement; + + if (loopIncrement >= maxLoopCount) { + kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 + previousThrottle = rcCommand[THROTTLE]; + loopIncrement = 0; + } else { + loopIncrement++; + } + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + static uint8_t configP[3], configI[3], configD[3]; + + // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now + // Prepare all parameters for PID controller + if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { + Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; + Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; + Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; + b[axis] = pidProfile->ptermSetpointWeight / 100.0f; + c[axis] = pidProfile->dtermSetpointWeight / 100.0f; + yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); + + configP[axis] = pidProfile->P8[axis]; + configI[axis] = pidProfile->I8[axis]; + configD[axis] = pidProfile->D8[axis]; + } + + // Limit abrupt yaw inputs + if (axis == YAW && pidProfile->pidMaxVelocityYaw) { + float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; + if (ABS(yawCurrentVelocity) > yawMaxVelocity) { + setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; + } + yawPreviousRate = setpointRate[axis]; + } + + // Yaw control is GYRO based, direct sticks control is applied to rate PID + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to the max inclination +#ifdef GPS + const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#else + const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; + } else { + // HORIZON mode - direct sticks control is applied to rate PID + // mix up angle error to desired AngleRate to add a little auto-level feel + setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); + } + } + + PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec + + // --------low-level gyro-based PID based on 2DOF PID controller. ---------- + // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // ----- calculate error / angle rates ---------- + errorRate = setpointRate[axis] - PVRate; // r - y + rP = b[axis] * setpointRate[axis] - PVRate; // br - y + + // Slowly restore original setpoint with more stick input + float diffRate = errorRate - rP; + rP += diffRate * rcInput[axis]; + + // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount + float dynReduction = tpaFactor; + if (pidProfile->toleranceBand) { + const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; + static uint8_t zeroCrossCount[3]; + static uint8_t currentErrorPolarity[3]; + if (ABS(errorRate) < pidProfile->toleranceBand) { + if (zeroCrossCount[axis]) { + if (currentErrorPolarity[axis] == POSITIVE_ERROR) { + if (errorRate < 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = NEGATIVE_ERROR; + } + } else { + if (errorRate > 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = POSITIVE_ERROR; + } + } + } else { + dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); + } + } else { + zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; + currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; + } + } + + // -----calculate P component + PTerm = Kp[axis] * rP * dynReduction; + + // -----calculate I component. + // Reduce strong Iterm accumulation during higher stick inputs + float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); + + // Handle All windup Scenarios + // limit maximum integrator value to prevent WindUp + float itermScaler = setpointRateScaler * kiThrottleGain; + + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); + + // I coefficient (I8) moved before integration to make limiting independent from PID settings + ITerm = errorGyroIf[axis]; + + //-----calculate D-term (Yaw D not yet supported) + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = lrintf(PTerm + ITerm); + + DTerm = 0.0f; // needed for blackbox + } else { + rD = c[axis] * setpointRate[axis] - PVRate; // cr - y + delta = rD - lastRateError[axis]; + lastRateError[axis] = rD; + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta *= (1.0f / getdT()); + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; + + // Filter delta + if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); + + if (pidProfile->dterm_lpf_hz) { + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + } + + DTerm = Kd[axis] * delta * dynReduction; + + // -----calculate total PID output + axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); + } + + // Disable PID control at zero throttle + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif + } +} +#endif + diff --git a/src/main/flight/pid_legacy.c b/src/main/flight/pid_legacy.c new file mode 100644 index 0000000000..0424f5461f --- /dev/null +++ b/src/main/flight/pid_legacy.c @@ -0,0 +1,210 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#include "build_config.h" +#include "debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" + +#include "drivers/accgyro.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/gtune.h" + +#include "config/runtime_config.h" + +extern uint8_t motorCount; +extern uint8_t PIDweight[3]; +extern bool pidStabilisationEnabled; +extern float setpointRate[3]; +extern int32_t errorGyroI[3]; +extern pt1Filter_t deltaFilter[3]; +extern pt1Filter_t yawFilter; +extern biquadFilter_t dtermFilterLpf[3]; +extern bool dtermBiquadLpfInitialised; + + +void initFilters(const pidProfile_t *pidProfile); +float getdT(void); + + +// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. +// Don't expect much development in the future +void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) +{ + int axis; + int32_t PTerm, ITerm, DTerm, delta; + static int32_t lastRateError[3]; + int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; + + int8_t horizonLevelStrength = 100; + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection + // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. + // For more rate mode increase D and slower flips and rolls will be possible + horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + // -----Get the desired angle rate depending on flight mode + AngleRateTmp = (int32_t)setpointRate[axis]; + + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to max configured inclination +#ifdef GPS + const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +#else + const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; + } else { + // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, + // horizonLevelStrength is scaled to the stick input + AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); + } + } + + // --------low-level gyro-based PID. ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // -----calculate scaled error.AngleRates + // multiplication of rcCommand corresponds to changing the sticks scaling here + gyroRate = gyroADC[axis] / 4; + + RateError = AngleRateTmp - gyroRate; + + // -----calculate P component + PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + + // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply + if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { + PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); + } + + // -----calculate I component + // there should be no division before accumulating the error to integrator, because the precision would be reduced. + // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. + // Time correction (to avoid different I scaling for different builds based on average cycle time) + // is normalized to cycle time = 2048. + // Prevent Accumulation + uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); + uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; + + errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; + + // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. + // I coefficient (I8) moved before integration to make limiting independent from PID settings + errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); + + ITerm = errorGyroI[axis] >> 13; + + //-----calculate D-term + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = PTerm + ITerm; + + if (motorCount >= 4) { + int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); + + // prevent "yaw jump" during yaw correction + axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); + } + + DTerm = 0; // needed for blackbox + } else { + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = RateError - lastRateError[axis]; + lastRateError[axis] = RateError; + } else { + delta = -(gyroRate - lastRateError[axis]); + lastRateError[axis] = gyroRate; + } + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // Filter delta + if (pidProfile->dterm_lpf_hz) { + float deltaf = delta; // single conversion + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + delta = lrintf(deltaf); + } + + DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // -----calculate total PID output + axisPID[axis] = PTerm + ITerm + DTerm; + } + + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif + } +} + From 45f2ce04d54f32257bd4a50481bfadc415df726e Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 10:12:31 +0200 Subject: [PATCH 184/456] Just the defaults --- src/main/config/config.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index c1ddd2d281..bb177b2f41 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -203,10 +203,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[ROLL] = 45; pidProfile->I8[ROLL] = 40; - pidProfile->D8[ROLL] = 18; - pidProfile->P8[PITCH] = 50; - pidProfile->I8[PITCH] = 55; - pidProfile->D8[PITCH] = 22; + pidProfile->D8[ROLL] = 25; + pidProfile->P8[PITCH] = 60; + pidProfile->I8[PITCH] = 60; + pidProfile->D8[PITCH] = 30; pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; @@ -234,7 +234,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_lpf_hz = 80; pidProfile->rollPitchItermIgnoreRate = 200; pidProfile->yawItermIgnoreRate = 50; - pidProfile->dterm_filter_type = FILTER_PT1; + pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 0; pidProfile->dterm_notch_cutoff = 150; @@ -479,7 +479,7 @@ static void resetConf(void) masterConfig.pid_process_denom = 2; #endif masterConfig.gyro_soft_type = FILTER_PT1; - masterConfig.gyro_soft_lpf_hz = 80; + masterConfig.gyro_soft_lpf_hz = 100; masterConfig.gyro_soft_notch_hz = 0; masterConfig.gyro_soft_notch_cutoff = 150; From 49a521d17c36cac872d443d07d8991b68a0de9e5 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 10:12:31 +0200 Subject: [PATCH 185/456] Just the defaults --- src/main/config/config.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index c1ddd2d281..bb177b2f41 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -203,10 +203,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[ROLL] = 45; pidProfile->I8[ROLL] = 40; - pidProfile->D8[ROLL] = 18; - pidProfile->P8[PITCH] = 50; - pidProfile->I8[PITCH] = 55; - pidProfile->D8[PITCH] = 22; + pidProfile->D8[ROLL] = 25; + pidProfile->P8[PITCH] = 60; + pidProfile->I8[PITCH] = 60; + pidProfile->D8[PITCH] = 30; pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; @@ -234,7 +234,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_lpf_hz = 80; pidProfile->rollPitchItermIgnoreRate = 200; pidProfile->yawItermIgnoreRate = 50; - pidProfile->dterm_filter_type = FILTER_PT1; + pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 0; pidProfile->dterm_notch_cutoff = 150; @@ -479,7 +479,7 @@ static void resetConf(void) masterConfig.pid_process_denom = 2; #endif masterConfig.gyro_soft_type = FILTER_PT1; - masterConfig.gyro_soft_lpf_hz = 80; + masterConfig.gyro_soft_lpf_hz = 100; masterConfig.gyro_soft_notch_hz = 0; masterConfig.gyro_soft_notch_cutoff = 150; From fed2912f7bd8eeac5d4e9d572d8a85936b06f022 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 11:46:55 +0200 Subject: [PATCH 186/456] Cleanup ONESHOT125 feature // Default denom --- src/main/config/config.c | 3 + src/main/config/config.h | 9 ++- src/main/io/serial_cli.c | 5 +- src/main/main.c | 6 -- src/main/target/ALIENFLIGHTF1/config.c | 1 - src/main/target/ALIENFLIGHTF3/config.c | 1 - src/main/target/ALIENFLIGHTF4/config.c | 1 - src/main/target/F4BY/target.h | 2 +- src/main/target/FURYF4/target.h | 2 +- src/main/target/MOTOLAB/config.c | 82 ++++++++++++++++++++++++++ src/main/target/OMNIBUS/target.h | 2 +- src/main/target/OMNIBUSF4/target.h | 2 +- src/main/target/REVO/target.h | 2 +- src/main/target/VRRACE/target.h | 2 +- 14 files changed, 97 insertions(+), 23 deletions(-) create mode 100755 src/main/target/MOTOLAB/config.c diff --git a/src/main/config/config.c b/src/main/config/config.c index bb177b2f41..3b03fb23b6 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -474,6 +474,9 @@ static void resetConf(void) #ifdef STM32F10X masterConfig.gyro_sync_denom = 8; masterConfig.pid_process_denom = 1; +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) + masterConfig.gyro_sync_denom = 8; + masterConfig.pid_process_denom = 4; #else masterConfig.gyro_sync_denom = 4; masterConfig.pid_process_denom = 2; diff --git a/src/main/config/config.h b/src/main/config/config.h index b97ac6b45b..cc239da923 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -46,16 +46,15 @@ typedef enum { FEATURE_RSSI_ADC = 1 << 15, FEATURE_LED_STRIP = 1 << 16, FEATURE_DISPLAY = 1 << 17, - FEATURE_ONESHOT125 = 1 << 18, + FEATURE_OSD = 1 << 18, FEATURE_BLACKBOX = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_TRANSPONDER = 1 << 21, FEATURE_AIRMODE = 1 << 22, FEATURE_SUPEREXPO_RATES = 1 << 23, - FEATURE_OSD = 1 << 24, - FEATURE_VTX = 1 << 25, - FEATURE_RX_NRF24 = 1 << 26, - FEATURE_SOFTSPI = 1 << 27, + FEATURE_VTX = 1 << 24, + FEATURE_RX_NRF24 = 1 << 25, + FEATURE_SOFTSPI = 1 << 26, } features_e; void latchActiveFeatures(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index dcf8828457..5aef0f5392 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -206,9 +206,8 @@ static const char * const featureNames[] = { "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP", "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", - "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", - "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO_RATES", - "OSD", NULL + "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD", + "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO_RATES", NULL }; // sync this with rxFailsafeChannelMode_e diff --git a/src/main/main.c b/src/main/main.c index 7ec478feae..24b983308d 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -288,12 +288,6 @@ void init(void) pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif - if (masterConfig.motor_pwm_protocol == PWM_TYPE_ONESHOT125) { - featureSet(FEATURE_ONESHOT125); - } else { - featureClear(FEATURE_ONESHOT125); - } - bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL || masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED; // Configurator feature abused for enabling Fast PWM diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index 3369b17635..b16a16444a 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -72,7 +72,6 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { - featureClear(FEATURE_ONESHOT125); masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; masterConfig.motor_pwm_rate = 32000; diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index be0a9bfea3..3aed5a3ceb 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -77,7 +77,6 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { - featureClear(FEATURE_ONESHOT125); masterConfig.mag_hardware = MAG_NONE; // disabled by default masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index ecce870a48..5264a16735 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -77,7 +77,6 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { - featureClear(FEATURE_ONESHOT125); masterConfig.mag_hardware = MAG_NONE; // disabled by default masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 8b4b846c40..7c9e89b48b 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -138,7 +138,7 @@ #define CURRENT_METER_ADC_PIN PC2 #define RSSI_ADC_PIN PC1 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 073e2fb916..8f69378592 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -150,7 +150,7 @@ #define RSSI_ADC_GPIO_PIN PC2 #define CURRENT_METER_ADC_PIN PC3 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/MOTOLAB/config.c b/src/main/target/MOTOLAB/config.c new file mode 100755 index 0000000000..490236ce04 --- /dev/null +++ b/src/main/target/MOTOLAB/config.c @@ -0,0 +1,82 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include + +#include "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +// Motolab target supports 2 different type of boards Tornado / Cyclone. +void targetConfiguration(void) { + masterConfig.gyro_sync_denom = 4; + masterConfig.pid_process_denom = 1; +} diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 0a0ec2593d..ad84573b59 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -167,7 +167,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX) #define BUTTONS #define BUTTON_A_PORT GPIOB diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 5250a336b7..4bd45103a3 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -121,7 +121,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125 | FEATURE_RX_SERIAL) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL) #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 9aab5b4b10..76738118d0 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -110,7 +110,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index da1581e9f3..cda9b14463 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -156,7 +156,7 @@ #define RSSI_ADC_GPIO_PIN PB1 #define CURRENT_METER_ADC_PIN PA5 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_ONESHOT125 | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS From 7d119ae669caceecdf9375b97946c2cc9e991adf Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 11:46:55 +0200 Subject: [PATCH 187/456] Cleanup ONESHOT125 feature // Default denom --- src/main/config/config.c | 3 + src/main/config/config.h | 9 ++- src/main/io/serial_cli.c | 5 +- src/main/main.c | 6 -- src/main/target/ALIENFLIGHTF1/config.c | 1 - src/main/target/ALIENFLIGHTF3/config.c | 1 - src/main/target/ALIENFLIGHTF4/config.c | 1 - src/main/target/F4BY/target.h | 2 +- src/main/target/FURYF4/target.h | 2 +- src/main/target/MOTOLAB/config.c | 82 ++++++++++++++++++++++++++ src/main/target/OMNIBUS/target.h | 2 +- src/main/target/OMNIBUSF4/target.h | 2 +- src/main/target/REVO/target.h | 2 +- src/main/target/VRRACE/target.h | 2 +- 14 files changed, 97 insertions(+), 23 deletions(-) create mode 100755 src/main/target/MOTOLAB/config.c diff --git a/src/main/config/config.c b/src/main/config/config.c index bb177b2f41..3b03fb23b6 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -474,6 +474,9 @@ static void resetConf(void) #ifdef STM32F10X masterConfig.gyro_sync_denom = 8; masterConfig.pid_process_denom = 1; +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) + masterConfig.gyro_sync_denom = 8; + masterConfig.pid_process_denom = 4; #else masterConfig.gyro_sync_denom = 4; masterConfig.pid_process_denom = 2; diff --git a/src/main/config/config.h b/src/main/config/config.h index b97ac6b45b..cc239da923 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -46,16 +46,15 @@ typedef enum { FEATURE_RSSI_ADC = 1 << 15, FEATURE_LED_STRIP = 1 << 16, FEATURE_DISPLAY = 1 << 17, - FEATURE_ONESHOT125 = 1 << 18, + FEATURE_OSD = 1 << 18, FEATURE_BLACKBOX = 1 << 19, FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_TRANSPONDER = 1 << 21, FEATURE_AIRMODE = 1 << 22, FEATURE_SUPEREXPO_RATES = 1 << 23, - FEATURE_OSD = 1 << 24, - FEATURE_VTX = 1 << 25, - FEATURE_RX_NRF24 = 1 << 26, - FEATURE_SOFTSPI = 1 << 27, + FEATURE_VTX = 1 << 24, + FEATURE_RX_NRF24 = 1 << 25, + FEATURE_SOFTSPI = 1 << 26, } features_e; void latchActiveFeatures(void); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index dcf8828457..5aef0f5392 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -206,9 +206,8 @@ static const char * const featureNames[] = { "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP", "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", - "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", - "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO_RATES", - "OSD", NULL + "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD", + "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO_RATES", NULL }; // sync this with rxFailsafeChannelMode_e diff --git a/src/main/main.c b/src/main/main.c index 7ec478feae..24b983308d 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -288,12 +288,6 @@ void init(void) pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; #endif - if (masterConfig.motor_pwm_protocol == PWM_TYPE_ONESHOT125) { - featureSet(FEATURE_ONESHOT125); - } else { - featureClear(FEATURE_ONESHOT125); - } - bool use_unsyncedPwm = masterConfig.use_unsyncedPwm || masterConfig.motor_pwm_protocol == PWM_TYPE_CONVENTIONAL || masterConfig.motor_pwm_protocol == PWM_TYPE_BRUSHED; // Configurator feature abused for enabling Fast PWM diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index 3369b17635..b16a16444a 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -72,7 +72,6 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { - featureClear(FEATURE_ONESHOT125); masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; masterConfig.motor_pwm_rate = 32000; diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index be0a9bfea3..3aed5a3ceb 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -77,7 +77,6 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { - featureClear(FEATURE_ONESHOT125); masterConfig.mag_hardware = MAG_NONE; // disabled by default masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index ecce870a48..5264a16735 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -77,7 +77,6 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { - featureClear(FEATURE_ONESHOT125); masterConfig.mag_hardware = MAG_NONE; // disabled by default masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 8b4b846c40..7c9e89b48b 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -138,7 +138,7 @@ #define CURRENT_METER_ADC_PIN PC2 #define RSSI_ADC_PIN PC1 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 073e2fb916..8f69378592 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -150,7 +150,7 @@ #define RSSI_ADC_GPIO_PIN PC2 #define CURRENT_METER_ADC_PIN PC3 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/MOTOLAB/config.c b/src/main/target/MOTOLAB/config.c new file mode 100755 index 0000000000..490236ce04 --- /dev/null +++ b/src/main/target/MOTOLAB/config.c @@ -0,0 +1,82 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include + +#include "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +// Motolab target supports 2 different type of boards Tornado / Cyclone. +void targetConfiguration(void) { + masterConfig.gyro_sync_denom = 4; + masterConfig.pid_process_denom = 1; +} diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 0a0ec2593d..ad84573b59 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -167,7 +167,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_BLACKBOX) #define BUTTONS #define BUTTON_A_PORT GPIOB diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 5250a336b7..4bd45103a3 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -121,7 +121,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_PPM -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125 | FEATURE_RX_SERIAL) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL) #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 9aab5b4b10..76738118d0 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -110,7 +110,7 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125) +#define DEFAULT_FEATURES (FEATURE_BLACKBOX) #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index da1581e9f3..cda9b14463 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -156,7 +156,7 @@ #define RSSI_ADC_GPIO_PIN PB1 #define CURRENT_METER_ADC_PIN PA5 -#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_ONESHOT125 | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY) +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_SOFTSERIAL | FEATURE_TELEMETRY) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS From a475d26fcc8135ba9c2d25239ab58433054adc06 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 11:00:40 +0100 Subject: [PATCH 188/456] Removed dependencies on sensors/ --- src/main/drivers/compass_ak8963.c | 3 --- src/main/drivers/compass_ak8975.c | 2 -- src/main/drivers/compass_hmc5883l.c | 2 -- 3 files changed, 7 deletions(-) diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index 0668fcdd77..d5b9a59dec 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -33,9 +33,6 @@ #include "bus_i2c.h" #include "bus_spi.h" -#include "sensors/boardalignment.h" -#include "sensors/sensors.h" - #include "sensor.h" #include "compass.h" diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index 0d648ecc58..0e4d3677c5 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -33,8 +33,6 @@ #include "gpio.h" #include "bus_i2c.h" -#include "sensors/sensors.h" - #include "sensor.h" #include "compass.h" diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 3709cc510e..10fa9b3c6d 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -39,8 +39,6 @@ #include "sensor.h" #include "compass.h" -#include "sensors/sensors.h" - #include "compass_hmc5883l.h" //#define DEBUG_MAG_DATA_READY_INTERRUPT From c5be0fb3dcd8a5f538957875110264f13609c38b Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 14:20:39 +0200 Subject: [PATCH 189/456] Move PIDweight to a common place. pid_legacy should not depend on pid_betaflight. --- src/main/flight/pid.c | 3 +++ src/main/flight/pid.h | 3 +++ src/main/flight/pid_betaflight.c | 3 --- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5ca8e312a7..03c86f53ab 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -53,6 +53,9 @@ bool pidStabilisationEnabled; int16_t axisPID[3]; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + #ifdef BLACKBOX int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 79e83a2c04..04e129b9cf 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -127,6 +127,9 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool airmodeWasActivated; extern uint32_t targetPidLooptime; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +extern uint8_t PIDweight[3]; + void pidSetController(pidControllerType_e type); void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index 72cdd4ea4c..eae0f4c17b 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -62,9 +62,6 @@ extern biquadFilter_t dtermFilterNotch[3]; extern bool dtermNotchInitialised; extern bool dtermBiquadLpfInitialised; -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; - void initFilters(const pidProfile_t *pidProfile); float getdT(void); From 87ffc6d27f698ceb1bf76850b048f88fb1d21192 Mon Sep 17 00:00:00 2001 From: NightHawk32 Date: Thu, 4 Aug 2016 09:09:40 -0400 Subject: [PATCH 190/456] Changing ADC pin mapping to fit KOMBINI current sensor --- src/main/target/PIKOBLX/target.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 111f6fc476..279a17a9fe 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -76,9 +76,8 @@ #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC #define ADC_INSTANCE ADC2 -#define CURRENT_METER_ADC_PIN PA2 +#define CURRENT_METER_ADC_PIN PB2 #define VBAT_ADC_PIN PA5 -#define RSSI_ADC_PIN PB2 #define LED_STRIP #if 1 From 43851b95e6a97a71e3a30914c10ea71fe23085e3 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 15:16:30 +0200 Subject: [PATCH 191/456] Added missing include. Target VRRACE build failed. --- src/main/target/VRRACE/target.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/VRRACE/target.c b/src/main/target/VRRACE/target.c index 094ade421e..90270b1fa8 100644 --- a/src/main/target/VRRACE/target.c +++ b/src/main/target/VRRACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input From 0e95e3d811e9ac06652c396867a09834b979fb79 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 17:30:08 +0100 Subject: [PATCH 192/456] Used forward declarations to remove #includes from header files --- src/main/blackbox/blackbox.c | 1 + src/main/blackbox/blackbox_io.c | 1 + src/main/common/printf.h | 11 +++-------- src/main/flight/altitudehold.h | 16 +++++++++------- src/main/flight/imu.h | 3 ++- src/main/flight/pid.c | 2 ++ src/main/flight/pid.h | 1 - src/main/io/display.c | 2 ++ src/main/io/rc_controls.h | 9 ++++----- src/main/io/rc_curves.c | 12 +++++++----- src/main/io/rc_curves.h | 4 +++- src/main/io/serial_4way.c | 10 ++++++---- src/main/io/serial_4way.h | 6 ++++-- src/main/io/serial_4way_avrootloader.c | 3 ++- src/main/io/serial_4way_impl.h | 2 +- src/main/io/serial_4way_stk500v2.c | 3 +++ src/main/io/serial_msp.c | 1 + src/main/io/serial_msp.h | 11 +++++------ src/main/rx/rx.c | 15 +++++++-------- src/main/rx/xbus.h | 6 +++--- src/main/sensors/battery.c | 2 ++ src/main/telemetry/frsky.h | 8 ++++---- 22 files changed, 72 insertions(+), 57 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index a59cc8d78f..96dedade74 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -72,6 +72,7 @@ #include "flight/altitudehold.h" #include "flight/failsafe.h" #include "flight/imu.h" +#include "flight/pid.h" #include "flight/navigation.h" #include "config/runtime_config.h" diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index cd4f05d980..88306767c6 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -53,6 +53,7 @@ #include "flight/altitudehold.h" #include "flight/failsafe.h" #include "flight/imu.h" +#include "flight/pid.h" #include "flight/navigation.h" #include "config/runtime_config.h" diff --git a/src/main/common/printf.h b/src/main/common/printf.h index ac5493590b..467b054d52 100644 --- a/src/main/common/printf.h +++ b/src/main/common/printf.h @@ -102,13 +102,10 @@ For further details see source code. regs Kusti, 23.10.2004 */ -#ifndef __TFP_PRINTF__ -#define __TFP_PRINTF__ +#pragma once #include -#include "drivers/serial.h" - void init_printf(void *putp, void (*putf) (void *, char)); int tfp_printf(const char *fmt, ...); @@ -119,8 +116,6 @@ int tfp_format(void *putp, void (*putf) (void *, char), const char *fmt, va_list #define printf tfp_printf #define sprintf tfp_sprintf -void setPrintfSerialPort(serialPort_t *serialPort); +struct serialPort_s; +void setPrintfSerialPort(struct serialPort_s *serialPort); void printfSupportInit(void); - - -#endif diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index 3677fbb287..da16978814 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -15,19 +15,21 @@ * along with Cleanflight. If not, see . */ -#include "io/escservo.h" -#include "io/rc_controls.h" -#include "flight/pid.h" - -#include "sensors/barometer.h" +#pragma once extern int32_t AltHold; extern int32_t vario; void calculateEstimatedAltitude(uint32_t currentTime); -void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig); -void applyAltHold(airplaneConfig_t *airplaneConfig); +struct pidProfile_s; +struct barometerConfig_s; +struct rcControlsConfig_s; +struct escAndServoConfig_s; +void configureAltitudeHold(struct pidProfile_s *initialPidProfile, struct barometerConfig_s *intialBarometerConfig, struct rcControlsConfig_s *initialRcControlsConfig, struct escAndServoConfig_s *initialEscAndServoConfig); + +struct airplaneConfig_t; +void applyAltHold(struct airplaneConfig_s *airplaneConfig); void updateAltHoldState(void); void updateSonarAltHoldState(void); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 94b361fcf6..e5229a2ead 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -69,9 +69,10 @@ typedef struct accProcessor_s { accProcessorState_e state; } accProcessor_t; +struct pidProfile_s; void imuConfigure( imuRuntimeConfig_t *initialImuRuntimeConfig, - pidProfile_t *initialPidProfile, + struct pidProfile_s *initialPidProfile, accDeadband_t *initialAccDeadband, uint16_t throttle_correction_angle ); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 03c86f53ab..5d334346cb 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -51,6 +51,8 @@ uint32_t targetPidLooptime; bool pidStabilisationEnabled; +uint8_t PIDweight[3]; + int16_t axisPID[3]; // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 04e129b9cf..2970a28033 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -14,7 +14,6 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ -#include "rx/rx.h" #pragma once diff --git a/src/main/io/display.c b/src/main/io/display.c index 1e5eee9746..a22ca88023 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -63,6 +63,8 @@ #include "io/display.h" +#include "rx/rx.h" + #include "scheduler/scheduler.h" extern profile_t *currentProfile; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index aace839d9b..a3622aacab 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -17,8 +17,6 @@ #pragma once -#include "rx/rx.h" - typedef enum { BOXARM = 0, BOXANGLE, @@ -167,8 +165,9 @@ typedef struct rcControlsConfig_s { bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); -throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); -void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); +struct rxConfig_s; +throttleStatus_e calculateThrottleStatus(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void processRcStickPositions(struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); @@ -257,7 +256,7 @@ bool isAirmodeActive(void); bool isSuperExpoActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); -void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); +void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig); bool isUsingSticksForArming(void); diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index 89b46522f2..f96426bfdf 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -18,13 +18,15 @@ #include #include -#include "io/rc_controls.h" -#include "io/escservo.h" - -#include "io/rc_curves.h" - #include "config/config.h" +#include "io/escservo.h" +#include "io/rc_curves.h" +#include "io/rc_controls.h" + +#include "rx/rx.h" + + #define THROTTLE_LOOKUP_LENGTH 12 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE diff --git a/src/main/io/rc_curves.h b/src/main/io/rc_curves.h index 747a934df3..406218d03b 100644 --- a/src/main/io/rc_curves.h +++ b/src/main/io/rc_curves.h @@ -17,7 +17,9 @@ #pragma once -void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig); +struct controlRateConfig_s; +struct escAndServoConfig_s; +void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct escAndServoConfig_s *escAndServoConfig); int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate); int16_t rcLookupThrottle(int32_t tmp); diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 9bd2ba69e9..0065cff899 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -23,21 +23,23 @@ #include #include "platform.h" + #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE -#include "drivers/serial.h" + #include "drivers/buf_writer.h" -#include "drivers/gpio.h" +#include "drivers/io.h" +#include "drivers/serial.h" #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "drivers/light_led.h" #include "drivers/system.h" + #include "flight/mixer.h" + #include "io/beeper.h" #include "io/serial_msp.h" -#include "io/serial_msp.h" #include "io/serial_4way.h" -#include "io/serial_4way_impl.h" #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #include "io/serial_4way_avrootloader.h" diff --git a/src/main/io/serial_4way.h b/src/main/io/serial_4way.h index b1bba5ff69..8e0c934a97 100644 --- a/src/main/io/serial_4way.h +++ b/src/main/io/serial_4way.h @@ -17,7 +17,8 @@ */ #pragma once -#include "serial_4way_impl.h" +#include "drivers/io_types.h" +#include "io/serial_4way_impl.h" #define USE_SERIAL_4WAY_BLHELI_BOOTLOADER #define USE_SERIAL_4WAY_SK_BOOTLOADER @@ -46,5 +47,6 @@ typedef union __attribute__ ((packed)) { bool isMcuConnected(void); uint8_t esc4wayInit(void); -void esc4wayProcess(serialPort_t *mspPort); +struct serialPort_s; +void esc4wayProcess(struct serialPort_s *mspPort); void esc4wayRelease(void); diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 223d3e10cd..f3eeba5320 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -25,7 +25,8 @@ #include "platform.h" #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE -#include "drivers/io_types.h" + +#include "drivers/io.h" #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/timer.h" diff --git a/src/main/io/serial_4way_impl.h b/src/main/io/serial_4way_impl.h index a8f83bfae8..351aba0b62 100644 --- a/src/main/io/serial_4way_impl.h +++ b/src/main/io/serial_4way_impl.h @@ -17,7 +17,7 @@ */ #pragma once -#include "drivers/io.h" +#include "drivers/io_types.h" typedef struct { IO_t io; diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c index 342a0c8b06..0b406531f0 100644 --- a/src/main/io/serial_4way_stk500v2.c +++ b/src/main/io/serial_4way_stk500v2.c @@ -23,7 +23,10 @@ #include #include "platform.h" + #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE + +#include "drivers/io.h" #include "drivers/serial.h" #include "config/config.h" #include "io/serial.h" diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index b5d170cfbc..8b2f81fb6f 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -37,6 +37,7 @@ #include "drivers/serial.h" #include "drivers/bus_i2c.h" +#include "drivers/io.h" #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 2778da7e22..114e94ab84 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -17,9 +17,6 @@ #pragma once -#include "io/serial.h" -#include "drivers/serial.h" - // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports. #define MAX_MSP_PORT_COUNT 2 @@ -46,7 +43,9 @@ typedef struct mspPort_s { uint8_t cmdMSP; } mspPort_t; -void mspInit(serialConfig_t *serialConfig); +struct serialConfig_s; +void mspInit(struct serialConfig_s *serialConfig); void mspProcess(void); -void mspAllocateSerialPorts(serialConfig_t *serialConfig); -void mspReleasePortIfAllocated(serialPort_t *serialPort); +void mspAllocateSerialPorts(struct serialConfig_s *serialConfig); +struct serialPort_s; +void mspReleasePortIfAllocated(struct serialPort_s *serialPort); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index a7a09a44b7..bcba5bb86e 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -32,15 +32,16 @@ #include "drivers/serial.h" #include "drivers/adc.h" -#include "io/serial.h" -#include "io/rc_controls.h" - -#include "flight/failsafe.h" - -#include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/system.h" + +#include "flight/failsafe.h" + +#include "io/serial.h" +#include "io/rc_controls.h" + +#include "rx/rx.h" #include "rx/pwm.h" #include "rx/sbus.h" #include "rx/spektrum.h" @@ -51,8 +52,6 @@ #include "rx/ibus.h" #include "rx/jetiexbus.h" -#include "rx/rx.h" - //#define DEBUG_RX_SIGNAL_LOSS diff --git a/src/main/rx/xbus.h b/src/main/rx/xbus.h index bc76f56786..cfaa920ffd 100644 --- a/src/main/rx/xbus.h +++ b/src/main/rx/xbus.h @@ -17,7 +17,7 @@ #pragma once -#include "rx/rx.h" - -bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); +struct rxConfig_s; +struct rxRuntimeConfig_s; +bool xBusInit(struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig, rcReadRawDataPtr *callback); uint8_t xBusFrameStatus(void); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 75aac6cf0a..6d165cbc57 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -35,6 +35,8 @@ #include "io/rc_controls.h" #include "io/beeper.h" +#include "rx/rx.h" + #define VBATT_PRESENT_THRESHOLD_MV 10 #define VBATT_LPF_FREQ 0.4f diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index 555eb43e83..89ba617c8f 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -17,17 +17,17 @@ #pragma once -#include "rx/rx.h" - typedef enum { FRSKY_VFAS_PRECISION_LOW = 0, FRSKY_VFAS_PRECISION_HIGH } frskyVFasPrecision_e; -void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); +struct rxConfig_s; +void handleFrSkyTelemetry(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); void checkFrSkyTelemetryState(void); -void initFrSkyTelemetry(telemetryConfig_t *telemetryConfig); +struct telemetryConfig_s; +void initFrSkyTelemetry(struct telemetryConfig_s *telemetryConfig); void configureFrSkyTelemetryPort(void); void freeFrSkyTelemetryPort(void); From 9eeb6bb2e74a01d7f9716ccd74f1f23f7ee6ef22 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 19:45:43 +0100 Subject: [PATCH 193/456] Removed #includes from header files --- src/main/config/config.c | 1 + src/main/drivers/accgyro_mpu.c | 2 +- src/main/drivers/accgyro_mpu.h | 1 + src/main/drivers/bus_spi.c | 1 + src/main/drivers/exti.h | 2 +- src/main/drivers/rcc.h | 2 -- src/main/drivers/serial_usb_vcp.h | 5 ++--- src/main/drivers/system_stm32f4xx.c | 2 +- src/main/drivers/timer_stm32f10x.c | 2 ++ src/main/drivers/timer_stm32f30x.c | 2 ++ src/main/drivers/timer_stm32f4xx.c | 2 ++ src/main/io/beeper.c | 4 ++-- src/main/rx/jetiexbus.c | 10 ++++++---- src/main/sensors/battery.h | 2 +- src/main/target/ALIENFLIGHTF3/hardware_revision.h | 2 +- 15 files changed, 24 insertions(+), 16 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b666fe0e77..12bbaf5573 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -36,6 +36,7 @@ #include "drivers/accgyro.h" #include "drivers/compass.h" #include "drivers/system.h" +#include "drivers/io.h" #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 1c705ef137..4284697217 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -29,7 +29,7 @@ #include "nvic.h" #include "system.h" -#include "gpio.h" +#include "io.h" #include "exti.h" #include "bus_i2c.h" diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 132c9e13a0..0b70ebb14a 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -17,6 +17,7 @@ #pragma once +#include "io_types.h" #include "exti.h" // MPU6050 diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 690f25b52e..c86fdbdb48 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -21,6 +21,7 @@ #include #include "bus_spi.h" +#include "exti.h" #include "io.h" #include "io_impl.h" #include "rcc.h" diff --git a/src/main/drivers/exti.h b/src/main/drivers/exti.h index 8071c65e33..adc95f13ab 100644 --- a/src/main/drivers/exti.h +++ b/src/main/drivers/exti.h @@ -18,7 +18,7 @@ #pragma once -#include "drivers/io.h" +#include "io_types.h" // old EXTI interface, to be replaced typedef struct extiConfig_s { diff --git a/src/main/drivers/rcc.h b/src/main/drivers/rcc.h index 69dce965f9..4dee74ee79 100644 --- a/src/main/drivers/rcc.h +++ b/src/main/drivers/rcc.h @@ -1,7 +1,5 @@ #pragma once -#include "platform.h" -#include "common/utils.h" #include "rcc_types.h" enum rcc_reg { diff --git a/src/main/drivers/serial_usb_vcp.h b/src/main/drivers/serial_usb_vcp.h index 7d1b88be1f..26ef11215f 100644 --- a/src/main/drivers/serial_usb_vcp.h +++ b/src/main/drivers/serial_usb_vcp.h @@ -17,8 +17,6 @@ #pragma once -#include "serial.h" - typedef struct { serialPort_t port; @@ -30,4 +28,5 @@ typedef struct { } vcpPort_t; serialPort_t *usbVcpOpen(void); -uint32_t usbVcpGetBaudRate(serialPort_t *instance); +struct serialPort_s; +uint32_t usbVcpGetBaudRate(struct serialPort_s *instance); diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 5c09ebdf6a..8b48f82f5e 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -22,7 +22,7 @@ #include "platform.h" #include "accgyro_mpu.h" -#include "gpio.h" +#include "exti.h" #include "nvic.h" #include "system.h" diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index 49106818f0..d918b07112 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -10,6 +10,8 @@ #include "platform.h" +#include "common/utils.h" + #include "stm32f10x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 99111bad77..871cfb6d78 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -10,6 +10,8 @@ #include "platform.h" +#include "common/utils.h" + #include "stm32f30x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index ccce10350a..750324e94a 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -10,6 +10,8 @@ #include "platform.h" +#include "common/utils.h" + #include "stm32f4xx.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 716cf84b36..cd616044d2 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -22,14 +22,14 @@ #include #include "build_config.h" -#include "io/rc_controls.h" - #include "drivers/gpio.h" #include "drivers/sound_beeper.h" #include "drivers/system.h" #include "sensors/battery.h" #include "sensors/sensors.h" +#include "io/rc_controls.h" + #include "io/statusindicator.h" #include "io/vtx.h" diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 2836d5ebca..d4050b8c84 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -39,12 +39,17 @@ #include "platform.h" -#include "common/utils.h" #include "build_config.h" +#include "debug.h" + +#include "common/utils.h" + #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/serial_uart.h" + #include "io/serial.h" + #include "rx/rx.h" #include "rx/jetiexbus.h" @@ -61,9 +66,6 @@ #endif //TELEMETRY -#include "debug.h" -#include "rx/rx.h" - // // Serial driver for Jeti EX Bus receiver // diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index f459a3bcc4..3cfc2702d3 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -17,7 +17,7 @@ #pragma once -#include "common/maths.h" +#include "common/maths.h" // for fix12_t #ifndef VBAT_SCALE_DEFAULT #define VBAT_SCALE_DEFAULT 110 diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.h b/src/main/target/ALIENFLIGHTF3/hardware_revision.h index 9b23c614d1..48d56a86a3 100644 --- a/src/main/target/ALIENFLIGHTF3/hardware_revision.h +++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.h @@ -29,4 +29,4 @@ extern uint8_t hardwareRevision; void updateHardwareRevision(void); void detectHardwareRevision(void); -const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void); \ No newline at end of file +const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void); From 06be182e5066c9f8cd7661dc22551f5bd6d0fbf2 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 5 Aug 2016 00:02:38 +0200 Subject: [PATCH 194/456] Add yaw smoothing back // adjust defaults // KISS fc remove some shared timers --- src/main/config/config.c | 6 +++--- src/main/flight/pid.c | 2 +- src/main/mw.c | 4 +--- src/main/target/KISSFC/target.c | 5 ----- 4 files changed, 5 insertions(+), 12 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 3b03fb23b6..2f047d64e4 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -203,10 +203,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[ROLL] = 45; pidProfile->I8[ROLL] = 40; - pidProfile->D8[ROLL] = 25; + pidProfile->D8[ROLL] = 20; pidProfile->P8[PITCH] = 60; pidProfile->I8[PITCH] = 60; - pidProfile->D8[PITCH] = 30; + pidProfile->D8[PITCH] = 25; pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; @@ -244,7 +244,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; - pidProfile->dtermSetpointWeight = 200; + pidProfile->dtermSetpointWeight = 120; pidProfile->pidMaxVelocityYaw = 200; pidProfile->toleranceBand = 20; pidProfile->toleranceBandReduction = 40; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ae735be925..b405a9ab4f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -194,7 +194,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc configD[axis] = pidProfile->D8[axis]; } - // Limit abrupt yaw inputs + // Limit abrupt yaw inputs / stops if (axis == YAW && pidProfile->pidMaxVelocityYaw) { float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; if (ABS(yawCurrentVelocity) > yawMaxVelocity) { diff --git a/src/main/mw.c b/src/main/mw.c index c3ebdf8299..eded995564 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -263,9 +263,7 @@ void processRcCommand(void) } if (readyToCalculateRate || isRXDataNew) { - // Don't smooth yaw axis - int axisToCalculate = (isRXDataNew) ? 3 : 2; - for (int axis = 0; axis < axisToCalculate; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); + for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); isRXDataNew = false; diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 167265ed78..cbb9d74818 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -29,11 +29,6 @@ const uint16_t multiPPM[] = { PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; From 3371cadd862b0d64e29d3e74a25a21e3f0deb628 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 5 Aug 2016 00:02:38 +0200 Subject: [PATCH 195/456] Add yaw smoothing back // adjust defaults // KISS fc remove some shared timers --- src/main/config/config.c | 6 +++--- src/main/mw.c | 4 +--- src/main/target/KISSFC/target.c | 5 ----- 3 files changed, 4 insertions(+), 11 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b666fe0e77..a1927e274d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -204,10 +204,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[ROLL] = 45; pidProfile->I8[ROLL] = 40; - pidProfile->D8[ROLL] = 25; + pidProfile->D8[ROLL] = 20; pidProfile->P8[PITCH] = 60; pidProfile->I8[PITCH] = 60; - pidProfile->D8[PITCH] = 30; + pidProfile->D8[PITCH] = 25; pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; @@ -245,7 +245,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; - pidProfile->dtermSetpointWeight = 200; + pidProfile->dtermSetpointWeight = 120; pidProfile->pidMaxVelocityYaw = 200; pidProfile->toleranceBand = 20; pidProfile->toleranceBandReduction = 40; diff --git a/src/main/mw.c b/src/main/mw.c index c3ebdf8299..eded995564 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -263,9 +263,7 @@ void processRcCommand(void) } if (readyToCalculateRate || isRXDataNew) { - // Don't smooth yaw axis - int axisToCalculate = (isRXDataNew) ? 3 : 2; - for (int axis = 0; axis < axisToCalculate; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); + for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); isRXDataNew = false; diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index f105e9aaf7..a443fe862f 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -30,11 +30,6 @@ const uint16_t multiPPM[] = { PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; From c33e14db4b2a273a07224842e295b443a0441f94 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 01:37:59 +0200 Subject: [PATCH 196/456] Start on version 3.1.0 --- src/main/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/version.h b/src/main/version.h index 0f770674c1..c3c6529ed3 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,7 +16,7 @@ */ #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 0 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x From 2676408071a8013e654e8821201cb8b4d1a25e48 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 00:55:16 +0100 Subject: [PATCH 197/456] Optimisation of driver header files --- src/main/config/config.c | 1 + src/main/drivers/accgyro_mma845x.c | 2 +- src/main/drivers/adc.c | 2 + src/main/drivers/adc.h | 2 +- src/main/drivers/adc_impl.h | 6 +-- src/main/drivers/barometer_spi_bmp280.c | 2 + src/main/drivers/bus_i2c.h | 4 +- src/main/drivers/bus_i2c_soft.c | 2 +- src/main/drivers/bus_i2c_stm32f30x.c | 5 +- src/main/drivers/bus_spi.h | 5 +- src/main/drivers/compass_hmc5883l.h | 2 +- src/main/drivers/dma.c | 4 +- src/main/drivers/dma_stm32f4xx.c | 4 +- src/main/drivers/flash_m25p16.c | 7 +-- src/main/drivers/flash_m25p16.h | 2 +- src/main/drivers/gyro_sync.c | 6 +-- src/main/drivers/io.h | 24 +-------- src/main/drivers/io_types.h | 28 +++++++++++ src/main/drivers/light_ws2811strip.c | 4 +- .../drivers/light_ws2811strip_stm32f10x.c | 3 +- .../drivers/light_ws2811strip_stm32f30x.c | 2 +- src/main/drivers/max7456.c | 15 +++--- src/main/drivers/pwm_mapping.h | 5 +- src/main/drivers/pwm_rx.c | 2 +- src/main/drivers/rcc.h | 4 +- src/main/drivers/rcc_types.h | 4 ++ src/main/drivers/sdcard.c | 4 +- src/main/drivers/serial_usb_vcp.c | 4 +- src/main/drivers/sonar_hcsr04.h | 3 +- src/main/drivers/sound_beeper.h | 2 +- src/main/drivers/timer.c | 2 +- src/main/drivers/timer.h | 9 ++-- src/main/drivers/timer_stm32f10x.c | 5 ++ src/main/drivers/timer_stm32f30x.c | 5 ++ src/main/drivers/timer_stm32f4xx.c | 7 ++- src/main/drivers/transponder_ir.c | 6 +-- src/main/drivers/transponder_ir_stm32f30x.c | 6 +-- src/main/drivers/vtx_rtc6705.c | 6 +-- src/main/drivers/vtx_soft_spi_rtc6705.c | 7 +-- src/main/io/serial_4way_avrootloader.c | 2 + src/main/main.c | 1 + src/main/sensors/initialisation.c | 2 +- src/main/target/AIORACERF3/target.c | 1 + src/main/target/AIR32/target.c | 1 + src/main/target/ALIENFLIGHTF1/target.c | 1 + src/main/target/ALIENFLIGHTF1/target.h | 1 + src/main/target/ALIENFLIGHTF3/target.c | 1 + src/main/target/ALIENFLIGHTF3/target.h | 3 +- src/main/target/ALIENFLIGHTF4/target.c | 1 + src/main/target/BLUEJAYF4/target.c | 1 + src/main/target/CC3D/target.c | 1 + src/main/target/CHEBUZZF3/target.c | 1 + src/main/target/CJMCU/target.c | 1 + src/main/target/CJMCU/target.h | 1 + src/main/target/COLIBRI_RACE/target.c | 1 + src/main/target/DOGE/target.c | 1 + src/main/target/DOGE/target.h | 9 ++-- src/main/target/EUSTM32F103RC/target.c | 1 + src/main/target/EUSTM32F103RC/target.h | 1 + src/main/target/F4BY/target.c | 49 ++++++++++--------- src/main/target/F4BY/target.h | 3 +- src/main/target/FURYF3/target.c | 1 + src/main/target/FURYF4/target.c | 1 + src/main/target/FURYF4/target.h | 3 +- src/main/target/IRCFUSIONF3/target.c | 1 + src/main/target/IRCFUSIONF3/target.h | 3 +- src/main/target/KISSFC/target.c | 1 + src/main/target/KISSFC/target.h | 3 +- src/main/target/LUX_RACE/target.c | 1 + src/main/target/MICROSCISKY/target.c | 1 + src/main/target/MICROSCISKY/target.h | 1 + src/main/target/MOTOLAB/target.c | 1 + src/main/target/NAZE/target.c | 1 + src/main/target/NAZE/target.h | 1 + src/main/target/OLIMEXINO/target.c | 1 + src/main/target/OLIMEXINO/target.h | 1 + src/main/target/OMNIBUS/target.c | 1 + src/main/target/OMNIBUSF4/target.c | 1 + src/main/target/PIKOBLX/target.c | 1 + src/main/target/PIKOBLX/target.h | 3 +- src/main/target/PORT103R/target.c | 1 + src/main/target/PORT103R/target.h | 1 + src/main/target/REVO/target.c | 1 + src/main/target/REVONANO/target.c | 1 + src/main/target/RMDO/target.c | 1 + src/main/target/SINGULARITY/target.c | 1 + src/main/target/SINGULARITY/target.h | 3 +- src/main/target/SIRINFPV/target.c | 1 + src/main/target/SPARKY/target.c | 1 + src/main/target/SPRACINGF3/target.c | 1 + src/main/target/SPRACINGF3EVO/target.c | 1 + src/main/target/SPRACINGF3MINI/target.c | 1 + src/main/target/STM32F3DISCOVERY/target.c | 1 + src/main/target/STM32F3DISCOVERY/target.h | 1 + src/main/target/X_RACERSPI/target.c | 1 + src/main/target/X_RACERSPI/target.h | 3 +- src/main/target/ZCOREF3/target.c | 1 + src/main/target/ZCOREF3/target.h | 1 - src/main/telemetry/frsky.c | 2 + src/main/telemetry/telemetry.c | 2 +- 100 files changed, 212 insertions(+), 138 deletions(-) create mode 100644 src/main/drivers/io_types.h create mode 100644 src/main/drivers/rcc_types.h diff --git a/src/main/config/config.c b/src/main/config/config.c index 2f047d64e4..a1927e274d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -23,6 +23,7 @@ #include "debug.h" #include "build_config.h" +#include "debug.h" #include "blackbox/blackbox_io.h" diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index 202b90e804..69a82a0adb 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -21,7 +21,7 @@ #include "platform.h" #include "system.h" -#include "gpio.h" +#include "io.h" #include "bus_i2c.h" #include "sensor.h" diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index a65abf9128..6bba109396 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -27,6 +27,8 @@ #include "adc.h" #include "adc_impl.h" +#include "common/utils.h" + //#define DEBUG_ADC_CHANNELS #ifdef USE_ADC diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 55914a3365..47a4b52a0e 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -17,7 +17,7 @@ #pragma once -#include "io.h" +#include "io_types.h" typedef enum { ADC_BATTERY = 0, diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 146ada62be..c89dc1bf53 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -17,8 +17,8 @@ #pragma once -#include "io.h" -#include "rcc.h" +#include "io_types.h" +#include "rcc_types.h" #if defined(STM32F4) #define ADC_TAG_MAP_COUNT 16 @@ -65,4 +65,4 @@ extern const adcTagMap_t adcTagMap[ADC_TAG_MAP_COUNT]; extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; -uint8_t adcChannelByTag(ioTag_t ioTag); \ No newline at end of file +uint8_t adcChannelByTag(ioTag_t ioTag); diff --git a/src/main/drivers/barometer_spi_bmp280.c b/src/main/drivers/barometer_spi_bmp280.c index 9d97343e11..0c97684972 100644 --- a/src/main/drivers/barometer_spi_bmp280.c +++ b/src/main/drivers/barometer_spi_bmp280.c @@ -15,6 +15,7 @@ * along with Betaflight. If not, see . */ +#include #include #include @@ -24,6 +25,7 @@ #include "barometer.h" #include "barometer_bmp280.h" +#include "io.h" #ifdef USE_BARO_SPI_BMP280 #define DISABLE_BMP280 IOHi(bmp280CsPin) diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index 86f7a66181..26cce3792f 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -21,8 +21,8 @@ #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) #define I2C_DEFAULT_TIMEOUT I2C_SHORT_TIMEOUT -#include "drivers/io.h" -#include "drivers/rcc.h" +#include "io_types.h" +#include "rcc_types.h" #ifndef I2C_DEVICE #define I2C_DEVICE I2CINVALID diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c index 104c77c379..42ea8cadc9 100644 --- a/src/main/drivers/bus_i2c_soft.c +++ b/src/main/drivers/bus_i2c_soft.c @@ -22,7 +22,7 @@ #include "build_config.h" #include "bus_i2c.h" -#include "drivers/io.h" +#include "io.h" // Software I2C driver, using same pins as hardware I2C, with hw i2c module disabled. // Can be configured for I2C2 pinout (SCL: PB10, SDA: PB11) or I2C1 pinout (SCL: PB6, SDA: PB7) diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 88b267f111..f8c5b23697 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -20,9 +20,10 @@ #include -#include "gpio.h" #include "system.h" -#include "drivers/io_impl.h" +#include "io.h" +#include "io_impl.h" +#include "rcc.h" #include "bus_i2c.h" diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 5a6ea7dd37..58c76c9d78 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -17,9 +17,8 @@ #pragma once -#include -#include "io.h" -#include "rcc.h" +#include "io_types.h" +#include "rcc_types.h" #if defined(STM32F4) || defined(STM32F3) #define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index 035a2c936f..e2f71fd28d 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -17,7 +17,7 @@ #pragma once -#include "io.h" +#include "io_types.h" typedef struct hmc5883Config_s { ioTag_t intTag; diff --git a/src/main/drivers/dma.c b/src/main/drivers/dma.c index 0ad411312e..7e4942fbe1 100644 --- a/src/main/drivers/dma.c +++ b/src/main/drivers/dma.c @@ -21,8 +21,8 @@ #include -#include "drivers/nvic.h" -#include "drivers/dma.h" +#include "nvic.h" +#include "dma.h" /* * DMA descriptors. diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index ec6e7908d3..8372ad0bcb 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -21,8 +21,8 @@ #include -#include "drivers/nvic.h" -#include "drivers/dma.h" +#include "nvic.h" +#include "dma.h" /* * DMA descriptors. diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 0ab3d6122b..bf7cf6f0cb 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -22,9 +22,10 @@ #ifdef USE_FLASH_M25P16 -#include "drivers/flash_m25p16.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "flash_m25p16.h" +#include "io.h" +#include "bus_spi.h" +#include "system.h" #define M25P16_INSTRUCTION_RDID 0x9F #define M25P16_INSTRUCTION_READ_BYTES 0x03 diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h index 223efa1809..6cad8a8ea6 100644 --- a/src/main/drivers/flash_m25p16.h +++ b/src/main/drivers/flash_m25p16.h @@ -19,7 +19,7 @@ #include #include "flash.h" -#include "io.h" +#include "io_types.h" #define M25P16_PAGESIZE 256 diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index cd95e186e1..dbdc2244c9 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -10,9 +10,9 @@ #include "platform.h" -#include "drivers/sensor.h" -#include "drivers/accgyro.h" -#include "drivers/gyro_sync.h" +#include "sensor.h" +#include "accgyro.h" +#include "gyro_sync.h" static uint8_t mpuDividerDrops; diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index d655646282..5afa4abb76 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -6,16 +6,7 @@ #include #include "resource.h" -// IO pin identification -// make sure that ioTag_t can't be assigned into IO_t without warning -typedef uint8_t ioTag_t; // packet tag to specify IO pin -typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change - -// NONE initializer for ioTag_t variables -#define IOTAG_NONE ((ioTag_t)0) - -// NONE initializer for IO_t variable -#define IO_NONE ((IO_t)0) +#include "io_types.h" // preprocessor is used to convert pinid to requested C data value // compile-time error is generated if requested pin is not available (not set in TARGET_IO_PORTx) @@ -24,19 +15,6 @@ typedef void* IO_t; // type specifying IO pin. Currently ioRec_t poin // expand pinid to to ioTag_t #define IO_TAG(pinid) DEFIO_TAG(pinid) -// both ioTag_t and IO_t are guarantied to be zero if pinid is NONE (no pin) -// this simplifies initialization (globals are zeroed on start) and allows -// omitting unused fields in structure initializers. -// it is also possible to use IO_t and ioTag_t as boolean value -// TODO - this may conflict with requirement to generate warning/error on IO_t - ioTag_t assignment -// IO_t being pointer is only possibility I know of .. - -// pin config handling -// pin config is packed into ioConfig_t to decrease memory requirements -// IOCFG_x macros are defined for common combinations for all CPUs; this -// helps masking CPU differences - -typedef uint8_t ioConfig_t; // packed IO configuration #if defined(STM32F1) // mode is using only bits 6-2 diff --git a/src/main/drivers/io_types.h b/src/main/drivers/io_types.h new file mode 100644 index 0000000000..ba80cd3bc7 --- /dev/null +++ b/src/main/drivers/io_types.h @@ -0,0 +1,28 @@ +#pragma once + +#include + +// IO pin identification +// make sure that ioTag_t can't be assigned into IO_t without warning +typedef uint8_t ioTag_t; // packet tag to specify IO pin +typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change + +// NONE initializer for ioTag_t variables +#define IOTAG_NONE ((ioTag_t)0) + +// NONE initializer for IO_t variable +#define IO_NONE ((IO_t)0) + +// both ioTag_t and IO_t are guarantied to be zero if pinid is NONE (no pin) +// this simplifies initialization (globals are zeroed on start) and allows +// omitting unused fields in structure initializers. +// it is also possible to use IO_t and ioTag_t as boolean value +// TODO - this may conflict with requirement to generate warning/error on IO_t - ioTag_t assignment +// IO_t being pointer is only possibility I know of .. + +// pin config handling +// pin config is packed into ioConfig_t to decrease memory requirements +// IOCFG_x macros are defined for common combinations for all CPUs; this +// helps masking CPU differences + +typedef uint8_t ioConfig_t; // packed IO configuration diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 6c8f2bb18a..d9c29af370 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -36,8 +36,8 @@ #include "common/color.h" #include "common/colorconversion.h" -#include "drivers/dma.h" -#include "drivers/light_ws2811strip.h" +#include "dma.h" +#include "light_ws2811strip.h" uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; volatile uint8_t ws2811LedDataTransferInProgress = 0; diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 4b588a0c9f..911b7cdc66 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -23,10 +23,11 @@ #ifdef LED_STRIP #include "common/color.h" -#include "drivers/light_ws2811strip.h" +#include "light_ws2811strip.h" #include "nvic.h" #include "io.h" #include "dma.h" +#include "rcc.h" #include "timer.h" static IO_t ws2811IO = IO_NONE; diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 8379188f99..1fadfb1f03 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -26,7 +26,7 @@ #include "nvic.h" #include "common/color.h" -#include "drivers/light_ws2811strip.h" +#include "light_ws2811strip.h" #include "dma.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 82f3f29af9..7ddc2a058b 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -19,16 +19,19 @@ #include #include "platform.h" -#include "version.h" #ifdef USE_MAX7456 +#include "version.h" + #include "common/printf.h" -#include "drivers/bus_spi.h" -#include "drivers/light_led.h" -#include "drivers/system.h" -#include "drivers/nvic.h" -#include "drivers/dma.h" + +#include "bus_spi.h" +#include "light_led.h" +#include "io.h" +#include "system.h" +#include "nvic.h" +#include "dma.h" #include "max7456.h" #include "max7456_symbols.h" diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 29a5d02f15..c017061835 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -17,7 +17,7 @@ #pragma once -#include "timer.h" +#include "io_types.h" #define MAX_PWM_MOTORS 12 #define MAX_PWM_SERVOS 8 @@ -88,10 +88,11 @@ typedef enum { PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4) } pwmPortFlags_e; +struct timerHardware_s; typedef struct pwmPortConfiguration_s { uint8_t index; pwmPortFlags_e flags; - const timerHardware_t *timerHardware; + const struct timerHardware_s *timerHardware; } pwmPortConfiguration_t; typedef struct pwmOutputConfiguration_s { diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 3c72d104dc..60af85e97b 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -30,7 +30,7 @@ #include "system.h" #include "nvic.h" -#include "gpio.h" +#include "io.h" #include "timer.h" #include "pwm_output.h" diff --git a/src/main/drivers/rcc.h b/src/main/drivers/rcc.h index fb04ec7e2c..69dce965f9 100644 --- a/src/main/drivers/rcc.h +++ b/src/main/drivers/rcc.h @@ -2,6 +2,7 @@ #include "platform.h" #include "common/utils.h" +#include "rcc_types.h" enum rcc_reg { RCC_EMPTY = 0, // make sure that default value (0) does not enable anything @@ -17,9 +18,6 @@ enum rcc_reg { #define RCC_APB1(periph) RCC_ENCODE(RCC_APB1, RCC_APB1ENR_ ## periph ## EN) #define RCC_AHB1(periph) RCC_ENCODE(RCC_AHB1, RCC_AHB1ENR_ ## periph ## EN) -typedef uint8_t rccPeriphTag_t; - void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState); void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState); - diff --git a/src/main/drivers/rcc_types.h b/src/main/drivers/rcc_types.h new file mode 100644 index 0000000000..1b7d7b9f2f --- /dev/null +++ b/src/main/drivers/rcc_types.h @@ -0,0 +1,4 @@ +#pragma once + +typedef uint8_t rccPeriphTag_t; + diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 878d5eacf6..a86fc74577 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -25,8 +25,8 @@ #include "nvic.h" #include "io.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "bus_spi.h" +#include "system.h" #include "sdcard.h" #include "sdcard_standard.h" diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 2c2cebbac6..50c6055ddd 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -22,7 +22,7 @@ #include "build_config.h" #include "common/utils.h" -#include "drivers/io.h" +#include "io.h" #include "usb_core.h" #ifdef STM32F4 @@ -32,7 +32,7 @@ #include "hw_config.h" #endif -#include "drivers/system.h" +#include "system.h" #include "serial.h" #include "serial_usb_vcp.h" diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h index d734f62f4f..cb3f38c989 100644 --- a/src/main/drivers/sonar_hcsr04.h +++ b/src/main/drivers/sonar_hcsr04.h @@ -17,8 +17,7 @@ #pragma once -#include "platform.h" -#include "io.h" +#include "io_types.h" typedef struct sonarHardware_s { ioTag_t triggerTag; diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index ab7a7c3dfc..9caad4f554 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -17,7 +17,7 @@ #pragma once -#include "drivers/io.h" +#include "io_types.h" #ifdef BEEPER #define BEEP_TOGGLE systemBeepToggle() diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 15e36cce3f..5051588c99 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -25,7 +25,7 @@ #include "nvic.h" -#include "gpio.h" +#include "io.h" #include "rcc.h" #include "system.h" diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 7050bbabda..00410a9c68 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -17,12 +17,11 @@ #pragma once -#include "io.h" -#include "rcc.h" +#include +#include -#if !defined(USABLE_TIMER_CHANNEL_COUNT) -#define USABLE_TIMER_CHANNEL_COUNT 14 -#endif +#include "io_types.h" +#include "rcc_types.h" typedef uint16_t captureCompare_t; // 16 bit on both 103 and 303, just register access must be 32bit sometimes (use timCCR_t) diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index 1a5085e63a..49106818f0 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -5,6 +5,11 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f10x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 04069b20c3..99111bad77 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -5,6 +5,11 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f30x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index af09f85e1b..ccce10350a 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -5,9 +5,14 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f4xx.h" -#include "timer.h" #include "rcc.h" +#include "timer.h" /** * @brief Selects the TIM Output Compare Mode. diff --git a/src/main/drivers/transponder_ir.c b/src/main/drivers/transponder_ir.c index 626c60097c..dbc6749af7 100644 --- a/src/main/drivers/transponder_ir.c +++ b/src/main/drivers/transponder_ir.c @@ -21,9 +21,9 @@ #include -#include "drivers/dma.h" -#include "drivers/nvic.h" -#include "drivers/transponder_ir.h" +#include "dma.h" +#include "nvic.h" +#include "transponder_ir.h" /* * Implementation note: diff --git a/src/main/drivers/transponder_ir_stm32f30x.c b/src/main/drivers/transponder_ir_stm32f30x.c index 3962e1c999..0bff2e112c 100644 --- a/src/main/drivers/transponder_ir_stm32f30x.c +++ b/src/main/drivers/transponder_ir_stm32f30x.c @@ -20,9 +20,9 @@ #include -#include "drivers/gpio.h" -#include "drivers/transponder_ir.h" -#include "drivers/nvic.h" +#include "gpio.h" +#include "transponder_ir.h" +#include "nvic.h" #ifndef TRANSPONDER_GPIO #define USE_TRANSPONDER_ON_DMA1_CHANNEL3 diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index 93ffa83ecb..08ec758e8d 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -31,9 +31,9 @@ #include "common/maths.h" -#include "drivers/vtx_rtc6705.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "vtx_rtc6705.h" +#include "bus_spi.h" +#include "system.h" #define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400 #define RTC6705_SET_A1 0x8F3031 //5865 diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.c b/src/main/drivers/vtx_soft_spi_rtc6705.c index 4ceba50df9..e6340680fa 100644 --- a/src/main/drivers/vtx_soft_spi_rtc6705.c +++ b/src/main/drivers/vtx_soft_spi_rtc6705.c @@ -22,9 +22,10 @@ #ifdef USE_RTC6705 -#include "drivers/bus_spi.h" -#include "drivers/system.h" -#include "drivers/light_led.h" +#include "bus_spi.h" +#include "io.h" +#include "system.h" +#include "light_led.h" #include "vtx_soft_spi_rtc6705.h" diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index f0f5525813..223d3e10cd 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -25,8 +25,10 @@ #include "platform.h" #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +#include "drivers/io_types.h" #include "drivers/system.h" #include "drivers/serial.h" +#include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "io/serial.h" #include "io/serial_msp.h" diff --git a/src/main/main.c b/src/main/main.c index 24b983308d..436f5b5798 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -33,6 +33,7 @@ #include "drivers/system.h" #include "drivers/dma.h" #include "drivers/gpio.h" +#include "drivers/io.h" #include "drivers/light_led.h" #include "drivers/sound_beeper.h" #include "drivers/timer.h" diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index bb46059ac1..c3465b3934 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,7 +24,7 @@ #include "common/axis.h" -#include "drivers/gpio.h" +#include "drivers/io.h" #include "drivers/system.h" #include "drivers/exti.h" diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index e780364ec2..f950c67844 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index 378627daaf..b5b9889379 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c index 7116b53551..dec8739ba0 100644 --- a/src/main/target/ALIENFLIGHTF1/target.c +++ b/src/main/target/ALIENFLIGHTF1/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 6d945008f7..04984912cd 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -90,4 +90,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index c1946aa17c..8169e9e912 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 28a896ff01..d2e9e92ba4 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -35,8 +35,6 @@ #define BEEPER PA5 -#define USABLE_TIMER_CHANNEL_COUNT 11 - #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_EXTI @@ -131,5 +129,6 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 11 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index efedb56689..dafcdf86da 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index b85e50f539..83e0700cd5 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index a870de9e24..589c6af30d 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 631d83e6aa..c2aca46716 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index 0f8228559e..3562e3234d 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 973e0984f9..beb4be9657 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -71,4 +71,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 91b21c8b36..099f9b62e7 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index 05b642f088..ca13ef7d0b 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 2399ede8d1..3f40c4ab43 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -67,11 +67,6 @@ #define M25P16_CS_PIN PC15 #define M25P16_SPI_INSTANCE SPI2 -// timer definitions in drivers/timer.c -// channel mapping in drivers/pwm_mapping.c -// only 6 outputs available on hardware -#define USABLE_TIMER_CHANNEL_COUNT 9 - #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 @@ -147,5 +142,9 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +// timer definitions in drivers/timer.c +// channel mapping in drivers/pwm_mapping.c +// only 6 outputs available on hardware +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/EUSTM32F103RC/target.c b/src/main/target/EUSTM32F103RC/target.c index b2a0db9a16..0dc2d66044 100644 --- a/src/main/target/EUSTM32F103RC/target.c +++ b/src/main/target/EUSTM32F103RC/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 12ec85f371..d883641d4f 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -113,5 +113,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index a8d80e823d..e9befde0ab 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -3,17 +3,18 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM7 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed @@ -47,13 +48,13 @@ const uint16_t multiPWM[] = { const uint16_t airPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), PWM12 | (MAP_TO_SERVO_OUTPUT << 8), PWM13 | (MAP_TO_SERVO_OUTPUT << 8), PWM14 | (MAP_TO_SERVO_OUTPUT << 8), - PWM15 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed - PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM7 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed @@ -84,23 +85,23 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT - { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT - { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT + { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT + { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // sonar echo if needed }; diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 7c9e89b48b..d8f8154031 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -74,8 +74,6 @@ #define SDCARD_DMA_CHANNEL DMA_Channel_0 -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define USE_VCP #define VBUS_SENSING_PIN PA9 @@ -156,5 +154,6 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) |TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index bf4ff7b20a..db28609d97 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index ac600ea439..a53aa4d447 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 8f69378592..05ff286283 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -101,8 +101,6 @@ #define M25P16_CS_PIN PB3 #define M25P16_SPI_INSTANCE SPI3 -#define USABLE_TIMER_CHANNEL_COUNT 5 - #define USE_VCP #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED @@ -167,5 +165,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 5 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index dd87a86410..2e4e12fb0a 100644 --- a/src/main/target/IRCFUSIONF3/target.c +++ b/src/main/target/IRCFUSIONF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 8dbd6e55ba..9ceabb35d2 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -23,8 +23,6 @@ #define LED0 PB3 -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready, no MAG #define USE_MPU_DATA_READY_SIGNAL @@ -105,5 +103,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index cbb9d74818..a443fe862f 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM7 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 05346145d6..e532b9d782 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -28,8 +28,6 @@ #define BEEPER PB13 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 12 - #define USE_EXTI #define MPU_INT_EXTI PB2 #define USE_MPU_DATA_READY_SIGNAL @@ -84,4 +82,5 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTF (BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 91b21c8b36..099f9b62e7 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/MICROSCISKY/target.c b/src/main/target/MICROSCISKY/target.c index 7116b53551..dec8739ba0 100644 --- a/src/main/target/MICROSCISKY/target.c +++ b/src/main/target/MICROSCISKY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 80d4f04856..9138f74e86 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -97,4 +97,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index a89063cc9a..cc898ba834 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index 7116b53551..dec8739ba0 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 0e8ba62be2..7e3705b47a 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -163,4 +163,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/OLIMEXINO/target.c b/src/main/target/OLIMEXINO/target.c index ea20267b3c..5d0a271c5b 100644 --- a/src/main/target/OLIMEXINO/target.c +++ b/src/main/target/OLIMEXINO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index bb270cb4fc..ae8ac3b38e 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -91,4 +91,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 977d623f85..a3cd3e54d3 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 7dd021055e..2ed3eab8fb 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index a89063cc9a..cc898ba834 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 111f6fc476..7a104b7466 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -28,8 +28,6 @@ #define BEEPER PA0 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 9 - // MPU6000 interrupts #define USE_EXTI #define MPU_INT_EXTI PA15 @@ -136,5 +134,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/PORT103R/target.c b/src/main/target/PORT103R/target.c index 8f21b7cbb7..093fa6ee27 100644 --- a/src/main/target/PORT103R/target.c +++ b/src/main/target/PORT103R/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 66e83fa3d5..9f0239d572 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -123,5 +123,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 67f7327285..a253a0b31f 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index 9a579f2bc9..bf5023fc24 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index ee96272c11..d344cda357 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index 8131579519..e48d0c33b4 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 09a5678174..ed23489a3a 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -25,8 +25,6 @@ #define BEEPER PC15 -#define USABLE_TIMER_CHANNEL_COUNT 10 - #define USE_EXTI #define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL @@ -115,5 +113,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index b52fbd00e4..7d602c74b1 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM7 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index 389617278b..20b1f185d3 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index ec9176a27f..99f79ec9b7 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 843ad3d9aa..87eb5db35f 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index d1e3a0d8b2..bf8776aa92 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index 1712bebae2..d2c17db5a2 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 592833bc48..a75e952171 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -165,5 +165,6 @@ #define TARGET_IO_PORTE 0xffff #define TARGET_IO_PORTF 0x00ff +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index 85db79093c..a63258100e 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -5,6 +5,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 7838b1f460..9cc822ebee 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -26,8 +26,6 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH @@ -128,5 +126,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/ZCOREF3/target.c b/src/main/target/ZCOREF3/target.c index b72400e6fd..48a6ce60da 100644 --- a/src/main/target/ZCOREF3/target.c +++ b/src/main/target/ZCOREF3/target.c @@ -5,6 +5,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h index c65abb7d92..5c07c78190 100644 --- a/src/main/target/ZCOREF3/target.h +++ b/src/main/target/ZCOREF3/target.h @@ -101,6 +101,5 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 17 // PPM, 8 PWM, UART3 RX/TX, LED Strip - #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 33bd13193c..d698b958a0 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -19,6 +19,8 @@ * Initial FrSky Telemetry implementation by silpstream @ rcgroups. * Addition protocol work by airmamaf @ github. */ + +#include #include #include diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 96e2193e9a..6373d166eb 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#include #include #include @@ -22,7 +23,6 @@ #ifdef TELEMETRY -#include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" From c1c94d67bdf35e91123ae2934eea680323dd98d5 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 01:10:46 +0100 Subject: [PATCH 198/456] Added #pragma once to debug.h --- src/main/debug.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/debug.h b/src/main/debug.h index 2e2a90e89c..6ebbba735a 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -15,6 +15,8 @@ * along with Cleanflight. If not, see . */ +#pragma once + #define DEBUG16_VALUE_COUNT 4 extern int16_t debug[DEBUG16_VALUE_COUNT]; extern uint8_t debugMode; From eb3da3ae2c425f6614d2a12179e861274e4f8b38 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 11:00:40 +0100 Subject: [PATCH 199/456] Removed dependencies on sensors/ --- src/main/drivers/compass_ak8963.c | 3 --- src/main/drivers/compass_ak8975.c | 2 -- src/main/drivers/compass_hmc5883l.c | 2 -- 3 files changed, 7 deletions(-) diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index 0668fcdd77..d5b9a59dec 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -33,9 +33,6 @@ #include "bus_i2c.h" #include "bus_spi.h" -#include "sensors/boardalignment.h" -#include "sensors/sensors.h" - #include "sensor.h" #include "compass.h" diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index 0d648ecc58..0e4d3677c5 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -33,8 +33,6 @@ #include "gpio.h" #include "bus_i2c.h" -#include "sensors/sensors.h" - #include "sensor.h" #include "compass.h" diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 3709cc510e..10fa9b3c6d 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -39,8 +39,6 @@ #include "sensor.h" #include "compass.h" -#include "sensors/sensors.h" - #include "compass_hmc5883l.h" //#define DEBUG_MAG_DATA_READY_INTERRUPT From ec558e8339b73fd6d93ff40286a09c8f9e120441 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 09:02:31 +0100 Subject: [PATCH 200/456] Put each PID in its own .c file --- Makefile | 2 + src/main/flight/pid.c | 387 ++----------------------------- src/main/flight/pid.h | 5 + src/main/flight/pid_betaflight.c | 277 ++++++++++++++++++++++ src/main/flight/pid_legacy.c | 210 +++++++++++++++++ 5 files changed, 509 insertions(+), 372 deletions(-) create mode 100644 src/main/flight/pid_betaflight.c create mode 100644 src/main/flight/pid_legacy.c diff --git a/Makefile b/Makefile index 281b169c63..68364d7b7c 100644 --- a/Makefile +++ b/Makefile @@ -392,6 +392,8 @@ COMMON_SRC = \ flight/imu.c \ flight/mixer.c \ flight/pid.c \ + flight/pid_legacy.c \ + flight/pid_betaflight.c \ io/beeper.c \ io/rc_controls.c \ io/rc_curves.c \ diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b405a9ab4f..03c86f53ab 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -47,32 +47,25 @@ #include "config/runtime_config.h" -extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float setpointRate[3]; -extern float rcInput[3]; -static bool pidStabilisationEnabled; +bool pidStabilisationEnabled; int16_t axisPID[3]; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + #ifdef BLACKBOX int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; +int32_t errorGyroI[3]; +float errorGyroIf[3]; -static int32_t errorGyroI[3]; -static float errorGyroIf[3]; - -static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); #ifdef SKIP_PID_FLOAT pidControllerFuncPtr pid_controller = pidLegacy; // which pid controller are we using #else -static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using #endif @@ -94,7 +87,7 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false; } -float getdT (void) +float getdT(void) { static float dT; if (!dT) dT = (float)targetPidLooptime * 0.000001f; @@ -104,13 +97,15 @@ float getdT (void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static pt1Filter_t deltaFilter[3]; -static pt1Filter_t yawFilter; -static biquadFilter_t dtermFilterLpf[3]; -static biquadFilter_t dtermFilterNotch[3]; -static bool dtermNotchInitialised, dtermBiquadLpfInitialised; +pt1Filter_t deltaFilter[3]; +pt1Filter_t yawFilter; +biquadFilter_t dtermFilterLpf[3]; +biquadFilter_t dtermFilterNotch[3]; +bool dtermNotchInitialised; +bool dtermBiquadLpfInitialised; -void initFilters(const pidProfile_t *pidProfile) { +void initFilters(const pidProfile_t *pidProfile) +{ int axis; if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { @@ -125,358 +120,6 @@ void initFilters(const pidProfile_t *pidProfile) { } } -#ifndef SKIP_PID_FLOAT -// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab) -static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - float errorRate = 0, rP = 0, rD = 0, PVRate = 0; - float ITerm,PTerm,DTerm; - static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; - float delta; - int axis; - float horizonLevelStrength = 1; - - float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection - if(pidProfile->D8[PIDLEVEL] == 0){ - horizonLevelStrength = 0; - } else { - horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); - } - } - - // Yet Highly experimental and under test and development - // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) - static float kiThrottleGain = 1.0f; - if (pidProfile->itermThrottleGain) { - const uint16_t maxLoopCount = 20000 / targetPidLooptime; - const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; - static int16_t previousThrottle; - static uint16_t loopIncrement; - - if (loopIncrement >= maxLoopCount) { - kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 - previousThrottle = rcCommand[THROTTLE]; - loopIncrement = 0; - } else { - loopIncrement++; - } - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - static uint8_t configP[3], configI[3], configD[3]; - - // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now - // Prepare all parameters for PID controller - if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { - Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; - Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; - Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; - b[axis] = pidProfile->ptermSetpointWeight / 100.0f; - c[axis] = pidProfile->dtermSetpointWeight / 100.0f; - yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); - - configP[axis] = pidProfile->P8[axis]; - configI[axis] = pidProfile->I8[axis]; - configD[axis] = pidProfile->D8[axis]; - } - - // Limit abrupt yaw inputs / stops - if (axis == YAW && pidProfile->pidMaxVelocityYaw) { - float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; - if (ABS(yawCurrentVelocity) > yawMaxVelocity) { - setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; - } - yawPreviousRate = setpointRate[axis]; - } - - // Yaw control is GYRO based, direct sticks control is applied to rate PID - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to the max inclination -#ifdef GPS - const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#else - const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; - } else { - // HORIZON mode - direct sticks control is applied to rate PID - // mix up angle error to desired AngleRate to add a little auto-level feel - setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); - } - } - - PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec - - // --------low-level gyro-based PID based on 2DOF PID controller. ---------- - // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // ----- calculate error / angle rates ---------- - errorRate = setpointRate[axis] - PVRate; // r - y - rP = b[axis] * setpointRate[axis] - PVRate; // br - y - - // Slowly restore original setpoint with more stick input - float diffRate = errorRate - rP; - rP += diffRate * rcInput[axis]; - - // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount - float dynReduction = tpaFactor; - if (pidProfile->toleranceBand) { - const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; - static uint8_t zeroCrossCount[3]; - static uint8_t currentErrorPolarity[3]; - if (ABS(errorRate) < pidProfile->toleranceBand) { - if (zeroCrossCount[axis]) { - if (currentErrorPolarity[axis] == POSITIVE_ERROR) { - if (errorRate < 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = NEGATIVE_ERROR; - } - } else { - if (errorRate > 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = POSITIVE_ERROR; - } - } - } else { - dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); - } - } else { - zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; - currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; - } - } - - // -----calculate P component - PTerm = Kp[axis] * rP * dynReduction; - - // -----calculate I component. - // Reduce strong Iterm accumulation during higher stick inputs - float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); - - // Handle All windup Scenarios - // limit maximum integrator value to prevent WindUp - float itermScaler = setpointRateScaler * kiThrottleGain; - - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); - - // I coefficient (I8) moved before integration to make limiting independent from PID settings - ITerm = errorGyroIf[axis]; - - //-----calculate D-term (Yaw D not yet supported) - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = lrintf(PTerm + ITerm); - - DTerm = 0.0f; // needed for blackbox - } else { - rD = c[axis] * setpointRate[axis] - PVRate; // cr - y - delta = rD - lastRateError[axis]; - lastRateError[axis] = rD; - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta *= (1.0f / getdT()); - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; - - // Filter delta - if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); - - if (pidProfile->dterm_lpf_hz) { - if (dtermBiquadLpfInitialised) { - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } - } - - DTerm = Kd[axis] * delta * dynReduction; - - // -----calculate total PID output - axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); - } - - // Disable PID control at zero throttle - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} -#endif - -// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future -static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - int axis; - int32_t PTerm, ITerm, DTerm, delta; - static int32_t lastRateError[3]; - int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; - - int8_t horizonLevelStrength = 100; - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection - // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. - // For more rate mode increase D and slower flips and rolls will be possible - horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - // -----Get the desired angle rate depending on flight mode - AngleRateTmp = (int32_t)setpointRate[axis]; - - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to max configured inclination -#ifdef GPS - const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#else - const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; - } else { - // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, - // horizonLevelStrength is scaled to the stick input - AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); - } - } - - // --------low-level gyro-based PID. ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // -----calculate scaled error.AngleRates - // multiplication of rcCommand corresponds to changing the sticks scaling here - gyroRate = gyroADC[axis] / 4; - - RateError = AngleRateTmp - gyroRate; - - // -----calculate P component - PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; - - // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply - if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { - PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); - } - - // -----calculate I component - // there should be no division before accumulating the error to integrator, because the precision would be reduced. - // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. - // Time correction (to avoid different I scaling for different builds based on average cycle time) - // is normalized to cycle time = 2048. - // Prevent Accumulation - uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); - uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - - errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; - - // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. - // I coefficient (I8) moved before integration to make limiting independent from PID settings - errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - - ITerm = errorGyroI[axis] >> 13; - - //-----calculate D-term - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = PTerm + ITerm; - - if (motorCount >= 4) { - int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); - - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); - } - - DTerm = 0; // needed for blackbox - } else { - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastRateError[axis]; - lastRateError[axis] = RateError; - } else { - delta = -(gyroRate - lastRateError[axis]); - lastRateError[axis] = gyroRate; - } - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // Filter delta - if (pidProfile->dterm_lpf_hz) { - float deltaf = delta; // single conversion - if (dtermBiquadLpfInitialised) { - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } - delta = lrintf(deltaf); - } - - DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // -----calculate total PID output - axisPID[axis] = PTerm + ITerm + DTerm; - } - - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} void pidSetController(pidControllerType_e type) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4f3623c319..79e83a2c04 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -117,6 +117,11 @@ struct rxConfig_s; typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype +void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); +void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); + extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool airmodeWasActivated; diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c new file mode 100644 index 0000000000..72cdd4ea4c --- /dev/null +++ b/src/main/flight/pid_betaflight.c @@ -0,0 +1,277 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#ifndef SKIP_PID_FLOAT + +#include "build_config.h" +#include "debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" + +#include "drivers/accgyro.h" +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/gtune.h" + +#include "config/runtime_config.h" + +extern float rcInput[3]; +extern float setpointRate[3]; + +extern float errorGyroIf[3]; +extern bool pidStabilisationEnabled; + +extern pt1Filter_t deltaFilter[3]; +extern pt1Filter_t yawFilter; +extern biquadFilter_t dtermFilterLpf[3]; +extern biquadFilter_t dtermFilterNotch[3]; +extern bool dtermNotchInitialised; +extern bool dtermBiquadLpfInitialised; + +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + +void initFilters(const pidProfile_t *pidProfile); +float getdT(void); + +// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. +// Based on 2DOF reference design (matlab) +void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) +{ + float errorRate = 0, rP = 0, rD = 0, PVRate = 0; + float ITerm,PTerm,DTerm; + static float lastRateError[2]; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; + float delta; + int axis; + float horizonLevelStrength = 1; + + float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection + if(pidProfile->D8[PIDLEVEL] == 0){ + horizonLevelStrength = 0; + } else { + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); + } + } + + // Yet Highly experimental and under test and development + // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) + static float kiThrottleGain = 1.0f; + if (pidProfile->itermThrottleGain) { + const uint16_t maxLoopCount = 20000 / targetPidLooptime; + const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; + static int16_t previousThrottle; + static uint16_t loopIncrement; + + if (loopIncrement >= maxLoopCount) { + kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 + previousThrottle = rcCommand[THROTTLE]; + loopIncrement = 0; + } else { + loopIncrement++; + } + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + static uint8_t configP[3], configI[3], configD[3]; + + // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now + // Prepare all parameters for PID controller + if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { + Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; + Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; + Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; + b[axis] = pidProfile->ptermSetpointWeight / 100.0f; + c[axis] = pidProfile->dtermSetpointWeight / 100.0f; + yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); + + configP[axis] = pidProfile->P8[axis]; + configI[axis] = pidProfile->I8[axis]; + configD[axis] = pidProfile->D8[axis]; + } + + // Limit abrupt yaw inputs + if (axis == YAW && pidProfile->pidMaxVelocityYaw) { + float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; + if (ABS(yawCurrentVelocity) > yawMaxVelocity) { + setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; + } + yawPreviousRate = setpointRate[axis]; + } + + // Yaw control is GYRO based, direct sticks control is applied to rate PID + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to the max inclination +#ifdef GPS + const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#else + const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; + } else { + // HORIZON mode - direct sticks control is applied to rate PID + // mix up angle error to desired AngleRate to add a little auto-level feel + setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); + } + } + + PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec + + // --------low-level gyro-based PID based on 2DOF PID controller. ---------- + // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // ----- calculate error / angle rates ---------- + errorRate = setpointRate[axis] - PVRate; // r - y + rP = b[axis] * setpointRate[axis] - PVRate; // br - y + + // Slowly restore original setpoint with more stick input + float diffRate = errorRate - rP; + rP += diffRate * rcInput[axis]; + + // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount + float dynReduction = tpaFactor; + if (pidProfile->toleranceBand) { + const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; + static uint8_t zeroCrossCount[3]; + static uint8_t currentErrorPolarity[3]; + if (ABS(errorRate) < pidProfile->toleranceBand) { + if (zeroCrossCount[axis]) { + if (currentErrorPolarity[axis] == POSITIVE_ERROR) { + if (errorRate < 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = NEGATIVE_ERROR; + } + } else { + if (errorRate > 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = POSITIVE_ERROR; + } + } + } else { + dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); + } + } else { + zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; + currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; + } + } + + // -----calculate P component + PTerm = Kp[axis] * rP * dynReduction; + + // -----calculate I component. + // Reduce strong Iterm accumulation during higher stick inputs + float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); + + // Handle All windup Scenarios + // limit maximum integrator value to prevent WindUp + float itermScaler = setpointRateScaler * kiThrottleGain; + + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); + + // I coefficient (I8) moved before integration to make limiting independent from PID settings + ITerm = errorGyroIf[axis]; + + //-----calculate D-term (Yaw D not yet supported) + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = lrintf(PTerm + ITerm); + + DTerm = 0.0f; // needed for blackbox + } else { + rD = c[axis] * setpointRate[axis] - PVRate; // cr - y + delta = rD - lastRateError[axis]; + lastRateError[axis] = rD; + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta *= (1.0f / getdT()); + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; + + // Filter delta + if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); + + if (pidProfile->dterm_lpf_hz) { + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + } + + DTerm = Kd[axis] * delta * dynReduction; + + // -----calculate total PID output + axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); + } + + // Disable PID control at zero throttle + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif + } +} +#endif + diff --git a/src/main/flight/pid_legacy.c b/src/main/flight/pid_legacy.c new file mode 100644 index 0000000000..0424f5461f --- /dev/null +++ b/src/main/flight/pid_legacy.c @@ -0,0 +1,210 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#include "build_config.h" +#include "debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" + +#include "drivers/accgyro.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/gtune.h" + +#include "config/runtime_config.h" + +extern uint8_t motorCount; +extern uint8_t PIDweight[3]; +extern bool pidStabilisationEnabled; +extern float setpointRate[3]; +extern int32_t errorGyroI[3]; +extern pt1Filter_t deltaFilter[3]; +extern pt1Filter_t yawFilter; +extern biquadFilter_t dtermFilterLpf[3]; +extern bool dtermBiquadLpfInitialised; + + +void initFilters(const pidProfile_t *pidProfile); +float getdT(void); + + +// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. +// Don't expect much development in the future +void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) +{ + int axis; + int32_t PTerm, ITerm, DTerm, delta; + static int32_t lastRateError[3]; + int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; + + int8_t horizonLevelStrength = 100; + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection + // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. + // For more rate mode increase D and slower flips and rolls will be possible + horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + // -----Get the desired angle rate depending on flight mode + AngleRateTmp = (int32_t)setpointRate[axis]; + + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to max configured inclination +#ifdef GPS + const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +#else + const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; + } else { + // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, + // horizonLevelStrength is scaled to the stick input + AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); + } + } + + // --------low-level gyro-based PID. ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // -----calculate scaled error.AngleRates + // multiplication of rcCommand corresponds to changing the sticks scaling here + gyroRate = gyroADC[axis] / 4; + + RateError = AngleRateTmp - gyroRate; + + // -----calculate P component + PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + + // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply + if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { + PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); + } + + // -----calculate I component + // there should be no division before accumulating the error to integrator, because the precision would be reduced. + // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. + // Time correction (to avoid different I scaling for different builds based on average cycle time) + // is normalized to cycle time = 2048. + // Prevent Accumulation + uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); + uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; + + errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; + + // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. + // I coefficient (I8) moved before integration to make limiting independent from PID settings + errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); + + ITerm = errorGyroI[axis] >> 13; + + //-----calculate D-term + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = PTerm + ITerm; + + if (motorCount >= 4) { + int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); + + // prevent "yaw jump" during yaw correction + axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); + } + + DTerm = 0; // needed for blackbox + } else { + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = RateError - lastRateError[axis]; + lastRateError[axis] = RateError; + } else { + delta = -(gyroRate - lastRateError[axis]); + lastRateError[axis] = gyroRate; + } + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // Filter delta + if (pidProfile->dterm_lpf_hz) { + float deltaf = delta; // single conversion + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + delta = lrintf(deltaf); + } + + DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // -----calculate total PID output + axisPID[axis] = PTerm + ITerm + DTerm; + } + + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif + } +} + From 64dc6b6b1a3def12c1705ce250c5f518af5ea7b3 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 14:20:39 +0200 Subject: [PATCH 201/456] Move PIDweight to a common place. pid_legacy should not depend on pid_betaflight. --- src/main/flight/pid.h | 3 +++ src/main/flight/pid_betaflight.c | 3 --- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 79e83a2c04..04e129b9cf 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -127,6 +127,9 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool airmodeWasActivated; extern uint32_t targetPidLooptime; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +extern uint8_t PIDweight[3]; + void pidSetController(pidControllerType_e type); void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index 72cdd4ea4c..eae0f4c17b 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -62,9 +62,6 @@ extern biquadFilter_t dtermFilterNotch[3]; extern bool dtermNotchInitialised; extern bool dtermBiquadLpfInitialised; -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; - void initFilters(const pidProfile_t *pidProfile); float getdT(void); From d2600ed95b6632198be512bdf5dcb5665669a7f5 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 15:16:30 +0200 Subject: [PATCH 202/456] Added missing include. Target VRRACE build failed. --- src/main/target/VRRACE/target.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/VRRACE/target.c b/src/main/target/VRRACE/target.c index 094ade421e..90270b1fa8 100644 --- a/src/main/target/VRRACE/target.c +++ b/src/main/target/VRRACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input From 38e812a5a505323212b8f9ea48c8f484e3d23d10 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 5 Aug 2016 00:55:12 +0200 Subject: [PATCH 203/456] Add roll pitch velocity --- src/main/config/config.c | 3 ++- src/main/flight/pid.c | 14 ++++++++------ src/main/flight/pid.h | 3 ++- src/main/io/serial_cli.c | 5 +++-- src/main/io/serial_msp.c | 4 ++-- 5 files changed, 17 insertions(+), 12 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 2f047d64e4..f1a88d02f5 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 142; +static const uint8_t EEPROM_CONF_VERSION = 143; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -246,6 +246,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->ptermSetpointWeight = 75; pidProfile->dtermSetpointWeight = 120; pidProfile->pidMaxVelocityYaw = 200; + pidProfile->pidMaxVelocityRollPitch = 0; pidProfile->toleranceBand = 20; pidProfile->toleranceBandReduction = 40; pidProfile->zeroCrossAllowanceCount = 2; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b405a9ab4f..e147423412 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -133,7 +133,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; float delta; int axis; float horizonLevelStrength = 1; @@ -188,6 +188,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc b[axis] = pidProfile->ptermSetpointWeight / 100.0f; c[axis] = pidProfile->dtermSetpointWeight / 100.0f; yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); + rollPitchMaxVelocity = pidProfile->pidMaxVelocityRollPitch * 1000 * getdT(); configP[axis] = pidProfile->P8[axis]; configI[axis] = pidProfile->I8[axis]; @@ -195,12 +196,13 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc } // Limit abrupt yaw inputs / stops - if (axis == YAW && pidProfile->pidMaxVelocityYaw) { - float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; - if (ABS(yawCurrentVelocity) > yawMaxVelocity) { - setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; + float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; + if (maxVelocity) { + float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; + if (ABS(currentVelocity) > maxVelocity) { + setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; } - yawPreviousRate = setpointRate[axis]; + previousSetpoint[axis] = setpointRate[axis]; } // Yaw control is GYRO based, direct sticks control is applied to rate PID diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4f3623c319..9aa625a33b 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -100,7 +100,8 @@ typedef struct pidProfile_s { uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm uint8_t ptermSetpointWeight; // Setpoint weight for Pterm (lower means more PV tracking) uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) - uint16_t pidMaxVelocityYaw; // velocity limiter for pid controller deg/sec/ms + uint16_t pidMaxVelocityYaw; // velocity yaw limiter for pid controller deg/sec/ms + uint16_t pidMaxVelocityRollPitch; // velocity roll/pitch limiter for pid controller deg/sec/ms #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5aef0f5392..15de603629 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -773,8 +773,8 @@ const clivalue_t valueTable[] = { { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, #endif - { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 250 } }, - { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 250 } }, + { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 300 } }, + { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 300 } }, { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, @@ -833,6 +833,7 @@ const clivalue_t valueTable[] = { { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 300 } }, { "max_yaw_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 1000 } }, + { "max_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityRollPitch, .config.minmax = {0, 1000 } }, { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index b5d170cfbc..0d36fda478 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1268,8 +1268,8 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentProfile->pidProfile.toleranceBand); serialize8(currentProfile->pidProfile.toleranceBandReduction); serialize8(currentProfile->pidProfile.itermThrottleGain); + serialize16(currentProfile->pidProfile.pidMaxVelocityRollPitch); serialize16(currentProfile->pidProfile.pidMaxVelocityYaw); - serialize16(1000); // Reserved for future break; case MSP_SENSOR_CONFIG: headSerialReply(3); @@ -1858,8 +1858,8 @@ static bool processInCommand(void) currentProfile->pidProfile.toleranceBand = read8(); currentProfile->pidProfile.toleranceBandReduction = read8(); currentProfile->pidProfile.itermThrottleGain = read8(); + currentProfile->pidProfile.pidMaxVelocityRollPitch = read16(); currentProfile->pidProfile.pidMaxVelocityYaw = read16(); - read16(); // reserved for future purposes break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); From 2427ad147809906b16deb6bc06626f7aa4ceac29 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 5 Aug 2016 13:02:06 +0200 Subject: [PATCH 204/456] New defaults // FIX for slow defaults on SPI targets // refactor --- src/main/blackbox/blackbox.c | 3 ++- src/main/config/config.c | 12 ++++++------ src/main/debug.h | 2 +- src/main/io/serial_cli.c | 14 +++++++------- src/main/io/serial_msp.c | 8 ++++---- src/main/mw.c | 8 ++++---- src/main/rx/rx.h | 4 ++-- 7 files changed, 26 insertions(+), 25 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index a59cc8d78f..406ce05d39 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1230,7 +1230,8 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware); BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.mag_hardware); BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm); - BLACKBOX_PRINT_HEADER_LINE("rc_smooth_interval:%d", masterConfig.rxConfig.rcSmoothInterval); + BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d", masterConfig.rxConfig.rcInterpolation); + BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d", masterConfig.rxConfig.rcInterpolationInterval); BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold); BLACKBOX_PRINT_HEADER_LINE("serialrx_provider:%d", masterConfig.rxConfig.serialrx_provider); BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", masterConfig.use_unsyncedPwm); diff --git a/src/main/config/config.c b/src/main/config/config.c index f1a88d02f5..956aae97f7 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -245,9 +245,9 @@ static void resetPidProfile(pidProfile_t *pidProfile) // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; pidProfile->dtermSetpointWeight = 120; - pidProfile->pidMaxVelocityYaw = 200; + pidProfile->pidMaxVelocityYaw = 220; pidProfile->pidMaxVelocityRollPitch = 0; - pidProfile->toleranceBand = 20; + pidProfile->toleranceBand = 15; pidProfile->toleranceBandReduction = 40; pidProfile->zeroCrossAllowanceCount = 2; pidProfile->itermThrottleGain = 0; @@ -476,14 +476,14 @@ static void resetConf(void) masterConfig.gyro_sync_denom = 8; masterConfig.pid_process_denom = 1; #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) - masterConfig.gyro_sync_denom = 8; + masterConfig.gyro_sync_denom = 1; masterConfig.pid_process_denom = 4; #else masterConfig.gyro_sync_denom = 4; masterConfig.pid_process_denom = 2; #endif masterConfig.gyro_soft_type = FILTER_PT1; - masterConfig.gyro_soft_lpf_hz = 100; + masterConfig.gyro_soft_lpf_hz = 90; masterConfig.gyro_soft_notch_hz = 0; masterConfig.gyro_soft_notch_cutoff = 150; @@ -535,8 +535,8 @@ static void resetConf(void) masterConfig.rxConfig.rssi_channel = 0; masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; masterConfig.rxConfig.rssi_ppm_invert = 0; - masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_AUTO; - masterConfig.rxConfig.rcSmoothInterval = 9; + masterConfig.rxConfig.rcInterpolation = RC_SMOOTHING_AUTO; + masterConfig.rxConfig.rcInterpolationInterval = 19; masterConfig.rxConfig.fpvCamAngleDegrees = 0; masterConfig.rxConfig.max_aux_channel = MAX_AUX_CHANNELS; masterConfig.rxConfig.airModeActivateThreshold = 1350; diff --git a/src/main/debug.h b/src/main/debug.h index 2e2a90e89c..4431953f31 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -51,7 +51,7 @@ typedef enum { DEBUG_AIRMODE, DEBUG_PIDLOOP, DEBUG_NOTCH, - DEBUG_RC_SMOOTHING, + DEBUG_RC_INTERPOLATION, DEBUG_VELOCITY, DEBUG_DTERM_FILTER, DEBUG_COUNT diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 15de603629..597637a9d1 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -467,7 +467,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "AIRMODE", "PIDLOOP", "NOTCH", - "RC_SMOOTHING", + "RC_INTERPOLATION", "VELOCITY", "DFILTER", }; @@ -492,8 +492,8 @@ static const char * const lookupTableDeltaMethod[] = { "ERROR", "MEASUREMENT" }; -static const char * const lookupTableRcSmoothing[] = { - "OFF", "DEFAULT", "AUTO", "MANUAL" +static const char * const lookupTableRcInterpolation[] = { + "OFF", "PRESET", "AUTO", "MANUAL" }; static const char * const lookupTableLowpassType[] = { @@ -536,7 +536,7 @@ typedef enum { TABLE_SUPEREXPO_YAW, TABLE_MOTOR_PWM_PROTOCOL, TABLE_DELTA_METHOD, - TABLE_RC_SMOOTHING, + TABLE_RC_INTERPOLATION, TABLE_LOWPASS_TYPE, #ifdef OSD TABLE_OSD, @@ -574,7 +574,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) }, { lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) }, { lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) }, - { lookupTableRcSmoothing, sizeof(lookupTableRcSmoothing) / sizeof(char *) }, + { lookupTableRcInterpolation, sizeof(lookupTableRcInterpolation) / sizeof(char *) }, { lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) }, #ifdef OSD { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, @@ -635,8 +635,8 @@ const clivalue_t valueTable[] = { { "max_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT } }, { "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } }, - { "rc_smoothing", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_RC_SMOOTHING } }, - { "rc_smooth_interval_ms", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rcSmoothInterval, .config.minmax = { 1, 50 } }, + { "rc_interpolation", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcInterpolation, .config.lookup = { TABLE_RC_INTERPOLATION } }, + { "rc_interpolation_interval", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rcInterpolationInterval, .config.minmax = { 1, 50 } }, { "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } }, { "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON } }, { "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 0d36fda478..6cf6a50057 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1041,8 +1041,8 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.rxConfig.spektrum_sat_bind); serialize16(masterConfig.rxConfig.rx_min_usec); serialize16(masterConfig.rxConfig.rx_max_usec); - serialize8(masterConfig.rxConfig.rcSmoothing); - serialize8(masterConfig.rxConfig.rcSmoothInterval); + serialize8(masterConfig.rxConfig.rcInterpolation); + serialize8(masterConfig.rxConfig.rcInterpolationInterval); serialize16(masterConfig.rxConfig.airModeActivateThreshold); break; @@ -1688,8 +1688,8 @@ static bool processInCommand(void) masterConfig.rxConfig.rx_max_usec = read16(); } if (currentPort->dataSize > 12) { - masterConfig.rxConfig.rcSmoothing = read8(); - masterConfig.rxConfig.rcSmoothInterval = read8(); + masterConfig.rxConfig.rcInterpolation = read8(); + masterConfig.rxConfig.rcInterpolationInterval = read8(); masterConfig.rxConfig.airModeActivateThreshold = read16(); } break; diff --git a/src/main/mw.c b/src/main/mw.c index eded995564..b2c569e98b 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -217,15 +217,15 @@ void processRcCommand(void) uint16_t rxRefreshRate; bool readyToCalculateRate = false; - if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) { + if (masterConfig.rxConfig.rcInterpolation || flightModeFlags) { if (isRXDataNew) { // Set RC refresh rate for sampling and channels to filter - switch (masterConfig.rxConfig.rcSmoothing) { + switch (masterConfig.rxConfig.rcInterpolation) { case(RC_SMOOTHING_AUTO): rxRefreshRate = constrain(getTaskDeltaTime(TASK_RX), 1000, 20000) + 1000; // Add slight overhead to prevent ramps break; case(RC_SMOOTHING_MANUAL): - rxRefreshRate = 1000 * masterConfig.rxConfig.rcSmoothInterval; + rxRefreshRate = 1000 * masterConfig.rxConfig.rcInterpolationInterval; break; case(RC_SMOOTHING_OFF): case(RC_SMOOTHING_DEFAULT): @@ -235,7 +235,7 @@ void processRcCommand(void) rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; - if (debugMode == DEBUG_RC_SMOOTHING) { + if (debugMode == DEBUG_RC_INTERPOLATION) { for (int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis]; debug[3] = rxRefreshRate; } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 1200e5e8f5..effd1cc526 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -121,8 +121,8 @@ typedef struct rxConfig_s { uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t mincheck; // minimum rc end uint16_t maxcheck; // maximum rc end - uint8_t rcSmoothing; - uint8_t rcSmoothInterval; + uint8_t rcInterpolation; + uint8_t rcInterpolationInterval; uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands uint8_t max_aux_channel; uint16_t airModeActivateThreshold; // Throttle setpoint where airmode gets activated From cb72f0a6d68dd885f9ad9e7eef405a94447e154a Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 5 Aug 2016 13:23:52 +0200 Subject: [PATCH 205/456] Match configurator output --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 597637a9d1..19238347af 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -831,7 +831,7 @@ const clivalue_t valueTable[] = { { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } }, - { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 300 } }, + { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 200 } }, { "max_yaw_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 1000 } }, { "max_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityRollPitch, .config.minmax = {0, 1000 } }, From d40c5f80faea450796f6e555603a707a67ba7d38 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 01:37:59 +0200 Subject: [PATCH 206/456] Start on version 3.1.0 --- src/main/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/version.h b/src/main/version.h index 0f770674c1..c3c6529ed3 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,7 +16,7 @@ */ #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 0 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x From 7b8bc40f2192d108158fa31c81425cb9407d38fa Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 00:55:16 +0100 Subject: [PATCH 207/456] Optimisation of driver header files --- src/main/config/config.c | 1 + src/main/drivers/accgyro_mma845x.c | 2 +- src/main/drivers/adc.c | 2 + src/main/drivers/adc.h | 2 +- src/main/drivers/adc_impl.h | 6 +-- src/main/drivers/barometer_spi_bmp280.c | 2 + src/main/drivers/bus_i2c.h | 4 +- src/main/drivers/bus_i2c_soft.c | 2 +- src/main/drivers/bus_i2c_stm32f30x.c | 5 +- src/main/drivers/bus_spi.h | 5 +- src/main/drivers/compass_hmc5883l.h | 2 +- src/main/drivers/dma.c | 4 +- src/main/drivers/dma_stm32f4xx.c | 4 +- src/main/drivers/flash_m25p16.c | 7 +-- src/main/drivers/flash_m25p16.h | 2 +- src/main/drivers/gyro_sync.c | 6 +-- src/main/drivers/io.h | 24 +-------- src/main/drivers/io_types.h | 28 +++++++++++ src/main/drivers/light_ws2811strip.c | 4 +- .../drivers/light_ws2811strip_stm32f10x.c | 3 +- .../drivers/light_ws2811strip_stm32f30x.c | 2 +- src/main/drivers/max7456.c | 15 +++--- src/main/drivers/pwm_mapping.h | 5 +- src/main/drivers/pwm_rx.c | 2 +- src/main/drivers/rcc.h | 4 +- src/main/drivers/rcc_types.h | 4 ++ src/main/drivers/sdcard.c | 4 +- src/main/drivers/serial_usb_vcp.c | 4 +- src/main/drivers/sonar_hcsr04.h | 3 +- src/main/drivers/sound_beeper.h | 2 +- src/main/drivers/timer.c | 2 +- src/main/drivers/timer.h | 9 ++-- src/main/drivers/timer_stm32f10x.c | 5 ++ src/main/drivers/timer_stm32f30x.c | 5 ++ src/main/drivers/timer_stm32f4xx.c | 7 ++- src/main/drivers/transponder_ir.c | 6 +-- src/main/drivers/transponder_ir_stm32f30x.c | 6 +-- src/main/drivers/vtx_rtc6705.c | 6 +-- src/main/drivers/vtx_soft_spi_rtc6705.c | 7 +-- src/main/io/serial_4way_avrootloader.c | 2 + src/main/main.c | 1 + src/main/sensors/initialisation.c | 2 +- src/main/target/AIORACERF3/target.c | 1 + src/main/target/AIR32/target.c | 1 + src/main/target/ALIENFLIGHTF1/target.c | 1 + src/main/target/ALIENFLIGHTF1/target.h | 1 + src/main/target/ALIENFLIGHTF3/target.c | 1 + src/main/target/ALIENFLIGHTF3/target.h | 3 +- src/main/target/ALIENFLIGHTF4/target.c | 1 + src/main/target/BLUEJAYF4/target.c | 1 + src/main/target/CC3D/target.c | 1 + src/main/target/CHEBUZZF3/target.c | 1 + src/main/target/CJMCU/target.c | 1 + src/main/target/CJMCU/target.h | 1 + src/main/target/COLIBRI_RACE/target.c | 1 + src/main/target/DOGE/target.c | 1 + src/main/target/DOGE/target.h | 9 ++-- src/main/target/EUSTM32F103RC/target.c | 1 + src/main/target/EUSTM32F103RC/target.h | 1 + src/main/target/F4BY/target.c | 49 ++++++++++--------- src/main/target/F4BY/target.h | 3 +- src/main/target/FURYF3/target.c | 1 + src/main/target/FURYF4/target.c | 1 + src/main/target/FURYF4/target.h | 3 +- src/main/target/IRCFUSIONF3/target.c | 1 + src/main/target/IRCFUSIONF3/target.h | 3 +- src/main/target/KISSFC/target.c | 1 + src/main/target/KISSFC/target.h | 3 +- src/main/target/LUX_RACE/target.c | 1 + src/main/target/MICROSCISKY/target.c | 1 + src/main/target/MICROSCISKY/target.h | 1 + src/main/target/MOTOLAB/target.c | 1 + src/main/target/NAZE/target.c | 1 + src/main/target/NAZE/target.h | 1 + src/main/target/OLIMEXINO/target.c | 1 + src/main/target/OLIMEXINO/target.h | 1 + src/main/target/OMNIBUS/target.c | 1 + src/main/target/OMNIBUSF4/target.c | 1 + src/main/target/PIKOBLX/target.c | 1 + src/main/target/PIKOBLX/target.h | 3 +- src/main/target/PORT103R/target.c | 1 + src/main/target/PORT103R/target.h | 1 + src/main/target/REVO/target.c | 1 + src/main/target/REVONANO/target.c | 1 + src/main/target/RMDO/target.c | 1 + src/main/target/SINGULARITY/target.c | 1 + src/main/target/SINGULARITY/target.h | 3 +- src/main/target/SIRINFPV/target.c | 1 + src/main/target/SPARKY/target.c | 1 + src/main/target/SPRACINGF3/target.c | 1 + src/main/target/SPRACINGF3EVO/target.c | 1 + src/main/target/SPRACINGF3MINI/target.c | 1 + src/main/target/STM32F3DISCOVERY/target.c | 1 + src/main/target/STM32F3DISCOVERY/target.h | 1 + src/main/target/X_RACERSPI/target.c | 1 + src/main/target/X_RACERSPI/target.h | 3 +- src/main/target/ZCOREF3/target.c | 1 + src/main/target/ZCOREF3/target.h | 1 - src/main/telemetry/frsky.c | 2 + src/main/telemetry/telemetry.c | 2 +- 100 files changed, 212 insertions(+), 138 deletions(-) create mode 100644 src/main/drivers/io_types.h create mode 100644 src/main/drivers/rcc_types.h diff --git a/src/main/config/config.c b/src/main/config/config.c index 956aae97f7..592f97e726 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -23,6 +23,7 @@ #include "debug.h" #include "build_config.h" +#include "debug.h" #include "blackbox/blackbox_io.h" diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index 202b90e804..69a82a0adb 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -21,7 +21,7 @@ #include "platform.h" #include "system.h" -#include "gpio.h" +#include "io.h" #include "bus_i2c.h" #include "sensor.h" diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index a65abf9128..6bba109396 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -27,6 +27,8 @@ #include "adc.h" #include "adc_impl.h" +#include "common/utils.h" + //#define DEBUG_ADC_CHANNELS #ifdef USE_ADC diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 55914a3365..47a4b52a0e 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -17,7 +17,7 @@ #pragma once -#include "io.h" +#include "io_types.h" typedef enum { ADC_BATTERY = 0, diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 146ada62be..c89dc1bf53 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -17,8 +17,8 @@ #pragma once -#include "io.h" -#include "rcc.h" +#include "io_types.h" +#include "rcc_types.h" #if defined(STM32F4) #define ADC_TAG_MAP_COUNT 16 @@ -65,4 +65,4 @@ extern const adcTagMap_t adcTagMap[ADC_TAG_MAP_COUNT]; extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; -uint8_t adcChannelByTag(ioTag_t ioTag); \ No newline at end of file +uint8_t adcChannelByTag(ioTag_t ioTag); diff --git a/src/main/drivers/barometer_spi_bmp280.c b/src/main/drivers/barometer_spi_bmp280.c index 9d97343e11..0c97684972 100644 --- a/src/main/drivers/barometer_spi_bmp280.c +++ b/src/main/drivers/barometer_spi_bmp280.c @@ -15,6 +15,7 @@ * along with Betaflight. If not, see . */ +#include #include #include @@ -24,6 +25,7 @@ #include "barometer.h" #include "barometer_bmp280.h" +#include "io.h" #ifdef USE_BARO_SPI_BMP280 #define DISABLE_BMP280 IOHi(bmp280CsPin) diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index 86f7a66181..26cce3792f 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -21,8 +21,8 @@ #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) #define I2C_DEFAULT_TIMEOUT I2C_SHORT_TIMEOUT -#include "drivers/io.h" -#include "drivers/rcc.h" +#include "io_types.h" +#include "rcc_types.h" #ifndef I2C_DEVICE #define I2C_DEVICE I2CINVALID diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c index 104c77c379..42ea8cadc9 100644 --- a/src/main/drivers/bus_i2c_soft.c +++ b/src/main/drivers/bus_i2c_soft.c @@ -22,7 +22,7 @@ #include "build_config.h" #include "bus_i2c.h" -#include "drivers/io.h" +#include "io.h" // Software I2C driver, using same pins as hardware I2C, with hw i2c module disabled. // Can be configured for I2C2 pinout (SCL: PB10, SDA: PB11) or I2C1 pinout (SCL: PB6, SDA: PB7) diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 88b267f111..f8c5b23697 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -20,9 +20,10 @@ #include -#include "gpio.h" #include "system.h" -#include "drivers/io_impl.h" +#include "io.h" +#include "io_impl.h" +#include "rcc.h" #include "bus_i2c.h" diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 5a6ea7dd37..58c76c9d78 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -17,9 +17,8 @@ #pragma once -#include -#include "io.h" -#include "rcc.h" +#include "io_types.h" +#include "rcc_types.h" #if defined(STM32F4) || defined(STM32F3) #define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index 035a2c936f..e2f71fd28d 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -17,7 +17,7 @@ #pragma once -#include "io.h" +#include "io_types.h" typedef struct hmc5883Config_s { ioTag_t intTag; diff --git a/src/main/drivers/dma.c b/src/main/drivers/dma.c index 0ad411312e..7e4942fbe1 100644 --- a/src/main/drivers/dma.c +++ b/src/main/drivers/dma.c @@ -21,8 +21,8 @@ #include -#include "drivers/nvic.h" -#include "drivers/dma.h" +#include "nvic.h" +#include "dma.h" /* * DMA descriptors. diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index ec6e7908d3..8372ad0bcb 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -21,8 +21,8 @@ #include -#include "drivers/nvic.h" -#include "drivers/dma.h" +#include "nvic.h" +#include "dma.h" /* * DMA descriptors. diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 0ab3d6122b..bf7cf6f0cb 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -22,9 +22,10 @@ #ifdef USE_FLASH_M25P16 -#include "drivers/flash_m25p16.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "flash_m25p16.h" +#include "io.h" +#include "bus_spi.h" +#include "system.h" #define M25P16_INSTRUCTION_RDID 0x9F #define M25P16_INSTRUCTION_READ_BYTES 0x03 diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h index 223efa1809..6cad8a8ea6 100644 --- a/src/main/drivers/flash_m25p16.h +++ b/src/main/drivers/flash_m25p16.h @@ -19,7 +19,7 @@ #include #include "flash.h" -#include "io.h" +#include "io_types.h" #define M25P16_PAGESIZE 256 diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index cd95e186e1..dbdc2244c9 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -10,9 +10,9 @@ #include "platform.h" -#include "drivers/sensor.h" -#include "drivers/accgyro.h" -#include "drivers/gyro_sync.h" +#include "sensor.h" +#include "accgyro.h" +#include "gyro_sync.h" static uint8_t mpuDividerDrops; diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index d655646282..5afa4abb76 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -6,16 +6,7 @@ #include #include "resource.h" -// IO pin identification -// make sure that ioTag_t can't be assigned into IO_t without warning -typedef uint8_t ioTag_t; // packet tag to specify IO pin -typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change - -// NONE initializer for ioTag_t variables -#define IOTAG_NONE ((ioTag_t)0) - -// NONE initializer for IO_t variable -#define IO_NONE ((IO_t)0) +#include "io_types.h" // preprocessor is used to convert pinid to requested C data value // compile-time error is generated if requested pin is not available (not set in TARGET_IO_PORTx) @@ -24,19 +15,6 @@ typedef void* IO_t; // type specifying IO pin. Currently ioRec_t poin // expand pinid to to ioTag_t #define IO_TAG(pinid) DEFIO_TAG(pinid) -// both ioTag_t and IO_t are guarantied to be zero if pinid is NONE (no pin) -// this simplifies initialization (globals are zeroed on start) and allows -// omitting unused fields in structure initializers. -// it is also possible to use IO_t and ioTag_t as boolean value -// TODO - this may conflict with requirement to generate warning/error on IO_t - ioTag_t assignment -// IO_t being pointer is only possibility I know of .. - -// pin config handling -// pin config is packed into ioConfig_t to decrease memory requirements -// IOCFG_x macros are defined for common combinations for all CPUs; this -// helps masking CPU differences - -typedef uint8_t ioConfig_t; // packed IO configuration #if defined(STM32F1) // mode is using only bits 6-2 diff --git a/src/main/drivers/io_types.h b/src/main/drivers/io_types.h new file mode 100644 index 0000000000..ba80cd3bc7 --- /dev/null +++ b/src/main/drivers/io_types.h @@ -0,0 +1,28 @@ +#pragma once + +#include + +// IO pin identification +// make sure that ioTag_t can't be assigned into IO_t without warning +typedef uint8_t ioTag_t; // packet tag to specify IO pin +typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change + +// NONE initializer for ioTag_t variables +#define IOTAG_NONE ((ioTag_t)0) + +// NONE initializer for IO_t variable +#define IO_NONE ((IO_t)0) + +// both ioTag_t and IO_t are guarantied to be zero if pinid is NONE (no pin) +// this simplifies initialization (globals are zeroed on start) and allows +// omitting unused fields in structure initializers. +// it is also possible to use IO_t and ioTag_t as boolean value +// TODO - this may conflict with requirement to generate warning/error on IO_t - ioTag_t assignment +// IO_t being pointer is only possibility I know of .. + +// pin config handling +// pin config is packed into ioConfig_t to decrease memory requirements +// IOCFG_x macros are defined for common combinations for all CPUs; this +// helps masking CPU differences + +typedef uint8_t ioConfig_t; // packed IO configuration diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 6c8f2bb18a..d9c29af370 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -36,8 +36,8 @@ #include "common/color.h" #include "common/colorconversion.h" -#include "drivers/dma.h" -#include "drivers/light_ws2811strip.h" +#include "dma.h" +#include "light_ws2811strip.h" uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; volatile uint8_t ws2811LedDataTransferInProgress = 0; diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 4b588a0c9f..911b7cdc66 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -23,10 +23,11 @@ #ifdef LED_STRIP #include "common/color.h" -#include "drivers/light_ws2811strip.h" +#include "light_ws2811strip.h" #include "nvic.h" #include "io.h" #include "dma.h" +#include "rcc.h" #include "timer.h" static IO_t ws2811IO = IO_NONE; diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 8379188f99..1fadfb1f03 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -26,7 +26,7 @@ #include "nvic.h" #include "common/color.h" -#include "drivers/light_ws2811strip.h" +#include "light_ws2811strip.h" #include "dma.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 82f3f29af9..7ddc2a058b 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -19,16 +19,19 @@ #include #include "platform.h" -#include "version.h" #ifdef USE_MAX7456 +#include "version.h" + #include "common/printf.h" -#include "drivers/bus_spi.h" -#include "drivers/light_led.h" -#include "drivers/system.h" -#include "drivers/nvic.h" -#include "drivers/dma.h" + +#include "bus_spi.h" +#include "light_led.h" +#include "io.h" +#include "system.h" +#include "nvic.h" +#include "dma.h" #include "max7456.h" #include "max7456_symbols.h" diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 29a5d02f15..c017061835 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -17,7 +17,7 @@ #pragma once -#include "timer.h" +#include "io_types.h" #define MAX_PWM_MOTORS 12 #define MAX_PWM_SERVOS 8 @@ -88,10 +88,11 @@ typedef enum { PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4) } pwmPortFlags_e; +struct timerHardware_s; typedef struct pwmPortConfiguration_s { uint8_t index; pwmPortFlags_e flags; - const timerHardware_t *timerHardware; + const struct timerHardware_s *timerHardware; } pwmPortConfiguration_t; typedef struct pwmOutputConfiguration_s { diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 3c72d104dc..60af85e97b 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -30,7 +30,7 @@ #include "system.h" #include "nvic.h" -#include "gpio.h" +#include "io.h" #include "timer.h" #include "pwm_output.h" diff --git a/src/main/drivers/rcc.h b/src/main/drivers/rcc.h index fb04ec7e2c..69dce965f9 100644 --- a/src/main/drivers/rcc.h +++ b/src/main/drivers/rcc.h @@ -2,6 +2,7 @@ #include "platform.h" #include "common/utils.h" +#include "rcc_types.h" enum rcc_reg { RCC_EMPTY = 0, // make sure that default value (0) does not enable anything @@ -17,9 +18,6 @@ enum rcc_reg { #define RCC_APB1(periph) RCC_ENCODE(RCC_APB1, RCC_APB1ENR_ ## periph ## EN) #define RCC_AHB1(periph) RCC_ENCODE(RCC_AHB1, RCC_AHB1ENR_ ## periph ## EN) -typedef uint8_t rccPeriphTag_t; - void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState); void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState); - diff --git a/src/main/drivers/rcc_types.h b/src/main/drivers/rcc_types.h new file mode 100644 index 0000000000..1b7d7b9f2f --- /dev/null +++ b/src/main/drivers/rcc_types.h @@ -0,0 +1,4 @@ +#pragma once + +typedef uint8_t rccPeriphTag_t; + diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 878d5eacf6..a86fc74577 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -25,8 +25,8 @@ #include "nvic.h" #include "io.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "bus_spi.h" +#include "system.h" #include "sdcard.h" #include "sdcard_standard.h" diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 2c2cebbac6..50c6055ddd 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -22,7 +22,7 @@ #include "build_config.h" #include "common/utils.h" -#include "drivers/io.h" +#include "io.h" #include "usb_core.h" #ifdef STM32F4 @@ -32,7 +32,7 @@ #include "hw_config.h" #endif -#include "drivers/system.h" +#include "system.h" #include "serial.h" #include "serial_usb_vcp.h" diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h index d734f62f4f..cb3f38c989 100644 --- a/src/main/drivers/sonar_hcsr04.h +++ b/src/main/drivers/sonar_hcsr04.h @@ -17,8 +17,7 @@ #pragma once -#include "platform.h" -#include "io.h" +#include "io_types.h" typedef struct sonarHardware_s { ioTag_t triggerTag; diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index ab7a7c3dfc..9caad4f554 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -17,7 +17,7 @@ #pragma once -#include "drivers/io.h" +#include "io_types.h" #ifdef BEEPER #define BEEP_TOGGLE systemBeepToggle() diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 15e36cce3f..5051588c99 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -25,7 +25,7 @@ #include "nvic.h" -#include "gpio.h" +#include "io.h" #include "rcc.h" #include "system.h" diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 7050bbabda..00410a9c68 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -17,12 +17,11 @@ #pragma once -#include "io.h" -#include "rcc.h" +#include +#include -#if !defined(USABLE_TIMER_CHANNEL_COUNT) -#define USABLE_TIMER_CHANNEL_COUNT 14 -#endif +#include "io_types.h" +#include "rcc_types.h" typedef uint16_t captureCompare_t; // 16 bit on both 103 and 303, just register access must be 32bit sometimes (use timCCR_t) diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index 1a5085e63a..49106818f0 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -5,6 +5,11 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f10x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 04069b20c3..99111bad77 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -5,6 +5,11 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f30x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index af09f85e1b..ccce10350a 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -5,9 +5,14 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f4xx.h" -#include "timer.h" #include "rcc.h" +#include "timer.h" /** * @brief Selects the TIM Output Compare Mode. diff --git a/src/main/drivers/transponder_ir.c b/src/main/drivers/transponder_ir.c index 626c60097c..dbc6749af7 100644 --- a/src/main/drivers/transponder_ir.c +++ b/src/main/drivers/transponder_ir.c @@ -21,9 +21,9 @@ #include -#include "drivers/dma.h" -#include "drivers/nvic.h" -#include "drivers/transponder_ir.h" +#include "dma.h" +#include "nvic.h" +#include "transponder_ir.h" /* * Implementation note: diff --git a/src/main/drivers/transponder_ir_stm32f30x.c b/src/main/drivers/transponder_ir_stm32f30x.c index 3962e1c999..0bff2e112c 100644 --- a/src/main/drivers/transponder_ir_stm32f30x.c +++ b/src/main/drivers/transponder_ir_stm32f30x.c @@ -20,9 +20,9 @@ #include -#include "drivers/gpio.h" -#include "drivers/transponder_ir.h" -#include "drivers/nvic.h" +#include "gpio.h" +#include "transponder_ir.h" +#include "nvic.h" #ifndef TRANSPONDER_GPIO #define USE_TRANSPONDER_ON_DMA1_CHANNEL3 diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index 93ffa83ecb..08ec758e8d 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -31,9 +31,9 @@ #include "common/maths.h" -#include "drivers/vtx_rtc6705.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "vtx_rtc6705.h" +#include "bus_spi.h" +#include "system.h" #define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400 #define RTC6705_SET_A1 0x8F3031 //5865 diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.c b/src/main/drivers/vtx_soft_spi_rtc6705.c index 4ceba50df9..e6340680fa 100644 --- a/src/main/drivers/vtx_soft_spi_rtc6705.c +++ b/src/main/drivers/vtx_soft_spi_rtc6705.c @@ -22,9 +22,10 @@ #ifdef USE_RTC6705 -#include "drivers/bus_spi.h" -#include "drivers/system.h" -#include "drivers/light_led.h" +#include "bus_spi.h" +#include "io.h" +#include "system.h" +#include "light_led.h" #include "vtx_soft_spi_rtc6705.h" diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index f0f5525813..223d3e10cd 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -25,8 +25,10 @@ #include "platform.h" #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +#include "drivers/io_types.h" #include "drivers/system.h" #include "drivers/serial.h" +#include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "io/serial.h" #include "io/serial_msp.h" diff --git a/src/main/main.c b/src/main/main.c index 24b983308d..436f5b5798 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -33,6 +33,7 @@ #include "drivers/system.h" #include "drivers/dma.h" #include "drivers/gpio.h" +#include "drivers/io.h" #include "drivers/light_led.h" #include "drivers/sound_beeper.h" #include "drivers/timer.h" diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index bb46059ac1..c3465b3934 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,7 +24,7 @@ #include "common/axis.h" -#include "drivers/gpio.h" +#include "drivers/io.h" #include "drivers/system.h" #include "drivers/exti.h" diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index e780364ec2..f950c67844 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index 378627daaf..b5b9889379 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c index 7116b53551..dec8739ba0 100644 --- a/src/main/target/ALIENFLIGHTF1/target.c +++ b/src/main/target/ALIENFLIGHTF1/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 6d945008f7..04984912cd 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -90,4 +90,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index c1946aa17c..8169e9e912 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 28a896ff01..d2e9e92ba4 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -35,8 +35,6 @@ #define BEEPER PA5 -#define USABLE_TIMER_CHANNEL_COUNT 11 - #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_EXTI @@ -131,5 +129,6 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 11 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index efedb56689..dafcdf86da 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index b85e50f539..83e0700cd5 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index a870de9e24..589c6af30d 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 631d83e6aa..c2aca46716 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index 0f8228559e..3562e3234d 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 973e0984f9..beb4be9657 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -71,4 +71,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 91b21c8b36..099f9b62e7 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index 05b642f088..ca13ef7d0b 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 2399ede8d1..3f40c4ab43 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -67,11 +67,6 @@ #define M25P16_CS_PIN PC15 #define M25P16_SPI_INSTANCE SPI2 -// timer definitions in drivers/timer.c -// channel mapping in drivers/pwm_mapping.c -// only 6 outputs available on hardware -#define USABLE_TIMER_CHANNEL_COUNT 9 - #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 @@ -147,5 +142,9 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +// timer definitions in drivers/timer.c +// channel mapping in drivers/pwm_mapping.c +// only 6 outputs available on hardware +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/EUSTM32F103RC/target.c b/src/main/target/EUSTM32F103RC/target.c index b2a0db9a16..0dc2d66044 100644 --- a/src/main/target/EUSTM32F103RC/target.c +++ b/src/main/target/EUSTM32F103RC/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 12ec85f371..d883641d4f 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -113,5 +113,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index a8d80e823d..e9befde0ab 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -3,17 +3,18 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM7 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed @@ -47,13 +48,13 @@ const uint16_t multiPWM[] = { const uint16_t airPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), PWM12 | (MAP_TO_SERVO_OUTPUT << 8), PWM13 | (MAP_TO_SERVO_OUTPUT << 8), PWM14 | (MAP_TO_SERVO_OUTPUT << 8), - PWM15 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed - PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM7 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed @@ -84,23 +85,23 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT - { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT - { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT + { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT + { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // sonar echo if needed }; diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 7c9e89b48b..d8f8154031 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -74,8 +74,6 @@ #define SDCARD_DMA_CHANNEL DMA_Channel_0 -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define USE_VCP #define VBUS_SENSING_PIN PA9 @@ -156,5 +154,6 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) |TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index bf4ff7b20a..db28609d97 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index ac600ea439..a53aa4d447 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 8f69378592..05ff286283 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -101,8 +101,6 @@ #define M25P16_CS_PIN PB3 #define M25P16_SPI_INSTANCE SPI3 -#define USABLE_TIMER_CHANNEL_COUNT 5 - #define USE_VCP #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED @@ -167,5 +165,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 5 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index dd87a86410..2e4e12fb0a 100644 --- a/src/main/target/IRCFUSIONF3/target.c +++ b/src/main/target/IRCFUSIONF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 8dbd6e55ba..9ceabb35d2 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -23,8 +23,6 @@ #define LED0 PB3 -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready, no MAG #define USE_MPU_DATA_READY_SIGNAL @@ -105,5 +103,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index cbb9d74818..a443fe862f 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM7 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 05346145d6..e532b9d782 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -28,8 +28,6 @@ #define BEEPER PB13 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 12 - #define USE_EXTI #define MPU_INT_EXTI PB2 #define USE_MPU_DATA_READY_SIGNAL @@ -84,4 +82,5 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTF (BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 91b21c8b36..099f9b62e7 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/MICROSCISKY/target.c b/src/main/target/MICROSCISKY/target.c index 7116b53551..dec8739ba0 100644 --- a/src/main/target/MICROSCISKY/target.c +++ b/src/main/target/MICROSCISKY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 80d4f04856..9138f74e86 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -97,4 +97,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index a89063cc9a..cc898ba834 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index 7116b53551..dec8739ba0 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 0e8ba62be2..7e3705b47a 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -163,4 +163,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/OLIMEXINO/target.c b/src/main/target/OLIMEXINO/target.c index ea20267b3c..5d0a271c5b 100644 --- a/src/main/target/OLIMEXINO/target.c +++ b/src/main/target/OLIMEXINO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index bb270cb4fc..ae8ac3b38e 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -91,4 +91,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 977d623f85..a3cd3e54d3 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 7dd021055e..2ed3eab8fb 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index a89063cc9a..cc898ba834 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 111f6fc476..7a104b7466 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -28,8 +28,6 @@ #define BEEPER PA0 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 9 - // MPU6000 interrupts #define USE_EXTI #define MPU_INT_EXTI PA15 @@ -136,5 +134,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/PORT103R/target.c b/src/main/target/PORT103R/target.c index 8f21b7cbb7..093fa6ee27 100644 --- a/src/main/target/PORT103R/target.c +++ b/src/main/target/PORT103R/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 66e83fa3d5..9f0239d572 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -123,5 +123,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 67f7327285..a253a0b31f 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index 9a579f2bc9..bf5023fc24 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index ee96272c11..d344cda357 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index 8131579519..e48d0c33b4 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 09a5678174..ed23489a3a 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -25,8 +25,6 @@ #define BEEPER PC15 -#define USABLE_TIMER_CHANNEL_COUNT 10 - #define USE_EXTI #define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL @@ -115,5 +113,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index b52fbd00e4..7d602c74b1 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM7 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index 389617278b..20b1f185d3 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index ec9176a27f..99f79ec9b7 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 843ad3d9aa..87eb5db35f 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index d1e3a0d8b2..bf8776aa92 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index 1712bebae2..d2c17db5a2 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 592833bc48..a75e952171 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -165,5 +165,6 @@ #define TARGET_IO_PORTE 0xffff #define TARGET_IO_PORTF 0x00ff +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index 85db79093c..a63258100e 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -5,6 +5,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 7838b1f460..9cc822ebee 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -26,8 +26,6 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH @@ -128,5 +126,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/ZCOREF3/target.c b/src/main/target/ZCOREF3/target.c index b72400e6fd..48a6ce60da 100644 --- a/src/main/target/ZCOREF3/target.c +++ b/src/main/target/ZCOREF3/target.c @@ -5,6 +5,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h index c65abb7d92..5c07c78190 100644 --- a/src/main/target/ZCOREF3/target.h +++ b/src/main/target/ZCOREF3/target.h @@ -101,6 +101,5 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 17 // PPM, 8 PWM, UART3 RX/TX, LED Strip - #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 33bd13193c..d698b958a0 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -19,6 +19,8 @@ * Initial FrSky Telemetry implementation by silpstream @ rcgroups. * Addition protocol work by airmamaf @ github. */ + +#include #include #include diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 96e2193e9a..6373d166eb 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#include #include #include @@ -22,7 +23,6 @@ #ifdef TELEMETRY -#include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" From b2721d554c82a277b3b96a1beb18a7d18e1d6a8b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 01:10:46 +0100 Subject: [PATCH 208/456] Added #pragma once to debug.h --- src/main/debug.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/debug.h b/src/main/debug.h index 4431953f31..87dfa0add9 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -15,6 +15,8 @@ * along with Cleanflight. If not, see . */ +#pragma once + #define DEBUG16_VALUE_COUNT 4 extern int16_t debug[DEBUG16_VALUE_COUNT]; extern uint8_t debugMode; From aad4b7d0051fd741c649db3160973caefc80cbe6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 11:00:40 +0100 Subject: [PATCH 209/456] Removed dependencies on sensors/ --- src/main/drivers/compass_ak8963.c | 3 --- src/main/drivers/compass_ak8975.c | 2 -- src/main/drivers/compass_hmc5883l.c | 2 -- 3 files changed, 7 deletions(-) diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index 0668fcdd77..d5b9a59dec 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -33,9 +33,6 @@ #include "bus_i2c.h" #include "bus_spi.h" -#include "sensors/boardalignment.h" -#include "sensors/sensors.h" - #include "sensor.h" #include "compass.h" diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index 0d648ecc58..0e4d3677c5 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -33,8 +33,6 @@ #include "gpio.h" #include "bus_i2c.h" -#include "sensors/sensors.h" - #include "sensor.h" #include "compass.h" diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 3709cc510e..10fa9b3c6d 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -39,8 +39,6 @@ #include "sensor.h" #include "compass.h" -#include "sensors/sensors.h" - #include "compass_hmc5883l.h" //#define DEBUG_MAG_DATA_READY_INTERRUPT From 0a767d37c444dca224ca195f5f73b43c6d44897a Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 09:02:31 +0100 Subject: [PATCH 210/456] Put each PID in its own .c file --- Makefile | 2 + src/main/flight/pid.c | 390 ++----------------------------- src/main/flight/pid.h | 5 + src/main/flight/pid_betaflight.c | 279 ++++++++++++++++++++++ src/main/flight/pid_legacy.c | 210 +++++++++++++++++ 5 files changed, 511 insertions(+), 375 deletions(-) create mode 100644 src/main/flight/pid_betaflight.c create mode 100644 src/main/flight/pid_legacy.c diff --git a/Makefile b/Makefile index 281b169c63..68364d7b7c 100644 --- a/Makefile +++ b/Makefile @@ -392,6 +392,8 @@ COMMON_SRC = \ flight/imu.c \ flight/mixer.c \ flight/pid.c \ + flight/pid_legacy.c \ + flight/pid_betaflight.c \ io/beeper.c \ io/rc_controls.c \ io/rc_curves.c \ diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e147423412..7d49f1224b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -47,32 +47,25 @@ #include "config/runtime_config.h" -extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float setpointRate[3]; -extern float rcInput[3]; -static bool pidStabilisationEnabled; +bool pidStabilisationEnabled; int16_t axisPID[3]; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + #ifdef BLACKBOX int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; +int32_t errorGyroI[3]; +float errorGyroIf[3]; -static int32_t errorGyroI[3]; -static float errorGyroIf[3]; - -static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); #ifdef SKIP_PID_FLOAT pidControllerFuncPtr pid_controller = pidLegacy; // which pid controller are we using #else -static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using #endif @@ -94,7 +87,7 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false; } -float getdT (void) +float getdT(void) { static float dT; if (!dT) dT = (float)targetPidLooptime * 0.000001f; @@ -104,13 +97,15 @@ float getdT (void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static pt1Filter_t deltaFilter[3]; -static pt1Filter_t yawFilter; -static biquadFilter_t dtermFilterLpf[3]; -static biquadFilter_t dtermFilterNotch[3]; -static bool dtermNotchInitialised, dtermBiquadLpfInitialised; +pt1Filter_t deltaFilter[3]; +pt1Filter_t yawFilter; +biquadFilter_t dtermFilterLpf[3]; +biquadFilter_t dtermFilterNotch[3]; +bool dtermNotchInitialised; +bool dtermBiquadLpfInitialised; -void initFilters(const pidProfile_t *pidProfile) { +void initFilters(const pidProfile_t *pidProfile) +{ int axis; if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { @@ -125,360 +120,6 @@ void initFilters(const pidProfile_t *pidProfile) { } } -#ifndef SKIP_PID_FLOAT -// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab) -static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - float errorRate = 0, rP = 0, rD = 0, PVRate = 0; - float ITerm,PTerm,DTerm; - static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; - float delta; - int axis; - float horizonLevelStrength = 1; - - float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection - if(pidProfile->D8[PIDLEVEL] == 0){ - horizonLevelStrength = 0; - } else { - horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); - } - } - - // Yet Highly experimental and under test and development - // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) - static float kiThrottleGain = 1.0f; - if (pidProfile->itermThrottleGain) { - const uint16_t maxLoopCount = 20000 / targetPidLooptime; - const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; - static int16_t previousThrottle; - static uint16_t loopIncrement; - - if (loopIncrement >= maxLoopCount) { - kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 - previousThrottle = rcCommand[THROTTLE]; - loopIncrement = 0; - } else { - loopIncrement++; - } - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - static uint8_t configP[3], configI[3], configD[3]; - - // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now - // Prepare all parameters for PID controller - if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { - Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; - Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; - Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; - b[axis] = pidProfile->ptermSetpointWeight / 100.0f; - c[axis] = pidProfile->dtermSetpointWeight / 100.0f; - yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); - rollPitchMaxVelocity = pidProfile->pidMaxVelocityRollPitch * 1000 * getdT(); - - configP[axis] = pidProfile->P8[axis]; - configI[axis] = pidProfile->I8[axis]; - configD[axis] = pidProfile->D8[axis]; - } - - // Limit abrupt yaw inputs / stops - float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; - if (maxVelocity) { - float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; - if (ABS(currentVelocity) > maxVelocity) { - setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; - } - previousSetpoint[axis] = setpointRate[axis]; - } - - // Yaw control is GYRO based, direct sticks control is applied to rate PID - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to the max inclination -#ifdef GPS - const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#else - const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; - } else { - // HORIZON mode - direct sticks control is applied to rate PID - // mix up angle error to desired AngleRate to add a little auto-level feel - setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); - } - } - - PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec - - // --------low-level gyro-based PID based on 2DOF PID controller. ---------- - // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // ----- calculate error / angle rates ---------- - errorRate = setpointRate[axis] - PVRate; // r - y - rP = b[axis] * setpointRate[axis] - PVRate; // br - y - - // Slowly restore original setpoint with more stick input - float diffRate = errorRate - rP; - rP += diffRate * rcInput[axis]; - - // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount - float dynReduction = tpaFactor; - if (pidProfile->toleranceBand) { - const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; - static uint8_t zeroCrossCount[3]; - static uint8_t currentErrorPolarity[3]; - if (ABS(errorRate) < pidProfile->toleranceBand) { - if (zeroCrossCount[axis]) { - if (currentErrorPolarity[axis] == POSITIVE_ERROR) { - if (errorRate < 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = NEGATIVE_ERROR; - } - } else { - if (errorRate > 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = POSITIVE_ERROR; - } - } - } else { - dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); - } - } else { - zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; - currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; - } - } - - // -----calculate P component - PTerm = Kp[axis] * rP * dynReduction; - - // -----calculate I component. - // Reduce strong Iterm accumulation during higher stick inputs - float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); - - // Handle All windup Scenarios - // limit maximum integrator value to prevent WindUp - float itermScaler = setpointRateScaler * kiThrottleGain; - - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); - - // I coefficient (I8) moved before integration to make limiting independent from PID settings - ITerm = errorGyroIf[axis]; - - //-----calculate D-term (Yaw D not yet supported) - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = lrintf(PTerm + ITerm); - - DTerm = 0.0f; // needed for blackbox - } else { - rD = c[axis] * setpointRate[axis] - PVRate; // cr - y - delta = rD - lastRateError[axis]; - lastRateError[axis] = rD; - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta *= (1.0f / getdT()); - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; - - // Filter delta - if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); - - if (pidProfile->dterm_lpf_hz) { - if (dtermBiquadLpfInitialised) { - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } - } - - DTerm = Kd[axis] * delta * dynReduction; - - // -----calculate total PID output - axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); - } - - // Disable PID control at zero throttle - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} -#endif - -// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future -static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - int axis; - int32_t PTerm, ITerm, DTerm, delta; - static int32_t lastRateError[3]; - int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; - - int8_t horizonLevelStrength = 100; - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection - // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. - // For more rate mode increase D and slower flips and rolls will be possible - horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - // -----Get the desired angle rate depending on flight mode - AngleRateTmp = (int32_t)setpointRate[axis]; - - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to max configured inclination -#ifdef GPS - const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#else - const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; - } else { - // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, - // horizonLevelStrength is scaled to the stick input - AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); - } - } - - // --------low-level gyro-based PID. ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // -----calculate scaled error.AngleRates - // multiplication of rcCommand corresponds to changing the sticks scaling here - gyroRate = gyroADC[axis] / 4; - - RateError = AngleRateTmp - gyroRate; - - // -----calculate P component - PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; - - // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply - if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { - PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); - } - - // -----calculate I component - // there should be no division before accumulating the error to integrator, because the precision would be reduced. - // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. - // Time correction (to avoid different I scaling for different builds based on average cycle time) - // is normalized to cycle time = 2048. - // Prevent Accumulation - uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); - uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - - errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; - - // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. - // I coefficient (I8) moved before integration to make limiting independent from PID settings - errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - - ITerm = errorGyroI[axis] >> 13; - - //-----calculate D-term - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = PTerm + ITerm; - - if (motorCount >= 4) { - int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); - - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); - } - - DTerm = 0; // needed for blackbox - } else { - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastRateError[axis]; - lastRateError[axis] = RateError; - } else { - delta = -(gyroRate - lastRateError[axis]); - lastRateError[axis] = gyroRate; - } - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // Filter delta - if (pidProfile->dterm_lpf_hz) { - float deltaf = delta; // single conversion - if (dtermBiquadLpfInitialised) { - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } - delta = lrintf(deltaf); - } - - DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // -----calculate total PID output - axisPID[axis] = PTerm + ITerm + DTerm; - } - - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} void pidSetController(pidControllerType_e type) { @@ -493,4 +134,3 @@ void pidSetController(pidControllerType_e type) #endif } } - diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9aa625a33b..4abe89da89 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -118,6 +118,11 @@ struct rxConfig_s; typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype +void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); +void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); + extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool airmodeWasActivated; diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c new file mode 100644 index 0000000000..db07f11bb6 --- /dev/null +++ b/src/main/flight/pid_betaflight.c @@ -0,0 +1,279 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#ifndef SKIP_PID_FLOAT + +#include "build_config.h" +#include "debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" + +#include "drivers/accgyro.h" +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/gtune.h" + +#include "config/runtime_config.h" + +extern float rcInput[3]; +extern float setpointRate[3]; + +extern float errorGyroIf[3]; +extern bool pidStabilisationEnabled; + +extern pt1Filter_t deltaFilter[3]; +extern pt1Filter_t yawFilter; +extern biquadFilter_t dtermFilterLpf[3]; +extern biquadFilter_t dtermFilterNotch[3]; +extern bool dtermNotchInitialised; +extern bool dtermBiquadLpfInitialised; + +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + +void initFilters(const pidProfile_t *pidProfile); +float getdT(void); + +// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. +// Based on 2DOF reference design (matlab) +void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) +{ + float errorRate = 0, rP = 0, rD = 0, PVRate = 0; + float ITerm,PTerm,DTerm; + static float lastRateError[2]; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; + float delta; + int axis; + float horizonLevelStrength = 1; + + float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection + if(pidProfile->D8[PIDLEVEL] == 0){ + horizonLevelStrength = 0; + } else { + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); + } + } + + // Yet Highly experimental and under test and development + // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) + static float kiThrottleGain = 1.0f; + if (pidProfile->itermThrottleGain) { + const uint16_t maxLoopCount = 20000 / targetPidLooptime; + const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; + static int16_t previousThrottle; + static uint16_t loopIncrement; + + if (loopIncrement >= maxLoopCount) { + kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 + previousThrottle = rcCommand[THROTTLE]; + loopIncrement = 0; + } else { + loopIncrement++; + } + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + static uint8_t configP[3], configI[3], configD[3]; + + // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now + // Prepare all parameters for PID controller + if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { + Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; + Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; + Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; + b[axis] = pidProfile->ptermSetpointWeight / 100.0f; + c[axis] = pidProfile->dtermSetpointWeight / 100.0f; + yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); + rollPitchMaxVelocity = pidProfile->pidMaxVelocityRollPitch * 1000 * getdT(); + + configP[axis] = pidProfile->P8[axis]; + configI[axis] = pidProfile->I8[axis]; + configD[axis] = pidProfile->D8[axis]; + } + + // Limit abrupt yaw inputs / stops + float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; + if (maxVelocity) { + float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; + if (ABS(currentVelocity) > maxVelocity) { + setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; + } + previousSetpoint[axis] = setpointRate[axis]; + } + + // Yaw control is GYRO based, direct sticks control is applied to rate PID + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to the max inclination +#ifdef GPS + const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#else + const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; + } else { + // HORIZON mode - direct sticks control is applied to rate PID + // mix up angle error to desired AngleRate to add a little auto-level feel + setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); + } + } + + PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec + + // --------low-level gyro-based PID based on 2DOF PID controller. ---------- + // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // ----- calculate error / angle rates ---------- + errorRate = setpointRate[axis] - PVRate; // r - y + rP = b[axis] * setpointRate[axis] - PVRate; // br - y + + // Slowly restore original setpoint with more stick input + float diffRate = errorRate - rP; + rP += diffRate * rcInput[axis]; + + // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount + float dynReduction = tpaFactor; + if (pidProfile->toleranceBand) { + const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; + static uint8_t zeroCrossCount[3]; + static uint8_t currentErrorPolarity[3]; + if (ABS(errorRate) < pidProfile->toleranceBand) { + if (zeroCrossCount[axis]) { + if (currentErrorPolarity[axis] == POSITIVE_ERROR) { + if (errorRate < 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = NEGATIVE_ERROR; + } + } else { + if (errorRate > 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = POSITIVE_ERROR; + } + } + } else { + dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); + } + } else { + zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; + currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; + } + } + + // -----calculate P component + PTerm = Kp[axis] * rP * dynReduction; + + // -----calculate I component. + // Reduce strong Iterm accumulation during higher stick inputs + float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); + + // Handle All windup Scenarios + // limit maximum integrator value to prevent WindUp + float itermScaler = setpointRateScaler * kiThrottleGain; + + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); + + // I coefficient (I8) moved before integration to make limiting independent from PID settings + ITerm = errorGyroIf[axis]; + + //-----calculate D-term (Yaw D not yet supported) + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = lrintf(PTerm + ITerm); + + DTerm = 0.0f; // needed for blackbox + } else { + rD = c[axis] * setpointRate[axis] - PVRate; // cr - y + delta = rD - lastRateError[axis]; + lastRateError[axis] = rD; + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta *= (1.0f / getdT()); + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; + + // Filter delta + if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); + + if (pidProfile->dterm_lpf_hz) { + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + } + + DTerm = Kd[axis] * delta * dynReduction; + + // -----calculate total PID output + axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); + } + + // Disable PID control at zero throttle + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif + } +} +#endif + diff --git a/src/main/flight/pid_legacy.c b/src/main/flight/pid_legacy.c new file mode 100644 index 0000000000..0424f5461f --- /dev/null +++ b/src/main/flight/pid_legacy.c @@ -0,0 +1,210 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#include "build_config.h" +#include "debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" + +#include "drivers/accgyro.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/gtune.h" + +#include "config/runtime_config.h" + +extern uint8_t motorCount; +extern uint8_t PIDweight[3]; +extern bool pidStabilisationEnabled; +extern float setpointRate[3]; +extern int32_t errorGyroI[3]; +extern pt1Filter_t deltaFilter[3]; +extern pt1Filter_t yawFilter; +extern biquadFilter_t dtermFilterLpf[3]; +extern bool dtermBiquadLpfInitialised; + + +void initFilters(const pidProfile_t *pidProfile); +float getdT(void); + + +// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. +// Don't expect much development in the future +void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) +{ + int axis; + int32_t PTerm, ITerm, DTerm, delta; + static int32_t lastRateError[3]; + int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; + + int8_t horizonLevelStrength = 100; + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection + // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. + // For more rate mode increase D and slower flips and rolls will be possible + horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + // -----Get the desired angle rate depending on flight mode + AngleRateTmp = (int32_t)setpointRate[axis]; + + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to max configured inclination +#ifdef GPS + const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +#else + const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; + } else { + // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, + // horizonLevelStrength is scaled to the stick input + AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); + } + } + + // --------low-level gyro-based PID. ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // -----calculate scaled error.AngleRates + // multiplication of rcCommand corresponds to changing the sticks scaling here + gyroRate = gyroADC[axis] / 4; + + RateError = AngleRateTmp - gyroRate; + + // -----calculate P component + PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + + // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply + if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { + PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); + } + + // -----calculate I component + // there should be no division before accumulating the error to integrator, because the precision would be reduced. + // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. + // Time correction (to avoid different I scaling for different builds based on average cycle time) + // is normalized to cycle time = 2048. + // Prevent Accumulation + uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); + uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; + + errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; + + // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. + // I coefficient (I8) moved before integration to make limiting independent from PID settings + errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); + + ITerm = errorGyroI[axis] >> 13; + + //-----calculate D-term + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = PTerm + ITerm; + + if (motorCount >= 4) { + int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); + + // prevent "yaw jump" during yaw correction + axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); + } + + DTerm = 0; // needed for blackbox + } else { + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = RateError - lastRateError[axis]; + lastRateError[axis] = RateError; + } else { + delta = -(gyroRate - lastRateError[axis]); + lastRateError[axis] = gyroRate; + } + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // Filter delta + if (pidProfile->dterm_lpf_hz) { + float deltaf = delta; // single conversion + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + delta = lrintf(deltaf); + } + + DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // -----calculate total PID output + axisPID[axis] = PTerm + ITerm + DTerm; + } + + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif + } +} + From 7c2879c8f72f2f999206d4d308ddf48746480219 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 14:20:39 +0200 Subject: [PATCH 211/456] Move PIDweight to a common place. pid_legacy should not depend on pid_betaflight. --- src/main/flight/pid.h | 3 +++ src/main/flight/pid_betaflight.c | 3 --- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4abe89da89..77e531f167 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -128,6 +128,9 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool airmodeWasActivated; extern uint32_t targetPidLooptime; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +extern uint8_t PIDweight[3]; + void pidSetController(pidControllerType_e type); void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index db07f11bb6..a2dc204d98 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -62,9 +62,6 @@ extern biquadFilter_t dtermFilterNotch[3]; extern bool dtermNotchInitialised; extern bool dtermBiquadLpfInitialised; -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; - void initFilters(const pidProfile_t *pidProfile); float getdT(void); From 50d58c027bb7b5ac123ce13d16a670813e0fb382 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 15:16:30 +0200 Subject: [PATCH 212/456] Added missing include. Target VRRACE build failed. --- src/main/target/VRRACE/target.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/VRRACE/target.c b/src/main/target/VRRACE/target.c index 094ade421e..90270b1fa8 100644 --- a/src/main/target/VRRACE/target.c +++ b/src/main/target/VRRACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input From 426ce5bd2d1dabaefdc9a1ba7be879da28331c60 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 00:55:16 +0100 Subject: [PATCH 213/456] Optimisation of driver header files From 57f9b4edb296b780d7abed8f5cc8853b007202d6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 11:00:40 +0100 Subject: [PATCH 214/456] Removed dependencies on sensors/ From 114bfb76ffded63101f817d9c17f534ddf2a9532 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 09:02:31 +0100 Subject: [PATCH 215/456] Put each PID in its own .c file --- src/main/flight/pid_betaflight.c | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index a2dc204d98..030324065d 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -62,6 +62,9 @@ extern biquadFilter_t dtermFilterNotch[3]; extern bool dtermNotchInitialised; extern bool dtermBiquadLpfInitialised; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + void initFilters(const pidProfile_t *pidProfile); float getdT(void); @@ -72,7 +75,7 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; float delta; int axis; float horizonLevelStrength = 1; @@ -134,14 +137,13 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio configD[axis] = pidProfile->D8[axis]; } - // Limit abrupt yaw inputs / stops - float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; - if (maxVelocity) { - float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; - if (ABS(currentVelocity) > maxVelocity) { - setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; + // Limit abrupt yaw inputs + if (axis == YAW && pidProfile->pidMaxVelocityYaw) { + float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; + if (ABS(yawCurrentVelocity) > yawMaxVelocity) { + setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; } - previousSetpoint[axis] = setpointRate[axis]; + yawPreviousRate = setpointRate[axis]; } // Yaw control is GYRO based, direct sticks control is applied to rate PID From f1d54a5679904c3bb0811030376cc8b867dafb17 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 14:20:39 +0200 Subject: [PATCH 216/456] Move PIDweight to a common place. pid_legacy should not depend on pid_betaflight. --- src/main/flight/pid_betaflight.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index 030324065d..058e587603 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -62,9 +62,6 @@ extern biquadFilter_t dtermFilterNotch[3]; extern bool dtermNotchInitialised; extern bool dtermBiquadLpfInitialised; -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; - void initFilters(const pidProfile_t *pidProfile); float getdT(void); From 437b901737ca3a74f0b2e730e144c856e9f111e7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 5 Aug 2016 13:55:09 +0200 Subject: [PATCH 217/456] Fix rebase artifacts --- src/main/flight/pid_betaflight.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index 058e587603..a2dc204d98 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -72,7 +72,7 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; float delta; int axis; float horizonLevelStrength = 1; @@ -134,13 +134,14 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio configD[axis] = pidProfile->D8[axis]; } - // Limit abrupt yaw inputs - if (axis == YAW && pidProfile->pidMaxVelocityYaw) { - float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; - if (ABS(yawCurrentVelocity) > yawMaxVelocity) { - setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; + // Limit abrupt yaw inputs / stops + float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; + if (maxVelocity) { + float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; + if (ABS(currentVelocity) > maxVelocity) { + setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; } - yawPreviousRate = setpointRate[axis]; + previousSetpoint[axis] = setpointRate[axis]; } // Yaw control is GYRO based, direct sticks control is applied to rate PID From 81c06ace92a6bebb10fe88898c518262cc60494c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 5 Aug 2016 14:18:31 +0200 Subject: [PATCH 218/456] Refactor rate limit --- src/main/config/config.c | 4 ++-- src/main/flight/pid.c | 4 ++-- src/main/flight/pid.h | 4 ++-- src/main/io/serial_cli.c | 4 ++-- src/main/io/serial_msp.c | 8 ++++---- 5 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 956aae97f7..dcb0ce85c3 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -245,8 +245,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; pidProfile->dtermSetpointWeight = 120; - pidProfile->pidMaxVelocityYaw = 220; - pidProfile->pidMaxVelocityRollPitch = 0; + pidProfile->yawRateAccelLimit = 220; + pidProfile->rateAccelLimit = 0; pidProfile->toleranceBand = 15; pidProfile->toleranceBandReduction = 40; pidProfile->zeroCrossAllowanceCount = 2; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e147423412..0f91875e2a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -187,8 +187,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; b[axis] = pidProfile->ptermSetpointWeight / 100.0f; c[axis] = pidProfile->dtermSetpointWeight / 100.0f; - yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); - rollPitchMaxVelocity = pidProfile->pidMaxVelocityRollPitch * 1000 * getdT(); + yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT(); + rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT(); configP[axis] = pidProfile->P8[axis]; configI[axis] = pidProfile->I8[axis]; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9aa625a33b..c12404ec5a 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -100,8 +100,8 @@ typedef struct pidProfile_s { uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm uint8_t ptermSetpointWeight; // Setpoint weight for Pterm (lower means more PV tracking) uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) - uint16_t pidMaxVelocityYaw; // velocity yaw limiter for pid controller deg/sec/ms - uint16_t pidMaxVelocityRollPitch; // velocity roll/pitch limiter for pid controller deg/sec/ms + uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms + uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 19238347af..463ffb72da 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -832,8 +832,8 @@ const clivalue_t valueTable[] = { { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 200 } }, - { "max_yaw_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityYaw, .config.minmax = {0, 1000 } }, - { "max_acceleration", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidMaxVelocityRollPitch, .config.minmax = {0, 1000 } }, + { "yaw_rate_acceleration_limit",VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, + { "rate_acceleration_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 6cf6a50057..a2f472f45d 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1268,8 +1268,8 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentProfile->pidProfile.toleranceBand); serialize8(currentProfile->pidProfile.toleranceBandReduction); serialize8(currentProfile->pidProfile.itermThrottleGain); - serialize16(currentProfile->pidProfile.pidMaxVelocityRollPitch); - serialize16(currentProfile->pidProfile.pidMaxVelocityYaw); + serialize16(currentProfile->pidProfile.rateAccelLimit); + serialize16(currentProfile->pidProfile.yawRateAccelLimit); break; case MSP_SENSOR_CONFIG: headSerialReply(3); @@ -1858,8 +1858,8 @@ static bool processInCommand(void) currentProfile->pidProfile.toleranceBand = read8(); currentProfile->pidProfile.toleranceBandReduction = read8(); currentProfile->pidProfile.itermThrottleGain = read8(); - currentProfile->pidProfile.pidMaxVelocityRollPitch = read16(); - currentProfile->pidProfile.pidMaxVelocityYaw = read16(); + currentProfile->pidProfile.rateAccelLimit = read16(); + currentProfile->pidProfile.yawRateAccelLimit = read16(); break; case MSP_SET_SENSOR_CONFIG: masterConfig.acc_hardware = read8(); From 361f937f6f03946d813c3420c9356a0842e7180b Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Fri, 5 Aug 2016 17:19:22 +0200 Subject: [PATCH 219/456] Enable spektrum sat bind on REVO. --- src/main/target/REVO/target.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 76738118d0..b697c887f3 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -112,6 +112,10 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define DEFAULT_FEATURES (FEATURE_BLACKBOX) +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff From e84b2e4b017ef30fda3e9c27fd733836a5498c56 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Fri, 5 Aug 2016 21:22:52 +0200 Subject: [PATCH 220/456] Enable spektrum sat bind on REVONANO and BLUEJAYF4. --- src/main/target/BLUEJAYF4/target.h | 3 +++ src/main/target/REVONANO/target.h | 4 ++++ 2 files changed, 7 insertions(+) diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 737338b054..76a494cd53 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -161,6 +161,9 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define SPEKTRUM_BIND +#define BIND_PIN PB11 + #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 448d235b81..3a4f8d157a 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -92,6 +92,10 @@ #define USE_SERVOS #define USE_CLI +#define SPEKTRUM_BIND +// USART2, PA3 +#define BIND_PIN PA3 + #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff From bb681c2230e9f68daa5e53eca6904aff10ea3ecf Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 5 Aug 2016 22:18:02 +0200 Subject: [PATCH 221/456] Sparky I2C initialization fix --- src/main/target/SPARKY/target.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 582b82bf76..6e056b7269 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -72,6 +72,9 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) +#define I2C2_SCL PA9 +#define I2C2_SDA PA10 + #define USE_ADC #define ADC_INSTANCE ADC2 #define VBAT_ADC_PIN PA4 From 8d2902a8eb6b44b048c53ce511788aeb5dff18bf Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sat, 6 Aug 2016 00:03:12 +0200 Subject: [PATCH 222/456] Support for Sparky 2 (new target) --- src/main/target/SPARKY2/SPARKY_OPBL.mk | 0 src/main/target/SPARKY2/target.c | 99 +++++++++++++++++++ src/main/target/SPARKY2/target.h | 131 +++++++++++++++++++++++++ src/main/target/SPARKY2/target.mk | 12 +++ 4 files changed, 242 insertions(+) create mode 100644 src/main/target/SPARKY2/SPARKY_OPBL.mk create mode 100644 src/main/target/SPARKY2/target.c create mode 100644 src/main/target/SPARKY2/target.h create mode 100644 src/main/target/SPARKY2/target.mk diff --git a/src/main/target/SPARKY2/SPARKY_OPBL.mk b/src/main/target/SPARKY2/SPARKY_OPBL.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/SPARKY2/target.c b/src/main/target/SPARKY2/target.c new file mode 100644 index 0000000000..a4e95ecee4 --- /dev/null +++ b/src/main/target/SPARKY2/target.c @@ -0,0 +1,99 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM IN + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S2_IN + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S3_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S5_IN + + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT +}; + diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h new file mode 100644 index 0000000000..0ed29e6616 --- /dev/null +++ b/src/main/target/SPARKY2/target.h @@ -0,0 +1,131 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once +#define TARGET_BOARD_IDENTIFIER "SPK2" + +#define CONFIG_START_FLASH_ADDRESS 0x08080000 //0x08080000 to 0x080A0000 (FLASH_Sector_8) + +#define USBD_PRODUCT_STRING "Sparky 2.0" +#ifdef OPBL + #define USBD_SERIALNUMBER_STRING "0x8020000" +#endif + +#define LED0 PB5 +#define LED1 PB4 +#define LED2 PB6 + +#define BEEPER PC9 + +#define INVERTER PC6 +#define INVERTER_USART USART6 + +// MPU9250 interrupt +#define USE_EXTI +#define MPU_INT_EXTI PC5 +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready +//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define MPU9250_CS_PIN PC4 +#define MPU9250_SPI_INSTANCE SPI1 + +#define ACC +#define USE_ACC_SPI_MPU9250 +#define ACC_MPU9250_ALIGN CW270_DEG + +#define GYRO +#define USE_GYRO_SPI_MPU9250 +#define GYRO_MPU9250_ALIGN CW270_DEG + +#define MAG +//#define USE_MAG_HMC5883 +#define USE_MAG_AK8963 + +//#define MAG_HMC5883_ALIGN CW180_DEG +#define MAG_AK8963_ALIGN CW270_DEG + +#define BARO +#define USE_BARO_MS5611 +//#define USE_BARO_BMP280 + +#define M25P16_CS_PIN PB3 +#define M25P16_SPI_INSTANCE SPI3 + +//#define RFM22B_CS_PIN PA15 +//#define RFM22B_SPI_INSTANCE SPI3 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define USE_FLASH_TOOLS + +#define USE_VCP +#define VBUS_SENSING_PIN PA8 + +#define USE_USART1 +#define USART1_RX_PIN PA10 +#define USART1_TX_PIN PA9 +#define USART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 + +#define USE_USART3 +#define USART3_RX_PIN PB11 +#define USART3_TX_PIN PB10 + +#define USE_USART6 +#define USART6_RX_PIN PC7 +#define USART6_TX_PIN PC6 //inverter + +#define SERIAL_PORT_COUNT 4 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 //MPU9250 +#define SPI1_NSS_PIN PC4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_3 //dataflash +#define SPI3_NSS_PIN PB3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) +//#define I2C_DEVICE_EXT (I2CDEV_2) + +#define USE_ADC + +#define LED_STRIP +#define LED_STRIP_TIMER TIM5 + +#define DEFAULT_FEATURES FEATURE_BLACKBOX +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART3 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff + +#define USABLE_TIMER_CHANNEL_COUNT 11 +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) + diff --git a/src/main/target/SPARKY2/target.mk b/src/main/target/SPARKY2/target.mk new file mode 100644 index 0000000000..56f2b2f444 --- /dev/null +++ b/src/main/target/SPARKY2/target.mk @@ -0,0 +1,12 @@ +F405_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_spi_mpu9250.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_ms5611.c \ + drivers/compass_ak8963.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f4xx.c + From c3638362fd4c16b94f432221e03123a0d33d48ca Mon Sep 17 00:00:00 2001 From: nathan Date: Fri, 5 Aug 2016 19:13:47 -0700 Subject: [PATCH 223/456] turn on shared ppm / serialrx3 on the omnibus so people dont need to desolder resistors --- src/main/target/OMNIBUS/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index ad84573b59..5434391b3e 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -175,6 +175,8 @@ #define BUTTON_B_PORT GPIOB #define BUTTON_B_PIN Pin_0 +#define AVOID_UART3_FOR_PWM_PPM + #define SPEKTRUM_BIND // USART3, #define BIND_PIN PB11 From 8fce0c2e6e0453422f7ba0133f0925f5708b870b Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Thu, 4 Aug 2016 13:48:03 +0300 Subject: [PATCH 224/456] Add support for Colibri fc, used in Gemini racer hex --- src/main/drivers/flash_m25p16.c | 5 + src/main/target/COLIBRI/config.c | 99 +++++++++++++++++++ src/main/target/COLIBRI/target.c | 123 ++++++++++++++++++++++++ src/main/target/COLIBRI/target.h | 148 +++++++++++++++++++++++++++++ src/main/target/COLIBRI/target.mk | 11 +++ src/main/target/system_stm32f4xx.c | 4 + 6 files changed, 390 insertions(+) create mode 100644 src/main/target/COLIBRI/config.c create mode 100644 src/main/target/COLIBRI/target.c create mode 100644 src/main/target/COLIBRI/target.h create mode 100644 src/main/target/COLIBRI/target.mk diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 0ab3d6122b..a0afb64804 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -43,6 +43,7 @@ #define JEDEC_ID_MICRON_M25P16 0x202015 #define JEDEC_ID_MICRON_N25Q064 0x20BA17 #define JEDEC_ID_WINBOND_W25Q64 0xEF4017 +#define JEDEC_ID_MACRONIX_MX25L3206E 0xC22016 #define JEDEC_ID_MACRONIX_MX25L6406E 0xC22017 #define JEDEC_ID_MICRON_N25Q128 0x20ba18 #define JEDEC_ID_WINBOND_W25Q128 0xEF4018 @@ -161,6 +162,10 @@ static bool m25p16_readIdentification() geometry.sectors = 32; geometry.pagesPerSector = 256; break; + case JEDEC_ID_MACRONIX_MX25L3206E: + geometry.sectors = 64; + geometry.pagesPerSector = 256; + break; case JEDEC_ID_MICRON_N25Q064: case JEDEC_ID_WINBOND_W25Q64: case JEDEC_ID_MACRONIX_MX25L6406E: diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c new file mode 100644 index 0000000000..7d20ab1d90 --- /dev/null +++ b/src/main/target/COLIBRI/config.c @@ -0,0 +1,99 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" +#include "debug.h" + +#include "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/gpio.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +// alternative defaults settings for Colibri/Gemini targets +void targetConfiguration(void) +{ + masterConfig.mixerMode = MIXER_HEX6X; + masterConfig.rxConfig.serialrx_provider = 2; + featureSet(FEATURE_RX_SERIAL); + + masterConfig.escAndServoConfig.minthrottle = 1070; + masterConfig.escAndServoConfig.maxthrottle = 2000; + + masterConfig.boardAlignment.pitchDegrees = 10; + //masterConfig.rcControlsConfig.deadband = 10; + //masterConfig.rcControlsConfig.yaw_deadband = 10; + masterConfig.mag_hardware = 1; + + masterConfig.profile[0].controlRateProfile[0].dynThrPID = 45; + masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint = 1700; + masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; +} diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c new file mode 100644 index 0000000000..dd87343119 --- /dev/null +++ b/src/main/target/COLIBRI/target.c @@ -0,0 +1,123 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM1 }, // S1_IN + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S2_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S4_IN + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM2 }, // S5_IN + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM2 }, // S6_IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM5 }, // S7_IN + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM5 }, // S8_IN + + + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S3_OUT + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12 }, // S4_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S5_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12 }, // S6_OUT + { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM10 }, // S7_OUT + { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM11 }, // S8_OUT +}; diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h new file mode 100644 index 0000000000..77cc0f2f57 --- /dev/null +++ b/src/main/target/COLIBRI/target.h @@ -0,0 +1,148 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define PLL_M 16 +#define PLL_N 336 + +#define TARGET_BOARD_IDENTIFIER "COLI" + +#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) + +#define USBD_PRODUCT_STRING "Colibri" +#ifdef OPBL +#define USBD_SERIALNUMBER_STRING "0x8020000" +#endif + +#define LED0 PC14 +#define LED1 PC13 +#define BEEPER PC5 +#define INVERTER PB2 // PB2 used as inverter select GPIO +#define INVERTER_USART USART2 + +#define MPU6000_CS_PIN PC4 +#define MPU6000_SPI_INSTANCE SPI1 + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW270_DEG_FLIP + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW270_DEG_FLIP + +// MPU6000 interrupts +#define USE_EXTI +#define MPU_INT_EXTI PC0 +#define USE_MPU_DATA_READY_SIGNAL +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) + +#define MAG +#define USE_MAG_HMC5883 +#define MAG_HMC5883_ALIGN CW270_DEG_FLIP + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH +#define MAG_INT_EXTI PC1 + +#define BARO +#define USE_BARO_MS5611 + +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + + +#define USE_VCP +#define VBUS_SENSING_PIN PA9 + +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +//#define USE_UART4 +#define UART4_RX_PIN PC11 +#define UART4_TX_PIN PC10 + +//#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define SERIAL_PORT_COUNT 4 //VCP, UART1, UART2, UART3 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PC4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_3) +#define I2C3_SCL PA8 +#define I2C3_SDA PC9 + +#define SENSORS_SET (SENSOR_ACC) + +#define LED_STRIP + +#define WS2811_PIN PB7 // Shared UART1 +#define WS2811_TIMER TIM4 +#define WS2811_TIMER_CHANNEL TIM_Channel_2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST3_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream3 +#define WS2811_DMA_FLAG DMA_FLAG_TCIF3 +#define WS2811_DMA_IT DMA_IT_TCIF3 +#define WS2811_DMA_CHANNEL DMA_Channel_2 +#define WS2811_DMA_IRQ DMA1_Stream3_IRQn + +// alternative defaults for Colibri/Gemini target +#define TARGET_CONFIG + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL) + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define USABLE_TIMER_CHANNEL_COUNT 16 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(10) | TIM_N(11)) diff --git a/src/main/target/COLIBRI/target.mk b/src/main/target/COLIBRI/target.mk new file mode 100644 index 0000000000..a3d0a413d8 --- /dev/null +++ b/src/main/target/COLIBRI/target.mk @@ -0,0 +1,11 @@ +F405_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH +HSE_VALUE = 16000000 + +TARGET_SRC = \ + drivers/accgyro_spi_mpu6000.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f4xx.c \ + diff --git a/src/main/target/system_stm32f4xx.c b/src/main/target/system_stm32f4xx.c index 5a6d90059b..879acff784 100644 --- a/src/main/target/system_stm32f4xx.c +++ b/src/main/target/system_stm32f4xx.c @@ -371,7 +371,11 @@ uint32_t hse_value = HSE_VALUE; /************************* PLL Parameters *************************************/ #if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F469_479xx) /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N */ +#if defined(COLIBRI) + #define PLL_M 16 +#else #define PLL_M 8 +#endif #elif defined (STM32F446xx) #define PLL_M 8 #elif defined (STM32F410xx) || defined (STM32F411xE) From aca449a043ddfebdec0fe164d5a4171adef71dec Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Sat, 6 Aug 2016 23:08:58 +0200 Subject: [PATCH 225/456] Fix CC3D beeper. --- src/main/target/CC3D/target.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 3e86856965..2c61276d16 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -22,8 +22,8 @@ #define INVERTER PB2 // PB2 (BOOT1) used as inverter select GPIO #define INVERTER_USART USART1 -#define BEEPER PB15 -#define BEEPER_OPT PB2 +#define BEEPER PA15 +#define BEEPER_OPT PA2 #define USE_EXTI #define MPU_INT_EXTI PA3 From b924defdb00ef26ae48485d3dd35498ef20b6c41 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 7 Aug 2016 00:21:44 +0200 Subject: [PATCH 226/456] Fix int pin for FURYF3 #903 --- src/main/target/FURYF3/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 858b2d76e9..1cc93b6f21 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -28,7 +28,7 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PC4 +#define MPU_INT_EXTI PA3 #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW From 35bd2deb2f6cad8fae5d56aa398d85262ace9e49 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sat, 6 Aug 2016 00:03:12 +0200 Subject: [PATCH 227/456] Support for Sparky 2 (new target) --- src/main/target/SPARKY2/SPARKY_OPBL.mk | 0 src/main/target/SPARKY2/target.c | 99 +++++++++++++++++++ src/main/target/SPARKY2/target.h | 131 +++++++++++++++++++++++++ src/main/target/SPARKY2/target.mk | 12 +++ 4 files changed, 242 insertions(+) create mode 100644 src/main/target/SPARKY2/SPARKY_OPBL.mk create mode 100644 src/main/target/SPARKY2/target.c create mode 100644 src/main/target/SPARKY2/target.h create mode 100644 src/main/target/SPARKY2/target.mk diff --git a/src/main/target/SPARKY2/SPARKY_OPBL.mk b/src/main/target/SPARKY2/SPARKY_OPBL.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/SPARKY2/target.c b/src/main/target/SPARKY2/target.c new file mode 100644 index 0000000000..a4e95ecee4 --- /dev/null +++ b/src/main/target/SPARKY2/target.c @@ -0,0 +1,99 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM IN + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S2_IN + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S3_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S5_IN + + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT +}; + diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h new file mode 100644 index 0000000000..0ed29e6616 --- /dev/null +++ b/src/main/target/SPARKY2/target.h @@ -0,0 +1,131 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once +#define TARGET_BOARD_IDENTIFIER "SPK2" + +#define CONFIG_START_FLASH_ADDRESS 0x08080000 //0x08080000 to 0x080A0000 (FLASH_Sector_8) + +#define USBD_PRODUCT_STRING "Sparky 2.0" +#ifdef OPBL + #define USBD_SERIALNUMBER_STRING "0x8020000" +#endif + +#define LED0 PB5 +#define LED1 PB4 +#define LED2 PB6 + +#define BEEPER PC9 + +#define INVERTER PC6 +#define INVERTER_USART USART6 + +// MPU9250 interrupt +#define USE_EXTI +#define MPU_INT_EXTI PC5 +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready +//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define MPU9250_CS_PIN PC4 +#define MPU9250_SPI_INSTANCE SPI1 + +#define ACC +#define USE_ACC_SPI_MPU9250 +#define ACC_MPU9250_ALIGN CW270_DEG + +#define GYRO +#define USE_GYRO_SPI_MPU9250 +#define GYRO_MPU9250_ALIGN CW270_DEG + +#define MAG +//#define USE_MAG_HMC5883 +#define USE_MAG_AK8963 + +//#define MAG_HMC5883_ALIGN CW180_DEG +#define MAG_AK8963_ALIGN CW270_DEG + +#define BARO +#define USE_BARO_MS5611 +//#define USE_BARO_BMP280 + +#define M25P16_CS_PIN PB3 +#define M25P16_SPI_INSTANCE SPI3 + +//#define RFM22B_CS_PIN PA15 +//#define RFM22B_SPI_INSTANCE SPI3 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define USE_FLASH_TOOLS + +#define USE_VCP +#define VBUS_SENSING_PIN PA8 + +#define USE_USART1 +#define USART1_RX_PIN PA10 +#define USART1_TX_PIN PA9 +#define USART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 + +#define USE_USART3 +#define USART3_RX_PIN PB11 +#define USART3_TX_PIN PB10 + +#define USE_USART6 +#define USART6_RX_PIN PC7 +#define USART6_TX_PIN PC6 //inverter + +#define SERIAL_PORT_COUNT 4 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 //MPU9250 +#define SPI1_NSS_PIN PC4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_3 //dataflash +#define SPI3_NSS_PIN PB3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) +//#define I2C_DEVICE_EXT (I2CDEV_2) + +#define USE_ADC + +#define LED_STRIP +#define LED_STRIP_TIMER TIM5 + +#define DEFAULT_FEATURES FEATURE_BLACKBOX +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART3 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff + +#define USABLE_TIMER_CHANNEL_COUNT 11 +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9)) + diff --git a/src/main/target/SPARKY2/target.mk b/src/main/target/SPARKY2/target.mk new file mode 100644 index 0000000000..56f2b2f444 --- /dev/null +++ b/src/main/target/SPARKY2/target.mk @@ -0,0 +1,12 @@ +F405_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_spi_mpu9250.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_ms5611.c \ + drivers/compass_ak8963.c \ + drivers/compass_hmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f4xx.c + From 4bb08161223290a21eb51cf8af28ed089f569d69 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 01:37:59 +0200 Subject: [PATCH 228/456] Start on version 3.1.0 --- src/main/version.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/version.h b/src/main/version.h index 0f770674c1..c3c6529ed3 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,7 +16,7 @@ */ #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 0 // increment when a minor release is made (small new feature, change etc) +#define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) #define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed #define STR_HELPER(x) #x From 07adf66bbbe7175fd11a041980f7d48c579b830f Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 00:55:16 +0100 Subject: [PATCH 229/456] Optimisation of driver header files --- src/main/config/config.c | 1 + src/main/drivers/accgyro_mma845x.c | 2 +- src/main/drivers/adc.c | 2 + src/main/drivers/adc.h | 2 +- src/main/drivers/adc_impl.h | 6 +-- src/main/drivers/barometer_spi_bmp280.c | 2 + src/main/drivers/bus_i2c.h | 4 +- src/main/drivers/bus_i2c_soft.c | 2 +- src/main/drivers/bus_i2c_stm32f30x.c | 5 +- src/main/drivers/bus_spi.h | 5 +- src/main/drivers/compass_hmc5883l.h | 2 +- src/main/drivers/dma.c | 4 +- src/main/drivers/dma_stm32f4xx.c | 4 +- src/main/drivers/flash_m25p16.c | 7 +-- src/main/drivers/flash_m25p16.h | 2 +- src/main/drivers/gyro_sync.c | 6 +-- src/main/drivers/io.h | 24 +-------- src/main/drivers/io_types.h | 28 +++++++++++ src/main/drivers/light_ws2811strip.c | 4 +- .../drivers/light_ws2811strip_stm32f10x.c | 3 +- .../drivers/light_ws2811strip_stm32f30x.c | 2 +- src/main/drivers/max7456.c | 15 +++--- src/main/drivers/pwm_mapping.h | 5 +- src/main/drivers/pwm_rx.c | 2 +- src/main/drivers/rcc.h | 4 +- src/main/drivers/rcc_types.h | 4 ++ src/main/drivers/sdcard.c | 4 +- src/main/drivers/serial_usb_vcp.c | 4 +- src/main/drivers/sonar_hcsr04.h | 3 +- src/main/drivers/sound_beeper.h | 2 +- src/main/drivers/timer.c | 2 +- src/main/drivers/timer.h | 9 ++-- src/main/drivers/timer_stm32f10x.c | 5 ++ src/main/drivers/timer_stm32f30x.c | 5 ++ src/main/drivers/timer_stm32f4xx.c | 7 ++- src/main/drivers/transponder_ir.c | 6 +-- src/main/drivers/transponder_ir_stm32f30x.c | 6 +-- src/main/drivers/vtx_rtc6705.c | 6 +-- src/main/drivers/vtx_soft_spi_rtc6705.c | 7 +-- src/main/io/serial_4way_avrootloader.c | 2 + src/main/main.c | 1 + src/main/sensors/initialisation.c | 2 +- src/main/target/AIORACERF3/target.c | 1 + src/main/target/AIR32/target.c | 1 + src/main/target/ALIENFLIGHTF1/target.c | 1 + src/main/target/ALIENFLIGHTF1/target.h | 1 + src/main/target/ALIENFLIGHTF3/target.c | 1 + src/main/target/ALIENFLIGHTF3/target.h | 3 +- src/main/target/ALIENFLIGHTF4/target.c | 1 + src/main/target/BLUEJAYF4/target.c | 1 + src/main/target/CC3D/target.c | 1 + src/main/target/CHEBUZZF3/target.c | 1 + src/main/target/CJMCU/target.c | 1 + src/main/target/CJMCU/target.h | 1 + src/main/target/COLIBRI_RACE/target.c | 1 + src/main/target/DOGE/target.c | 1 + src/main/target/DOGE/target.h | 9 ++-- src/main/target/EUSTM32F103RC/target.c | 1 + src/main/target/EUSTM32F103RC/target.h | 1 + src/main/target/F4BY/target.c | 49 ++++++++++--------- src/main/target/F4BY/target.h | 3 +- src/main/target/FURYF3/target.c | 1 + src/main/target/FURYF4/target.c | 1 + src/main/target/FURYF4/target.h | 3 +- src/main/target/IRCFUSIONF3/target.c | 1 + src/main/target/IRCFUSIONF3/target.h | 3 +- src/main/target/KISSFC/target.c | 1 + src/main/target/KISSFC/target.h | 3 +- src/main/target/LUX_RACE/target.c | 1 + src/main/target/MICROSCISKY/target.c | 1 + src/main/target/MICROSCISKY/target.h | 1 + src/main/target/MOTOLAB/target.c | 1 + src/main/target/NAZE/target.c | 1 + src/main/target/NAZE/target.h | 1 + src/main/target/OLIMEXINO/target.c | 1 + src/main/target/OLIMEXINO/target.h | 1 + src/main/target/OMNIBUS/target.c | 1 + src/main/target/OMNIBUSF4/target.c | 1 + src/main/target/PIKOBLX/target.c | 1 + src/main/target/PIKOBLX/target.h | 3 +- src/main/target/PORT103R/target.c | 1 + src/main/target/PORT103R/target.h | 1 + src/main/target/REVO/target.c | 1 + src/main/target/REVONANO/target.c | 1 + src/main/target/RMDO/target.c | 1 + src/main/target/SINGULARITY/target.c | 1 + src/main/target/SINGULARITY/target.h | 3 +- src/main/target/SIRINFPV/target.c | 1 + src/main/target/SPARKY/target.c | 1 + src/main/target/SPRACINGF3/target.c | 1 + src/main/target/SPRACINGF3EVO/target.c | 1 + src/main/target/SPRACINGF3MINI/target.c | 1 + src/main/target/STM32F3DISCOVERY/target.c | 1 + src/main/target/STM32F3DISCOVERY/target.h | 1 + src/main/target/X_RACERSPI/target.c | 1 + src/main/target/X_RACERSPI/target.h | 3 +- src/main/target/ZCOREF3/target.c | 1 + src/main/target/ZCOREF3/target.h | 1 - src/main/telemetry/frsky.c | 2 + src/main/telemetry/telemetry.c | 2 +- 100 files changed, 212 insertions(+), 138 deletions(-) create mode 100644 src/main/drivers/io_types.h create mode 100644 src/main/drivers/rcc_types.h diff --git a/src/main/config/config.c b/src/main/config/config.c index dcb0ce85c3..1af50acbc8 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -23,6 +23,7 @@ #include "debug.h" #include "build_config.h" +#include "debug.h" #include "blackbox/blackbox_io.h" diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index 202b90e804..69a82a0adb 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -21,7 +21,7 @@ #include "platform.h" #include "system.h" -#include "gpio.h" +#include "io.h" #include "bus_i2c.h" #include "sensor.h" diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index a65abf9128..6bba109396 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -27,6 +27,8 @@ #include "adc.h" #include "adc_impl.h" +#include "common/utils.h" + //#define DEBUG_ADC_CHANNELS #ifdef USE_ADC diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h index 55914a3365..47a4b52a0e 100644 --- a/src/main/drivers/adc.h +++ b/src/main/drivers/adc.h @@ -17,7 +17,7 @@ #pragma once -#include "io.h" +#include "io_types.h" typedef enum { ADC_BATTERY = 0, diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 146ada62be..c89dc1bf53 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -17,8 +17,8 @@ #pragma once -#include "io.h" -#include "rcc.h" +#include "io_types.h" +#include "rcc_types.h" #if defined(STM32F4) #define ADC_TAG_MAP_COUNT 16 @@ -65,4 +65,4 @@ extern const adcTagMap_t adcTagMap[ADC_TAG_MAP_COUNT]; extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; -uint8_t adcChannelByTag(ioTag_t ioTag); \ No newline at end of file +uint8_t adcChannelByTag(ioTag_t ioTag); diff --git a/src/main/drivers/barometer_spi_bmp280.c b/src/main/drivers/barometer_spi_bmp280.c index 9d97343e11..0c97684972 100644 --- a/src/main/drivers/barometer_spi_bmp280.c +++ b/src/main/drivers/barometer_spi_bmp280.c @@ -15,6 +15,7 @@ * along with Betaflight. If not, see . */ +#include #include #include @@ -24,6 +25,7 @@ #include "barometer.h" #include "barometer_bmp280.h" +#include "io.h" #ifdef USE_BARO_SPI_BMP280 #define DISABLE_BMP280 IOHi(bmp280CsPin) diff --git a/src/main/drivers/bus_i2c.h b/src/main/drivers/bus_i2c.h index 86f7a66181..26cce3792f 100644 --- a/src/main/drivers/bus_i2c.h +++ b/src/main/drivers/bus_i2c.h @@ -21,8 +21,8 @@ #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) #define I2C_DEFAULT_TIMEOUT I2C_SHORT_TIMEOUT -#include "drivers/io.h" -#include "drivers/rcc.h" +#include "io_types.h" +#include "rcc_types.h" #ifndef I2C_DEVICE #define I2C_DEVICE I2CINVALID diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c index 104c77c379..42ea8cadc9 100644 --- a/src/main/drivers/bus_i2c_soft.c +++ b/src/main/drivers/bus_i2c_soft.c @@ -22,7 +22,7 @@ #include "build_config.h" #include "bus_i2c.h" -#include "drivers/io.h" +#include "io.h" // Software I2C driver, using same pins as hardware I2C, with hw i2c module disabled. // Can be configured for I2C2 pinout (SCL: PB10, SDA: PB11) or I2C1 pinout (SCL: PB6, SDA: PB7) diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 88b267f111..f8c5b23697 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -20,9 +20,10 @@ #include -#include "gpio.h" #include "system.h" -#include "drivers/io_impl.h" +#include "io.h" +#include "io_impl.h" +#include "rcc.h" #include "bus_i2c.h" diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 5a6ea7dd37..58c76c9d78 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -17,9 +17,8 @@ #pragma once -#include -#include "io.h" -#include "rcc.h" +#include "io_types.h" +#include "rcc_types.h" #if defined(STM32F4) || defined(STM32F3) #define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h index 035a2c936f..e2f71fd28d 100644 --- a/src/main/drivers/compass_hmc5883l.h +++ b/src/main/drivers/compass_hmc5883l.h @@ -17,7 +17,7 @@ #pragma once -#include "io.h" +#include "io_types.h" typedef struct hmc5883Config_s { ioTag_t intTag; diff --git a/src/main/drivers/dma.c b/src/main/drivers/dma.c index 0ad411312e..7e4942fbe1 100644 --- a/src/main/drivers/dma.c +++ b/src/main/drivers/dma.c @@ -21,8 +21,8 @@ #include -#include "drivers/nvic.h" -#include "drivers/dma.h" +#include "nvic.h" +#include "dma.h" /* * DMA descriptors. diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c index ec6e7908d3..8372ad0bcb 100644 --- a/src/main/drivers/dma_stm32f4xx.c +++ b/src/main/drivers/dma_stm32f4xx.c @@ -21,8 +21,8 @@ #include -#include "drivers/nvic.h" -#include "drivers/dma.h" +#include "nvic.h" +#include "dma.h" /* * DMA descriptors. diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index a0afb64804..44d7b95464 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -22,9 +22,10 @@ #ifdef USE_FLASH_M25P16 -#include "drivers/flash_m25p16.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "flash_m25p16.h" +#include "io.h" +#include "bus_spi.h" +#include "system.h" #define M25P16_INSTRUCTION_RDID 0x9F #define M25P16_INSTRUCTION_READ_BYTES 0x03 diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h index 223efa1809..6cad8a8ea6 100644 --- a/src/main/drivers/flash_m25p16.h +++ b/src/main/drivers/flash_m25p16.h @@ -19,7 +19,7 @@ #include #include "flash.h" -#include "io.h" +#include "io_types.h" #define M25P16_PAGESIZE 256 diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index cd95e186e1..dbdc2244c9 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -10,9 +10,9 @@ #include "platform.h" -#include "drivers/sensor.h" -#include "drivers/accgyro.h" -#include "drivers/gyro_sync.h" +#include "sensor.h" +#include "accgyro.h" +#include "gyro_sync.h" static uint8_t mpuDividerDrops; diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index d655646282..5afa4abb76 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -6,16 +6,7 @@ #include #include "resource.h" -// IO pin identification -// make sure that ioTag_t can't be assigned into IO_t without warning -typedef uint8_t ioTag_t; // packet tag to specify IO pin -typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change - -// NONE initializer for ioTag_t variables -#define IOTAG_NONE ((ioTag_t)0) - -// NONE initializer for IO_t variable -#define IO_NONE ((IO_t)0) +#include "io_types.h" // preprocessor is used to convert pinid to requested C data value // compile-time error is generated if requested pin is not available (not set in TARGET_IO_PORTx) @@ -24,19 +15,6 @@ typedef void* IO_t; // type specifying IO pin. Currently ioRec_t poin // expand pinid to to ioTag_t #define IO_TAG(pinid) DEFIO_TAG(pinid) -// both ioTag_t and IO_t are guarantied to be zero if pinid is NONE (no pin) -// this simplifies initialization (globals are zeroed on start) and allows -// omitting unused fields in structure initializers. -// it is also possible to use IO_t and ioTag_t as boolean value -// TODO - this may conflict with requirement to generate warning/error on IO_t - ioTag_t assignment -// IO_t being pointer is only possibility I know of .. - -// pin config handling -// pin config is packed into ioConfig_t to decrease memory requirements -// IOCFG_x macros are defined for common combinations for all CPUs; this -// helps masking CPU differences - -typedef uint8_t ioConfig_t; // packed IO configuration #if defined(STM32F1) // mode is using only bits 6-2 diff --git a/src/main/drivers/io_types.h b/src/main/drivers/io_types.h new file mode 100644 index 0000000000..ba80cd3bc7 --- /dev/null +++ b/src/main/drivers/io_types.h @@ -0,0 +1,28 @@ +#pragma once + +#include + +// IO pin identification +// make sure that ioTag_t can't be assigned into IO_t without warning +typedef uint8_t ioTag_t; // packet tag to specify IO pin +typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change + +// NONE initializer for ioTag_t variables +#define IOTAG_NONE ((ioTag_t)0) + +// NONE initializer for IO_t variable +#define IO_NONE ((IO_t)0) + +// both ioTag_t and IO_t are guarantied to be zero if pinid is NONE (no pin) +// this simplifies initialization (globals are zeroed on start) and allows +// omitting unused fields in structure initializers. +// it is also possible to use IO_t and ioTag_t as boolean value +// TODO - this may conflict with requirement to generate warning/error on IO_t - ioTag_t assignment +// IO_t being pointer is only possibility I know of .. + +// pin config handling +// pin config is packed into ioConfig_t to decrease memory requirements +// IOCFG_x macros are defined for common combinations for all CPUs; this +// helps masking CPU differences + +typedef uint8_t ioConfig_t; // packed IO configuration diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 6c8f2bb18a..d9c29af370 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -36,8 +36,8 @@ #include "common/color.h" #include "common/colorconversion.h" -#include "drivers/dma.h" -#include "drivers/light_ws2811strip.h" +#include "dma.h" +#include "light_ws2811strip.h" uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; volatile uint8_t ws2811LedDataTransferInProgress = 0; diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index 4b588a0c9f..911b7cdc66 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -23,10 +23,11 @@ #ifdef LED_STRIP #include "common/color.h" -#include "drivers/light_ws2811strip.h" +#include "light_ws2811strip.h" #include "nvic.h" #include "io.h" #include "dma.h" +#include "rcc.h" #include "timer.h" static IO_t ws2811IO = IO_NONE; diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index 8379188f99..1fadfb1f03 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -26,7 +26,7 @@ #include "nvic.h" #include "common/color.h" -#include "drivers/light_ws2811strip.h" +#include "light_ws2811strip.h" #include "dma.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 82f3f29af9..7ddc2a058b 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -19,16 +19,19 @@ #include #include "platform.h" -#include "version.h" #ifdef USE_MAX7456 +#include "version.h" + #include "common/printf.h" -#include "drivers/bus_spi.h" -#include "drivers/light_led.h" -#include "drivers/system.h" -#include "drivers/nvic.h" -#include "drivers/dma.h" + +#include "bus_spi.h" +#include "light_led.h" +#include "io.h" +#include "system.h" +#include "nvic.h" +#include "dma.h" #include "max7456.h" #include "max7456_symbols.h" diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 29a5d02f15..c017061835 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -17,7 +17,7 @@ #pragma once -#include "timer.h" +#include "io_types.h" #define MAX_PWM_MOTORS 12 #define MAX_PWM_SERVOS 8 @@ -88,10 +88,11 @@ typedef enum { PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4) } pwmPortFlags_e; +struct timerHardware_s; typedef struct pwmPortConfiguration_s { uint8_t index; pwmPortFlags_e flags; - const timerHardware_t *timerHardware; + const struct timerHardware_s *timerHardware; } pwmPortConfiguration_t; typedef struct pwmOutputConfiguration_s { diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 3c72d104dc..60af85e97b 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -30,7 +30,7 @@ #include "system.h" #include "nvic.h" -#include "gpio.h" +#include "io.h" #include "timer.h" #include "pwm_output.h" diff --git a/src/main/drivers/rcc.h b/src/main/drivers/rcc.h index fb04ec7e2c..69dce965f9 100644 --- a/src/main/drivers/rcc.h +++ b/src/main/drivers/rcc.h @@ -2,6 +2,7 @@ #include "platform.h" #include "common/utils.h" +#include "rcc_types.h" enum rcc_reg { RCC_EMPTY = 0, // make sure that default value (0) does not enable anything @@ -17,9 +18,6 @@ enum rcc_reg { #define RCC_APB1(periph) RCC_ENCODE(RCC_APB1, RCC_APB1ENR_ ## periph ## EN) #define RCC_AHB1(periph) RCC_ENCODE(RCC_AHB1, RCC_AHB1ENR_ ## periph ## EN) -typedef uint8_t rccPeriphTag_t; - void RCC_ClockCmd(rccPeriphTag_t periphTag, FunctionalState NewState); void RCC_ResetCmd(rccPeriphTag_t periphTag, FunctionalState NewState); - diff --git a/src/main/drivers/rcc_types.h b/src/main/drivers/rcc_types.h new file mode 100644 index 0000000000..1b7d7b9f2f --- /dev/null +++ b/src/main/drivers/rcc_types.h @@ -0,0 +1,4 @@ +#pragma once + +typedef uint8_t rccPeriphTag_t; + diff --git a/src/main/drivers/sdcard.c b/src/main/drivers/sdcard.c index 878d5eacf6..a86fc74577 100644 --- a/src/main/drivers/sdcard.c +++ b/src/main/drivers/sdcard.c @@ -25,8 +25,8 @@ #include "nvic.h" #include "io.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "bus_spi.h" +#include "system.h" #include "sdcard.h" #include "sdcard_standard.h" diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 2c2cebbac6..50c6055ddd 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -22,7 +22,7 @@ #include "build_config.h" #include "common/utils.h" -#include "drivers/io.h" +#include "io.h" #include "usb_core.h" #ifdef STM32F4 @@ -32,7 +32,7 @@ #include "hw_config.h" #endif -#include "drivers/system.h" +#include "system.h" #include "serial.h" #include "serial_usb_vcp.h" diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h index d734f62f4f..cb3f38c989 100644 --- a/src/main/drivers/sonar_hcsr04.h +++ b/src/main/drivers/sonar_hcsr04.h @@ -17,8 +17,7 @@ #pragma once -#include "platform.h" -#include "io.h" +#include "io_types.h" typedef struct sonarHardware_s { ioTag_t triggerTag; diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index ab7a7c3dfc..9caad4f554 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -17,7 +17,7 @@ #pragma once -#include "drivers/io.h" +#include "io_types.h" #ifdef BEEPER #define BEEP_TOGGLE systemBeepToggle() diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 15e36cce3f..5051588c99 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -25,7 +25,7 @@ #include "nvic.h" -#include "gpio.h" +#include "io.h" #include "rcc.h" #include "system.h" diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 7050bbabda..00410a9c68 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -17,12 +17,11 @@ #pragma once -#include "io.h" -#include "rcc.h" +#include +#include -#if !defined(USABLE_TIMER_CHANNEL_COUNT) -#define USABLE_TIMER_CHANNEL_COUNT 14 -#endif +#include "io_types.h" +#include "rcc_types.h" typedef uint16_t captureCompare_t; // 16 bit on both 103 and 303, just register access must be 32bit sometimes (use timCCR_t) diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index 1a5085e63a..49106818f0 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -5,6 +5,11 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f10x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 04069b20c3..99111bad77 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -5,6 +5,11 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f30x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index af09f85e1b..ccce10350a 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -5,9 +5,14 @@ http://www.st.com/software_license_agreement_liberty_v2 */ +#include +#include + +#include "platform.h" + #include "stm32f4xx.h" -#include "timer.h" #include "rcc.h" +#include "timer.h" /** * @brief Selects the TIM Output Compare Mode. diff --git a/src/main/drivers/transponder_ir.c b/src/main/drivers/transponder_ir.c index 626c60097c..dbc6749af7 100644 --- a/src/main/drivers/transponder_ir.c +++ b/src/main/drivers/transponder_ir.c @@ -21,9 +21,9 @@ #include -#include "drivers/dma.h" -#include "drivers/nvic.h" -#include "drivers/transponder_ir.h" +#include "dma.h" +#include "nvic.h" +#include "transponder_ir.h" /* * Implementation note: diff --git a/src/main/drivers/transponder_ir_stm32f30x.c b/src/main/drivers/transponder_ir_stm32f30x.c index 3962e1c999..0bff2e112c 100644 --- a/src/main/drivers/transponder_ir_stm32f30x.c +++ b/src/main/drivers/transponder_ir_stm32f30x.c @@ -20,9 +20,9 @@ #include -#include "drivers/gpio.h" -#include "drivers/transponder_ir.h" -#include "drivers/nvic.h" +#include "gpio.h" +#include "transponder_ir.h" +#include "nvic.h" #ifndef TRANSPONDER_GPIO #define USE_TRANSPONDER_ON_DMA1_CHANNEL3 diff --git a/src/main/drivers/vtx_rtc6705.c b/src/main/drivers/vtx_rtc6705.c index 93ffa83ecb..08ec758e8d 100644 --- a/src/main/drivers/vtx_rtc6705.c +++ b/src/main/drivers/vtx_rtc6705.c @@ -31,9 +31,9 @@ #include "common/maths.h" -#include "drivers/vtx_rtc6705.h" -#include "drivers/bus_spi.h" -#include "drivers/system.h" +#include "vtx_rtc6705.h" +#include "bus_spi.h" +#include "system.h" #define RTC6705_SET_HEAD 0x3210 //fosc=8mhz r=400 #define RTC6705_SET_A1 0x8F3031 //5865 diff --git a/src/main/drivers/vtx_soft_spi_rtc6705.c b/src/main/drivers/vtx_soft_spi_rtc6705.c index 4ceba50df9..e6340680fa 100644 --- a/src/main/drivers/vtx_soft_spi_rtc6705.c +++ b/src/main/drivers/vtx_soft_spi_rtc6705.c @@ -22,9 +22,10 @@ #ifdef USE_RTC6705 -#include "drivers/bus_spi.h" -#include "drivers/system.h" -#include "drivers/light_led.h" +#include "bus_spi.h" +#include "io.h" +#include "system.h" +#include "light_led.h" #include "vtx_soft_spi_rtc6705.h" diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index f0f5525813..223d3e10cd 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -25,8 +25,10 @@ #include "platform.h" #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE +#include "drivers/io_types.h" #include "drivers/system.h" #include "drivers/serial.h" +#include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "io/serial.h" #include "io/serial_msp.h" diff --git a/src/main/main.c b/src/main/main.c index 24b983308d..436f5b5798 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -33,6 +33,7 @@ #include "drivers/system.h" #include "drivers/dma.h" #include "drivers/gpio.h" +#include "drivers/io.h" #include "drivers/light_led.h" #include "drivers/sound_beeper.h" #include "drivers/timer.h" diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index bb46059ac1..c3465b3934 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,7 +24,7 @@ #include "common/axis.h" -#include "drivers/gpio.h" +#include "drivers/io.h" #include "drivers/system.h" #include "drivers/exti.h" diff --git a/src/main/target/AIORACERF3/target.c b/src/main/target/AIORACERF3/target.c index e780364ec2..f950c67844 100644 --- a/src/main/target/AIORACERF3/target.c +++ b/src/main/target/AIORACERF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/AIR32/target.c b/src/main/target/AIR32/target.c index 378627daaf..b5b9889379 100644 --- a/src/main/target/AIR32/target.c +++ b/src/main/target/AIR32/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF1/target.c b/src/main/target/ALIENFLIGHTF1/target.c index 7116b53551..dec8739ba0 100644 --- a/src/main/target/ALIENFLIGHTF1/target.c +++ b/src/main/target/ALIENFLIGHTF1/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 6d945008f7..04984912cd 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -90,4 +90,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index c1946aa17c..8169e9e912 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 28a896ff01..d2e9e92ba4 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -35,8 +35,6 @@ #define BEEPER PA5 -#define USABLE_TIMER_CHANNEL_COUNT 11 - #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_EXTI @@ -131,5 +129,6 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 11 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index efedb56689..dafcdf86da 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index b85e50f539..83e0700cd5 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index a870de9e24..589c6af30d 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 631d83e6aa..c2aca46716 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index 0f8228559e..3562e3234d 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h index 973e0984f9..beb4be9657 100644 --- a/src/main/target/CJMCU/target.h +++ b/src/main/target/CJMCU/target.h @@ -71,4 +71,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 91b21c8b36..099f9b62e7 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index 05b642f088..ca13ef7d0b 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 2399ede8d1..3f40c4ab43 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -67,11 +67,6 @@ #define M25P16_CS_PIN PC15 #define M25P16_SPI_INSTANCE SPI2 -// timer definitions in drivers/timer.c -// channel mapping in drivers/pwm_mapping.c -// only 6 outputs available on hardware -#define USABLE_TIMER_CHANNEL_COUNT 9 - #define USE_ESCSERIAL #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 @@ -147,5 +142,9 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +// timer definitions in drivers/timer.c +// channel mapping in drivers/pwm_mapping.c +// only 6 outputs available on hardware +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15)) diff --git a/src/main/target/EUSTM32F103RC/target.c b/src/main/target/EUSTM32F103RC/target.c index b2a0db9a16..0dc2d66044 100644 --- a/src/main/target/EUSTM32F103RC/target.c +++ b/src/main/target/EUSTM32F103RC/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 12ec85f371..d883641d4f 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -113,5 +113,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index a8d80e823d..e9befde0ab 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -3,17 +3,18 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM7 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed @@ -47,13 +48,13 @@ const uint16_t multiPWM[] = { const uint16_t airPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), PWM12 | (MAP_TO_SERVO_OUTPUT << 8), PWM13 | (MAP_TO_SERVO_OUTPUT << 8), PWM14 | (MAP_TO_SERVO_OUTPUT << 8), - PWM15 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed - PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM7 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed @@ -84,23 +85,23 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN - { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN - { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN - { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN - { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN - { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN - { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN - { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN + { TIM3, IO_TAG(PC9), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S1_IN + { TIM3, IO_TAG(PC8), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S2_IN + { TIM3, IO_TAG(PC6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S3_IN + { TIM3, IO_TAG(PC7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM3}, // S4_IN + { TIM4, IO_TAG(PD15), TIM_Channel_4, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S5_IN + { TIM4, IO_TAG(PD14), TIM_Channel_3, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S6_IN + { TIM4, IO_TAG(PD13), TIM_Channel_2, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S7_IN + { TIM4, IO_TAG(PD12), TIM_Channel_1, TIM4_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM4}, // S8_IN - { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT - { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT - { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT - { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT - { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT - { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT - { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT - { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S1_OUT + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2}, // S2_OUT + { TIM5, IO_TAG(PA2), TIM_Channel_3, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S3_OUT + { TIM5, IO_TAG(PA3), TIM_Channel_4, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5}, // S4_OUT + { TIM1, IO_TAG(PE9), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S5_OUT + { TIM1, IO_TAG(PE11), TIM_Channel_2, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S6_OUT + { TIM1, IO_TAG(PE13), TIM_Channel_3, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S7_OUT + { TIM1, IO_TAG(PE14), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM1}, // S8_OUT { TIM9, IO_TAG(PE6), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM9 }, // sonar echo if needed }; diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 7c9e89b48b..d8f8154031 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -74,8 +74,6 @@ #define SDCARD_DMA_CHANNEL DMA_Channel_0 -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define USE_VCP #define VBUS_SENSING_PIN PA9 @@ -156,5 +154,6 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) |TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index bf4ff7b20a..db28609d97 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index ac600ea439..a53aa4d447 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 8f69378592..05ff286283 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -101,8 +101,6 @@ #define M25P16_CS_PIN PB3 #define M25P16_SPI_INSTANCE SPI3 -#define USABLE_TIMER_CHANNEL_COUNT 5 - #define USE_VCP #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED @@ -167,5 +165,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 5 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(9)) diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index dd87a86410..2e4e12fb0a 100644 --- a/src/main/target/IRCFUSIONF3/target.c +++ b/src/main/target/IRCFUSIONF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 8dbd6e55ba..9ceabb35d2 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -23,8 +23,6 @@ #define LED0 PB3 -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready, no MAG #define USE_MPU_DATA_READY_SIGNAL @@ -105,5 +103,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index cbb9d74818..a443fe862f 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM7 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 05346145d6..e532b9d782 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -28,8 +28,6 @@ #define BEEPER PB13 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 12 - #define USE_EXTI #define MPU_INT_EXTI PB2 #define USE_MPU_DATA_READY_SIGNAL @@ -84,4 +82,5 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTF (BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index 91b21c8b36..099f9b62e7 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/MICROSCISKY/target.c b/src/main/target/MICROSCISKY/target.c index 7116b53551..dec8739ba0 100644 --- a/src/main/target/MICROSCISKY/target.c +++ b/src/main/target/MICROSCISKY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 80d4f04856..9138f74e86 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -97,4 +97,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index a89063cc9a..cc898ba834 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index 7116b53551..dec8739ba0 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 0e8ba62be2..7e3705b47a 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -163,4 +163,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC ( BIT(13) | BIT(14) | BIT(15) ) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/OLIMEXINO/target.c b/src/main/target/OLIMEXINO/target.c index ea20267b3c..5d0a271c5b 100644 --- a/src/main/target/OLIMEXINO/target.c +++ b/src/main/target/OLIMEXINO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index bb270cb4fc..ae8ac3b38e 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -91,4 +91,5 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 977d623f85..a3cd3e54d3 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 7dd021055e..2ed3eab8fb 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index a89063cc9a..cc898ba834 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM9 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 111f6fc476..7a104b7466 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -28,8 +28,6 @@ #define BEEPER PA0 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 9 - // MPU6000 interrupts #define USE_EXTI #define MPU_INT_EXTI PA15 @@ -136,5 +134,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 9 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17)) diff --git a/src/main/target/PORT103R/target.c b/src/main/target/PORT103R/target.c index 8f21b7cbb7..093fa6ee27 100644 --- a/src/main/target/PORT103R/target.c +++ b/src/main/target/PORT103R/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 66e83fa3d5..9f0239d572 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -123,5 +123,6 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(0)|BIT(1)|BIT(2)) +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4)) diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 67f7327285..a253a0b31f 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index 9a579f2bc9..bf5023fc24 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index ee96272c11..d344cda357 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index 8131579519..e48d0c33b4 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index 09a5678174..ed23489a3a 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -25,8 +25,6 @@ #define BEEPER PC15 -#define USABLE_TIMER_CHANNEL_COUNT 10 - #define USE_EXTI #define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL @@ -115,5 +113,6 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 10 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(16) |TIM_N(17)) diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index b52fbd00e4..7d602c74b1 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM7 | (MAP_TO_PPM_INPUT << 8), diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index 389617278b..20b1f185d3 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index ec9176a27f..99f79ec9b7 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 843ad3d9aa..87eb5db35f 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index d1e3a0d8b2..bf8776aa92 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index 1712bebae2..d2c17db5a2 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 592833bc48..a75e952171 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -165,5 +165,6 @@ #define TARGET_IO_PORTE 0xffff #define TARGET_IO_PORTF 0x00ff +#define USABLE_TIMER_CHANNEL_COUNT 14 #define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(16) | TIM_N(17)) diff --git a/src/main/target/X_RACERSPI/target.c b/src/main/target/X_RACERSPI/target.c index 85db79093c..a63258100e 100644 --- a/src/main/target/X_RACERSPI/target.c +++ b/src/main/target/X_RACERSPI/target.c @@ -5,6 +5,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 7838b1f460..9cc822ebee 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -26,8 +26,6 @@ #define BEEPER PC15 #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 17 - #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH @@ -128,5 +126,6 @@ #define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) +#define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/target/ZCOREF3/target.c b/src/main/target/ZCOREF3/target.c index b72400e6fd..48a6ce60da 100644 --- a/src/main/target/ZCOREF3/target.c +++ b/src/main/target/ZCOREF3/target.c @@ -5,6 +5,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input diff --git a/src/main/target/ZCOREF3/target.h b/src/main/target/ZCOREF3/target.h index c65abb7d92..5c07c78190 100644 --- a/src/main/target/ZCOREF3/target.h +++ b/src/main/target/ZCOREF3/target.h @@ -101,6 +101,5 @@ #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) #define USABLE_TIMER_CHANNEL_COUNT 17 // PPM, 8 PWM, UART3 RX/TX, LED Strip - #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 33bd13193c..d698b958a0 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -19,6 +19,8 @@ * Initial FrSky Telemetry implementation by silpstream @ rcgroups. * Addition protocol work by airmamaf @ github. */ + +#include #include #include diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 96e2193e9a..6373d166eb 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -15,6 +15,7 @@ * along with Cleanflight. If not, see . */ +#include #include #include @@ -22,7 +23,6 @@ #ifdef TELEMETRY -#include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" From 66c2699c926a71a31c4aae551364b4aec047e728 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 01:10:46 +0100 Subject: [PATCH 230/456] Added #pragma once to debug.h --- src/main/debug.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/debug.h b/src/main/debug.h index 4431953f31..87dfa0add9 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -15,6 +15,8 @@ * along with Cleanflight. If not, see . */ +#pragma once + #define DEBUG16_VALUE_COUNT 4 extern int16_t debug[DEBUG16_VALUE_COUNT]; extern uint8_t debugMode; From 7bbb4e59f9f79d7bc20e705282db27c327161d0f Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 11:00:40 +0100 Subject: [PATCH 231/456] Removed dependencies on sensors/ --- src/main/drivers/compass_ak8963.c | 3 --- src/main/drivers/compass_ak8975.c | 2 -- src/main/drivers/compass_hmc5883l.c | 2 -- 3 files changed, 7 deletions(-) diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index 0668fcdd77..d5b9a59dec 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -33,9 +33,6 @@ #include "bus_i2c.h" #include "bus_spi.h" -#include "sensors/boardalignment.h" -#include "sensors/sensors.h" - #include "sensor.h" #include "compass.h" diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index 0d648ecc58..0e4d3677c5 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -33,8 +33,6 @@ #include "gpio.h" #include "bus_i2c.h" -#include "sensors/sensors.h" - #include "sensor.h" #include "compass.h" diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 3709cc510e..10fa9b3c6d 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -39,8 +39,6 @@ #include "sensor.h" #include "compass.h" -#include "sensors/sensors.h" - #include "compass_hmc5883l.h" //#define DEBUG_MAG_DATA_READY_INTERRUPT From 2b05cbf9165e42e8c6244ef2aff1a831e02b5bf3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 09:02:31 +0100 Subject: [PATCH 232/456] Put each PID in its own .c file --- Makefile | 2 + src/main/flight/pid.c | 391 ++----------------------------- src/main/flight/pid.h | 5 + src/main/flight/pid_betaflight.c | 279 ++++++++++++++++++++++ src/main/flight/pid_legacy.c | 210 +++++++++++++++++ 5 files changed, 511 insertions(+), 376 deletions(-) create mode 100644 src/main/flight/pid_betaflight.c create mode 100644 src/main/flight/pid_legacy.c diff --git a/Makefile b/Makefile index 281b169c63..68364d7b7c 100644 --- a/Makefile +++ b/Makefile @@ -392,6 +392,8 @@ COMMON_SRC = \ flight/imu.c \ flight/mixer.c \ flight/pid.c \ + flight/pid_legacy.c \ + flight/pid_betaflight.c \ io/beeper.c \ io/rc_controls.c \ io/rc_curves.c \ diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0f91875e2a..068012ad5a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -47,32 +47,25 @@ #include "config/runtime_config.h" -extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float setpointRate[3]; -extern float rcInput[3]; -static bool pidStabilisationEnabled; +bool pidStabilisationEnabled; int16_t axisPID[3]; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + #ifdef BLACKBOX int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; #endif -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; +int32_t errorGyroI[3]; +float errorGyroIf[3]; -static int32_t errorGyroI[3]; -static float errorGyroIf[3]; - -static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); #ifdef SKIP_PID_FLOAT pidControllerFuncPtr pid_controller = pidLegacy; // which pid controller are we using #else -static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig); pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using #endif @@ -94,7 +87,7 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false; } -float getdT (void) +float getdT(void) { static float dT; if (!dT) dT = (float)targetPidLooptime * 0.000001f; @@ -104,13 +97,15 @@ float getdT (void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static pt1Filter_t deltaFilter[3]; -static pt1Filter_t yawFilter; -static biquadFilter_t dtermFilterLpf[3]; -static biquadFilter_t dtermFilterNotch[3]; -static bool dtermNotchInitialised, dtermBiquadLpfInitialised; +pt1Filter_t deltaFilter[3]; +pt1Filter_t yawFilter; +biquadFilter_t dtermFilterLpf[3]; +biquadFilter_t dtermFilterNotch[3]; +bool dtermNotchInitialised; +bool dtermBiquadLpfInitialised; -void initFilters(const pidProfile_t *pidProfile) { +void initFilters(const pidProfile_t *pidProfile) +{ int axis; if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { @@ -125,361 +120,6 @@ void initFilters(const pidProfile_t *pidProfile) { } } -#ifndef SKIP_PID_FLOAT -// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. Based on 2DOF reference design (matlab) -static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - float errorRate = 0, rP = 0, rD = 0, PVRate = 0; - float ITerm,PTerm,DTerm; - static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; - float delta; - int axis; - float horizonLevelStrength = 1; - - float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection - if(pidProfile->D8[PIDLEVEL] == 0){ - horizonLevelStrength = 0; - } else { - horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); - } - } - - // Yet Highly experimental and under test and development - // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) - static float kiThrottleGain = 1.0f; - if (pidProfile->itermThrottleGain) { - const uint16_t maxLoopCount = 20000 / targetPidLooptime; - const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; - static int16_t previousThrottle; - static uint16_t loopIncrement; - - if (loopIncrement >= maxLoopCount) { - kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 - previousThrottle = rcCommand[THROTTLE]; - loopIncrement = 0; - } else { - loopIncrement++; - } - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - static uint8_t configP[3], configI[3], configD[3]; - - // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now - // Prepare all parameters for PID controller - if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { - Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; - Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; - Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; - b[axis] = pidProfile->ptermSetpointWeight / 100.0f; - c[axis] = pidProfile->dtermSetpointWeight / 100.0f; - yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT(); - rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT(); - - configP[axis] = pidProfile->P8[axis]; - configI[axis] = pidProfile->I8[axis]; - configD[axis] = pidProfile->D8[axis]; - } - - // Limit abrupt yaw inputs / stops - float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; - if (maxVelocity) { - float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; - if (ABS(currentVelocity) > maxVelocity) { - setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; - } - previousSetpoint[axis] = setpointRate[axis]; - } - - // Yaw control is GYRO based, direct sticks control is applied to rate PID - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to the max inclination -#ifdef GPS - const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#else - const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; - } else { - // HORIZON mode - direct sticks control is applied to rate PID - // mix up angle error to desired AngleRate to add a little auto-level feel - setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); - } - } - - PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec - - // --------low-level gyro-based PID based on 2DOF PID controller. ---------- - // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // ----- calculate error / angle rates ---------- - errorRate = setpointRate[axis] - PVRate; // r - y - rP = b[axis] * setpointRate[axis] - PVRate; // br - y - - // Slowly restore original setpoint with more stick input - float diffRate = errorRate - rP; - rP += diffRate * rcInput[axis]; - - // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount - float dynReduction = tpaFactor; - if (pidProfile->toleranceBand) { - const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; - static uint8_t zeroCrossCount[3]; - static uint8_t currentErrorPolarity[3]; - if (ABS(errorRate) < pidProfile->toleranceBand) { - if (zeroCrossCount[axis]) { - if (currentErrorPolarity[axis] == POSITIVE_ERROR) { - if (errorRate < 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = NEGATIVE_ERROR; - } - } else { - if (errorRate > 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = POSITIVE_ERROR; - } - } - } else { - dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); - } - } else { - zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; - currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; - } - } - - // -----calculate P component - PTerm = Kp[axis] * rP * dynReduction; - - // -----calculate I component. - // Reduce strong Iterm accumulation during higher stick inputs - float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); - - // Handle All windup Scenarios - // limit maximum integrator value to prevent WindUp - float itermScaler = setpointRateScaler * kiThrottleGain; - - errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); - - // I coefficient (I8) moved before integration to make limiting independent from PID settings - ITerm = errorGyroIf[axis]; - - //-----calculate D-term (Yaw D not yet supported) - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = lrintf(PTerm + ITerm); - - DTerm = 0.0f; // needed for blackbox - } else { - rD = c[axis] * setpointRate[axis] - PVRate; // cr - y - delta = rD - lastRateError[axis]; - lastRateError[axis] = rD; - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta *= (1.0f / getdT()); - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; - - // Filter delta - if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); - - if (pidProfile->dterm_lpf_hz) { - if (dtermBiquadLpfInitialised) { - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } - } - - DTerm = Kd[axis] * delta * dynReduction; - - // -----calculate total PID output - axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); - } - - // Disable PID control at zero throttle - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} -#endif - -// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. Don't expect much development in the future -static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) -{ - int axis; - int32_t PTerm, ITerm, DTerm, delta; - static int32_t lastRateError[3]; - int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; - - int8_t horizonLevelStrength = 100; - - initFilters(pidProfile); - - if (FLIGHT_MODE(HORIZON_MODE)) { - // Figure out the raw stick positions - const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); - const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); - const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); - // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection - // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. - // For more rate mode increase D and slower flips and rolls will be possible - horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); - } - - // ----------PID controller---------- - for (axis = 0; axis < 3; axis++) { - - // -----Get the desired angle rate depending on flight mode - AngleRateTmp = (int32_t)setpointRate[axis]; - - if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { - // calculate error angle and limit the angle to max configured inclination -#ifdef GPS - const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#else - const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; -#endif - if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed - AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; - } else { - // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, - // horizonLevelStrength is scaled to the stick input - AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); - } - } - - // --------low-level gyro-based PID. ---------- - // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes - // -----calculate scaled error.AngleRates - // multiplication of rcCommand corresponds to changing the sticks scaling here - gyroRate = gyroADC[axis] / 4; - - RateError = AngleRateTmp - gyroRate; - - // -----calculate P component - PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; - - // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply - if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { - PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); - } - - // -----calculate I component - // there should be no division before accumulating the error to integrator, because the precision would be reduced. - // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. - // Time correction (to avoid different I scaling for different builds based on average cycle time) - // is normalized to cycle time = 2048. - // Prevent Accumulation - uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); - uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; - - errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; - - // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. - // I coefficient (I8) moved before integration to make limiting independent from PID settings - errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - - ITerm = errorGyroI[axis] >> 13; - - //-----calculate D-term - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = PTerm + ITerm; - - if (motorCount >= 4) { - int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); - - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); - } - - DTerm = 0; // needed for blackbox - } else { - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = RateError - lastRateError[axis]; - lastRateError[axis] = RateError; - } else { - delta = -(gyroRate - lastRateError[axis]); - lastRateError[axis] = gyroRate; - } - - // Divide delta by targetLooptime to get differential (ie dr/dt) - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; - - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // Filter delta - if (pidProfile->dterm_lpf_hz) { - float deltaf = delta; // single conversion - if (dtermBiquadLpfInitialised) { - delta = biquadFilterApply(&dtermFilterLpf[axis], delta); - } else { - delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); - } - delta = lrintf(deltaf); - } - - DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; - - // -----calculate total PID output - axisPID[axis] = PTerm + ITerm + DTerm; - } - - if (!pidStabilisationEnabled) axisPID[axis] = 0; - -#ifdef GTUNE - if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { - calculate_Gtune(axis); - } -#endif - -#ifdef BLACKBOX - axisPID_P[axis] = PTerm; - axisPID_I[axis] = ITerm; - axisPID_D[axis] = DTerm; -#endif - } -} - void pidSetController(pidControllerType_e type) { switch (type) { @@ -493,4 +133,3 @@ void pidSetController(pidControllerType_e type) #endif } } - diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c12404ec5a..56a89e9382 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -118,6 +118,11 @@ struct rxConfig_s; typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype +void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); +void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, + const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); + extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool airmodeWasActivated; diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c new file mode 100644 index 0000000000..db07f11bb6 --- /dev/null +++ b/src/main/flight/pid_betaflight.c @@ -0,0 +1,279 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#ifndef SKIP_PID_FLOAT + +#include "build_config.h" +#include "debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" + +#include "drivers/accgyro.h" +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/gtune.h" + +#include "config/runtime_config.h" + +extern float rcInput[3]; +extern float setpointRate[3]; + +extern float errorGyroIf[3]; +extern bool pidStabilisationEnabled; + +extern pt1Filter_t deltaFilter[3]; +extern pt1Filter_t yawFilter; +extern biquadFilter_t dtermFilterLpf[3]; +extern biquadFilter_t dtermFilterNotch[3]; +extern bool dtermNotchInitialised; +extern bool dtermBiquadLpfInitialised; + +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + +void initFilters(const pidProfile_t *pidProfile); +float getdT(void); + +// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. +// Based on 2DOF reference design (matlab) +void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) +{ + float errorRate = 0, rP = 0, rD = 0, PVRate = 0; + float ITerm,PTerm,DTerm; + static float lastRateError[2]; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; + float delta; + int axis; + float horizonLevelStrength = 1; + + float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection + if(pidProfile->D8[PIDLEVEL] == 0){ + horizonLevelStrength = 0; + } else { + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->D8[PIDLEVEL])) + 1, 0, 1); + } + } + + // Yet Highly experimental and under test and development + // Throttle coupled to Igain like inverted TPA // 50hz calculation (should cover all rx protocols) + static float kiThrottleGain = 1.0f; + if (pidProfile->itermThrottleGain) { + const uint16_t maxLoopCount = 20000 / targetPidLooptime; + const float throttleItermGain = (float)pidProfile->itermThrottleGain * 0.001f; + static int16_t previousThrottle; + static uint16_t loopIncrement; + + if (loopIncrement >= maxLoopCount) { + kiThrottleGain = 1.0f + constrainf((float)(ABS(rcCommand[THROTTLE] - previousThrottle)) * throttleItermGain, 0.0f, 5.0f); // Limit to factor 5 + previousThrottle = rcCommand[THROTTLE]; + loopIncrement = 0; + } else { + loopIncrement++; + } + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + static uint8_t configP[3], configI[3], configD[3]; + + // Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now + // Prepare all parameters for PID controller + if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) { + Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; + Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; + Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; + b[axis] = pidProfile->ptermSetpointWeight / 100.0f; + c[axis] = pidProfile->dtermSetpointWeight / 100.0f; + yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); + rollPitchMaxVelocity = pidProfile->pidMaxVelocityRollPitch * 1000 * getdT(); + + configP[axis] = pidProfile->P8[axis]; + configI[axis] = pidProfile->I8[axis]; + configD[axis] = pidProfile->D8[axis]; + } + + // Limit abrupt yaw inputs / stops + float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; + if (maxVelocity) { + float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; + if (ABS(currentVelocity) > maxVelocity) { + setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; + } + previousSetpoint[axis] = setpointRate[axis]; + } + + // Yaw control is GYRO based, direct sticks control is applied to rate PID + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to the max inclination +#ifdef GPS + const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#else + const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; + } else { + // HORIZON mode - direct sticks control is applied to rate PID + // mix up angle error to desired AngleRate to add a little auto-level feel + setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); + } + } + + PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec + + // --------low-level gyro-based PID based on 2DOF PID controller. ---------- + // ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // ----- calculate error / angle rates ---------- + errorRate = setpointRate[axis] - PVRate; // r - y + rP = b[axis] * setpointRate[axis] - PVRate; // br - y + + // Slowly restore original setpoint with more stick input + float diffRate = errorRate - rP; + rP += diffRate * rcInput[axis]; + + // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount + float dynReduction = tpaFactor; + if (pidProfile->toleranceBand) { + const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; + static uint8_t zeroCrossCount[3]; + static uint8_t currentErrorPolarity[3]; + if (ABS(errorRate) < pidProfile->toleranceBand) { + if (zeroCrossCount[axis]) { + if (currentErrorPolarity[axis] == POSITIVE_ERROR) { + if (errorRate < 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = NEGATIVE_ERROR; + } + } else { + if (errorRate > 0 ) { + zeroCrossCount[axis]--; + currentErrorPolarity[axis] = POSITIVE_ERROR; + } + } + } else { + dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); + } + } else { + zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; + currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; + } + } + + // -----calculate P component + PTerm = Kp[axis] * rP * dynReduction; + + // -----calculate I component. + // Reduce strong Iterm accumulation during higher stick inputs + float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); + + // Handle All windup Scenarios + // limit maximum integrator value to prevent WindUp + float itermScaler = setpointRateScaler * kiThrottleGain; + + errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f); + + // I coefficient (I8) moved before integration to make limiting independent from PID settings + ITerm = errorGyroIf[axis]; + + //-----calculate D-term (Yaw D not yet supported) + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = lrintf(PTerm + ITerm); + + DTerm = 0.0f; // needed for blackbox + } else { + rD = c[axis] * setpointRate[axis] - PVRate; // cr - y + delta = rD - lastRateError[axis]; + lastRateError[axis] = rD; + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta *= (1.0f / getdT()); + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; + + // Filter delta + if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); + + if (pidProfile->dterm_lpf_hz) { + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + } + + DTerm = Kd[axis] * delta * dynReduction; + + // -----calculate total PID output + axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); + } + + // Disable PID control at zero throttle + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif + } +} +#endif + diff --git a/src/main/flight/pid_legacy.c b/src/main/flight/pid_legacy.c new file mode 100644 index 0000000000..0424f5461f --- /dev/null +++ b/src/main/flight/pid_legacy.c @@ -0,0 +1,210 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#include "build_config.h" +#include "debug.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.h" + +#include "drivers/sensor.h" + +#include "drivers/accgyro.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/acceleration.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation.h" +#include "flight/gtune.h" + +#include "config/runtime_config.h" + +extern uint8_t motorCount; +extern uint8_t PIDweight[3]; +extern bool pidStabilisationEnabled; +extern float setpointRate[3]; +extern int32_t errorGyroI[3]; +extern pt1Filter_t deltaFilter[3]; +extern pt1Filter_t yawFilter; +extern biquadFilter_t dtermFilterLpf[3]; +extern bool dtermBiquadLpfInitialised; + + +void initFilters(const pidProfile_t *pidProfile); +float getdT(void); + + +// Legacy pid controller betaflight evolved pid rewrite based on 2.9 releae. Good for fastest cycletimes for those who believe in that. +// Don't expect much development in the future +void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) +{ + int axis; + int32_t PTerm, ITerm, DTerm, delta; + static int32_t lastRateError[3]; + int32_t AngleRateTmp = 0, RateError = 0, gyroRate = 0; + + int8_t horizonLevelStrength = 100; + + initFilters(pidProfile); + + if (FLIGHT_MODE(HORIZON_MODE)) { + // Figure out the raw stick positions + const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc)); + const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc)); + const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle); + // Progressively turn off the horizon self level strength as the stick is banged over + horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection + // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine. + // For more rate mode increase D and slower flips and rolls will be possible + horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100); + } + + // ----------PID controller---------- + for (axis = 0; axis < 3; axis++) { + + // -----Get the desired angle rate depending on flight mode + AngleRateTmp = (int32_t)setpointRate[axis]; + + if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { + // calculate error angle and limit the angle to max configured inclination +#ifdef GPS + const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +#else + const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination), + +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]; +#endif + if (FLIGHT_MODE(ANGLE_MODE)) { + // ANGLE mode - control is angle based, so control loop is needed + AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4; + } else { + // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel, + // horizonLevelStrength is scaled to the stick input + AngleRateTmp = AngleRateTmp + ((errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4); + } + } + + // --------low-level gyro-based PID. ---------- + // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes + // -----calculate scaled error.AngleRates + // multiplication of rcCommand corresponds to changing the sticks scaling here + gyroRate = gyroADC[axis] / 4; + + RateError = AngleRateTmp - gyroRate; + + // -----calculate P component + PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + + // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply + if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { + PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); + } + + // -----calculate I component + // there should be no division before accumulating the error to integrator, because the precision would be reduced. + // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. + // Time correction (to avoid different I scaling for different builds based on average cycle time) + // is normalized to cycle time = 2048. + // Prevent Accumulation + uint16_t resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; + uint16_t dynamicFactor = (1 << 8) - constrain(((ABS(AngleRateTmp) << 6) / resetRate), 0, 1 << 8); + uint16_t dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8; + + errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * dynamicKi; + + // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. + // I coefficient (I8) moved before integration to make limiting independent from PID settings + errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); + + ITerm = errorGyroI[axis] >> 13; + + //-----calculate D-term + if (axis == YAW) { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = PTerm + ITerm; + + if (motorCount >= 4) { + int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); + + // prevent "yaw jump" during yaw correction + axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); + } + + DTerm = 0; // needed for blackbox + } else { + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = RateError - lastRateError[axis]; + lastRateError[axis] = RateError; + } else { + delta = -(gyroRate - lastRateError[axis]); + lastRateError[axis] = gyroRate; + } + + // Divide delta by targetLooptime to get differential (ie dr/dt) + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; + + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // Filter delta + if (pidProfile->dterm_lpf_hz) { + float deltaf = delta; // single conversion + if (dtermBiquadLpfInitialised) { + delta = biquadFilterApply(&dtermFilterLpf[axis], delta); + } else { + delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); + } + delta = lrintf(deltaf); + } + + DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // -----calculate total PID output + axisPID[axis] = PTerm + ITerm + DTerm; + } + + if (!pidStabilisationEnabled) axisPID[axis] = 0; + +#ifdef GTUNE + if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { + calculate_Gtune(axis); + } +#endif + +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = DTerm; +#endif + } +} + From 0cb6968e24a67dea179c295037ae47d8333965d6 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 14:20:39 +0200 Subject: [PATCH 233/456] Move PIDweight to a common place. pid_legacy should not depend on pid_betaflight. --- src/main/flight/pid.h | 3 +++ src/main/flight/pid_betaflight.c | 3 --- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 56a89e9382..60efb88a50 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -128,6 +128,9 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool airmodeWasActivated; extern uint32_t targetPidLooptime; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +extern uint8_t PIDweight[3]; + void pidSetController(pidControllerType_e type); void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index db07f11bb6..a2dc204d98 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -62,9 +62,6 @@ extern biquadFilter_t dtermFilterNotch[3]; extern bool dtermNotchInitialised; extern bool dtermBiquadLpfInitialised; -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; - void initFilters(const pidProfile_t *pidProfile); float getdT(void); From ade8e9fe3a1c0ee6e1de55b37e746a3143517aec Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 15:16:30 +0200 Subject: [PATCH 234/456] Added missing include. Target VRRACE build failed. --- src/main/target/VRRACE/target.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/VRRACE/target.c b/src/main/target/VRRACE/target.c index 094ade421e..90270b1fa8 100644 --- a/src/main/target/VRRACE/target.c +++ b/src/main/target/VRRACE/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input From cead17456fb7766f4c3c938a21e5b51feaefdd3b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 09:02:31 +0100 Subject: [PATCH 235/456] Put each PID in its own .c file --- src/main/flight/pid_betaflight.c | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index a2dc204d98..030324065d 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -62,6 +62,9 @@ extern biquadFilter_t dtermFilterNotch[3]; extern bool dtermNotchInitialised; extern bool dtermBiquadLpfInitialised; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + void initFilters(const pidProfile_t *pidProfile); float getdT(void); @@ -72,7 +75,7 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; float delta; int axis; float horizonLevelStrength = 1; @@ -134,14 +137,13 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio configD[axis] = pidProfile->D8[axis]; } - // Limit abrupt yaw inputs / stops - float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; - if (maxVelocity) { - float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; - if (ABS(currentVelocity) > maxVelocity) { - setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; + // Limit abrupt yaw inputs + if (axis == YAW && pidProfile->pidMaxVelocityYaw) { + float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; + if (ABS(yawCurrentVelocity) > yawMaxVelocity) { + setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; } - previousSetpoint[axis] = setpointRate[axis]; + yawPreviousRate = setpointRate[axis]; } // Yaw control is GYRO based, direct sticks control is applied to rate PID From 049b255e0227bbf15b4bbc9a992570df2469f80a Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 14:20:39 +0200 Subject: [PATCH 236/456] Move PIDweight to a common place. pid_legacy should not depend on pid_betaflight. --- src/main/flight/pid_betaflight.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index 030324065d..058e587603 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -62,9 +62,6 @@ extern biquadFilter_t dtermFilterNotch[3]; extern bool dtermNotchInitialised; extern bool dtermBiquadLpfInitialised; -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; - void initFilters(const pidProfile_t *pidProfile); float getdT(void); From 7ebd5402bbfbf551b8ee44a9350b628a00b66d13 Mon Sep 17 00:00:00 2001 From: kc10kevin Date: Sat, 6 Aug 2016 18:19:02 -0500 Subject: [PATCH 237/456] MPU6500 INT Fix for F4 --- src/main/drivers/accgyro_mpu6500.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index ed872230ea..514504d626 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -111,6 +111,9 @@ void mpu6500GyroInit(uint8_t lpf) #else mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN #endif + + delay(15); + #ifdef USE_MPU_DATA_READY_SIGNAL mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable #endif From fa78bc138a2515f2882d4df870affbb06df1c792 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 00:55:16 +0100 Subject: [PATCH 238/456] Optimisation of driver header files From 00a5b0b85749ed4e461509caf57bc6571f4f48e9 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 01:10:46 +0100 Subject: [PATCH 239/456] Added #pragma once to debug.h From 4f49ae7370574cf5822a6aa3bfd14d9ca92c6e37 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 11:00:40 +0100 Subject: [PATCH 240/456] Removed dependencies on sensors/ From e9545563d796a0320138c4494ff708a9533fedf1 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 09:02:31 +0100 Subject: [PATCH 241/456] Put each PID in its own .c file --- src/main/flight/pid_betaflight.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index 058e587603..030324065d 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -62,6 +62,9 @@ extern biquadFilter_t dtermFilterNotch[3]; extern bool dtermNotchInitialised; extern bool dtermBiquadLpfInitialised; +// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction +uint8_t PIDweight[3]; + void initFilters(const pidProfile_t *pidProfile); float getdT(void); From 433bc9a80ac748b380b5a7a2faf4f6592307b21a Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 14:20:39 +0200 Subject: [PATCH 242/456] Move PIDweight to a common place. pid_legacy should not depend on pid_betaflight. --- src/main/flight/pid_betaflight.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index 030324065d..058e587603 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -62,9 +62,6 @@ extern biquadFilter_t dtermFilterNotch[3]; extern bool dtermNotchInitialised; extern bool dtermBiquadLpfInitialised; -// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction -uint8_t PIDweight[3]; - void initFilters(const pidProfile_t *pidProfile); float getdT(void); From 8768384f4bcedd016c78cb9deee9eba2ec452893 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 15:16:30 +0200 Subject: [PATCH 243/456] Added missing include. Target VRRACE build failed. From 49ed96c7b5cf95f142abbd8ea07228fb8af2d348 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 4 Aug 2016 01:37:59 +0200 Subject: [PATCH 244/456] Start on version 3.1.0 From ccc9d862f4533d086c5a1fc0a92bbb79c4457a1b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 00:55:16 +0100 Subject: [PATCH 245/456] Optimisation of driver header files From bd0ab2967660eda0c9cf33213c39ea62b0610c5c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 01:10:46 +0100 Subject: [PATCH 246/456] Added #pragma once to debug.h From 7276a688d0ec0ab62febff438149d25194752c70 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 11:00:40 +0100 Subject: [PATCH 247/456] Removed dependencies on sensors/ From 82e6163b7f117c9cb4eba71b4870ca7b9ab8b858 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 14:20:39 +0200 Subject: [PATCH 248/456] Move PIDweight to a common place. pid_legacy should not depend on pid_betaflight. From 75e8b51d0afb7ca890a91bcacf7bfadf6e982c27 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Thu, 4 Aug 2016 15:16:30 +0200 Subject: [PATCH 249/456] Added missing include. Target VRRACE build failed. From 3999fbc95be957f22965f4da7403a09b6f1f9d8b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 5 Aug 2016 13:55:09 +0200 Subject: [PATCH 250/456] Fix rebase artifacts --- src/main/flight/pid_betaflight.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index 058e587603..a2dc204d98 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -72,7 +72,7 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], yawMaxVelocity, yawPreviousRate; + static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; float delta; int axis; float horizonLevelStrength = 1; @@ -134,13 +134,14 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio configD[axis] = pidProfile->D8[axis]; } - // Limit abrupt yaw inputs - if (axis == YAW && pidProfile->pidMaxVelocityYaw) { - float yawCurrentVelocity = setpointRate[axis] - yawPreviousRate; - if (ABS(yawCurrentVelocity) > yawMaxVelocity) { - setpointRate[axis] = (yawCurrentVelocity > 0) ? yawPreviousRate + yawMaxVelocity : yawPreviousRate - yawMaxVelocity; + // Limit abrupt yaw inputs / stops + float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity; + if (maxVelocity) { + float currentVelocity = setpointRate[axis] - previousSetpoint[axis]; + if (ABS(currentVelocity) > maxVelocity) { + setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity; } - yawPreviousRate = setpointRate[axis]; + previousSetpoint[axis] = setpointRate[axis]; } // Yaw control is GYRO based, direct sticks control is applied to rate PID From 0de27cf7c2540f31876d4474e3255e4d379516a0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 17:30:08 +0100 Subject: [PATCH 251/456] Used forward declarations to remove #includes from header files --- src/main/blackbox/blackbox.c | 1 + src/main/blackbox/blackbox_io.c | 1 + src/main/common/printf.h | 11 +++-------- src/main/flight/altitudehold.h | 16 +++++++++------- src/main/flight/imu.h | 3 ++- src/main/flight/pid.c | 2 ++ src/main/flight/pid.h | 1 - src/main/io/display.c | 2 ++ src/main/io/rc_controls.h | 9 ++++----- src/main/io/rc_curves.c | 12 +++++++----- src/main/io/rc_curves.h | 4 +++- src/main/io/serial_4way.c | 10 ++++++---- src/main/io/serial_4way.h | 6 ++++-- src/main/io/serial_4way_avrootloader.c | 3 ++- src/main/io/serial_4way_impl.h | 2 +- src/main/io/serial_4way_stk500v2.c | 3 +++ src/main/io/serial_msp.c | 1 + src/main/io/serial_msp.h | 11 +++++------ src/main/rx/rx.c | 15 +++++++-------- src/main/rx/xbus.h | 6 +++--- src/main/sensors/battery.c | 2 ++ src/main/telemetry/frsky.h | 8 ++++---- 22 files changed, 72 insertions(+), 57 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 406ce05d39..d2fcb40a5a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -72,6 +72,7 @@ #include "flight/altitudehold.h" #include "flight/failsafe.h" #include "flight/imu.h" +#include "flight/pid.h" #include "flight/navigation.h" #include "config/runtime_config.h" diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index cd4f05d980..88306767c6 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -53,6 +53,7 @@ #include "flight/altitudehold.h" #include "flight/failsafe.h" #include "flight/imu.h" +#include "flight/pid.h" #include "flight/navigation.h" #include "config/runtime_config.h" diff --git a/src/main/common/printf.h b/src/main/common/printf.h index ac5493590b..467b054d52 100644 --- a/src/main/common/printf.h +++ b/src/main/common/printf.h @@ -102,13 +102,10 @@ For further details see source code. regs Kusti, 23.10.2004 */ -#ifndef __TFP_PRINTF__ -#define __TFP_PRINTF__ +#pragma once #include -#include "drivers/serial.h" - void init_printf(void *putp, void (*putf) (void *, char)); int tfp_printf(const char *fmt, ...); @@ -119,8 +116,6 @@ int tfp_format(void *putp, void (*putf) (void *, char), const char *fmt, va_list #define printf tfp_printf #define sprintf tfp_sprintf -void setPrintfSerialPort(serialPort_t *serialPort); +struct serialPort_s; +void setPrintfSerialPort(struct serialPort_s *serialPort); void printfSupportInit(void); - - -#endif diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index 3677fbb287..da16978814 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -15,19 +15,21 @@ * along with Cleanflight. If not, see . */ -#include "io/escservo.h" -#include "io/rc_controls.h" -#include "flight/pid.h" - -#include "sensors/barometer.h" +#pragma once extern int32_t AltHold; extern int32_t vario; void calculateEstimatedAltitude(uint32_t currentTime); -void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig); -void applyAltHold(airplaneConfig_t *airplaneConfig); +struct pidProfile_s; +struct barometerConfig_s; +struct rcControlsConfig_s; +struct escAndServoConfig_s; +void configureAltitudeHold(struct pidProfile_s *initialPidProfile, struct barometerConfig_s *intialBarometerConfig, struct rcControlsConfig_s *initialRcControlsConfig, struct escAndServoConfig_s *initialEscAndServoConfig); + +struct airplaneConfig_t; +void applyAltHold(struct airplaneConfig_s *airplaneConfig); void updateAltHoldState(void); void updateSonarAltHoldState(void); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 94b361fcf6..e5229a2ead 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -69,9 +69,10 @@ typedef struct accProcessor_s { accProcessorState_e state; } accProcessor_t; +struct pidProfile_s; void imuConfigure( imuRuntimeConfig_t *initialImuRuntimeConfig, - pidProfile_t *initialPidProfile, + struct pidProfile_s *initialPidProfile, accDeadband_t *initialAccDeadband, uint16_t throttle_correction_angle ); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 068012ad5a..be1bcf2034 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -51,6 +51,8 @@ uint32_t targetPidLooptime; bool pidStabilisationEnabled; +uint8_t PIDweight[3]; + int16_t axisPID[3]; // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 60efb88a50..c4509887dc 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -14,7 +14,6 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ -#include "rx/rx.h" #pragma once diff --git a/src/main/io/display.c b/src/main/io/display.c index 1e5eee9746..a22ca88023 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -63,6 +63,8 @@ #include "io/display.h" +#include "rx/rx.h" + #include "scheduler/scheduler.h" extern profile_t *currentProfile; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index aace839d9b..a3622aacab 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -17,8 +17,6 @@ #pragma once -#include "rx/rx.h" - typedef enum { BOXARM = 0, BOXANGLE, @@ -167,8 +165,9 @@ typedef struct rcControlsConfig_s { bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); -throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); -void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); +struct rxConfig_s; +throttleStatus_e calculateThrottleStatus(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void processRcStickPositions(struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); @@ -257,7 +256,7 @@ bool isAirmodeActive(void); bool isSuperExpoActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); -void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); +void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig); bool isUsingSticksForArming(void); diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index 89b46522f2..f96426bfdf 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -18,13 +18,15 @@ #include #include -#include "io/rc_controls.h" -#include "io/escservo.h" - -#include "io/rc_curves.h" - #include "config/config.h" +#include "io/escservo.h" +#include "io/rc_curves.h" +#include "io/rc_controls.h" + +#include "rx/rx.h" + + #define THROTTLE_LOOKUP_LENGTH 12 static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE diff --git a/src/main/io/rc_curves.h b/src/main/io/rc_curves.h index 747a934df3..406218d03b 100644 --- a/src/main/io/rc_curves.h +++ b/src/main/io/rc_curves.h @@ -17,7 +17,9 @@ #pragma once -void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig); +struct controlRateConfig_s; +struct escAndServoConfig_s; +void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct escAndServoConfig_s *escAndServoConfig); int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate); int16_t rcLookupThrottle(int32_t tmp); diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 9bd2ba69e9..0065cff899 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -23,21 +23,23 @@ #include #include "platform.h" + #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE -#include "drivers/serial.h" + #include "drivers/buf_writer.h" -#include "drivers/gpio.h" +#include "drivers/io.h" +#include "drivers/serial.h" #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "drivers/light_led.h" #include "drivers/system.h" + #include "flight/mixer.h" + #include "io/beeper.h" #include "io/serial_msp.h" -#include "io/serial_msp.h" #include "io/serial_4way.h" -#include "io/serial_4way_impl.h" #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #include "io/serial_4way_avrootloader.h" diff --git a/src/main/io/serial_4way.h b/src/main/io/serial_4way.h index b1bba5ff69..8e0c934a97 100644 --- a/src/main/io/serial_4way.h +++ b/src/main/io/serial_4way.h @@ -17,7 +17,8 @@ */ #pragma once -#include "serial_4way_impl.h" +#include "drivers/io_types.h" +#include "io/serial_4way_impl.h" #define USE_SERIAL_4WAY_BLHELI_BOOTLOADER #define USE_SERIAL_4WAY_SK_BOOTLOADER @@ -46,5 +47,6 @@ typedef union __attribute__ ((packed)) { bool isMcuConnected(void); uint8_t esc4wayInit(void); -void esc4wayProcess(serialPort_t *mspPort); +struct serialPort_s; +void esc4wayProcess(struct serialPort_s *mspPort); void esc4wayRelease(void); diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index 223d3e10cd..f3eeba5320 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -25,7 +25,8 @@ #include "platform.h" #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE -#include "drivers/io_types.h" + +#include "drivers/io.h" #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/timer.h" diff --git a/src/main/io/serial_4way_impl.h b/src/main/io/serial_4way_impl.h index a8f83bfae8..351aba0b62 100644 --- a/src/main/io/serial_4way_impl.h +++ b/src/main/io/serial_4way_impl.h @@ -17,7 +17,7 @@ */ #pragma once -#include "drivers/io.h" +#include "drivers/io_types.h" typedef struct { IO_t io; diff --git a/src/main/io/serial_4way_stk500v2.c b/src/main/io/serial_4way_stk500v2.c index 342a0c8b06..0b406531f0 100644 --- a/src/main/io/serial_4way_stk500v2.c +++ b/src/main/io/serial_4way_stk500v2.c @@ -23,7 +23,10 @@ #include #include "platform.h" + #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE + +#include "drivers/io.h" #include "drivers/serial.h" #include "config/config.h" #include "io/serial.h" diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index a2f472f45d..031d0ddf6b 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -37,6 +37,7 @@ #include "drivers/serial.h" #include "drivers/bus_i2c.h" +#include "drivers/io.h" #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 2778da7e22..114e94ab84 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -17,9 +17,6 @@ #pragma once -#include "io/serial.h" -#include "drivers/serial.h" - // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports. #define MAX_MSP_PORT_COUNT 2 @@ -46,7 +43,9 @@ typedef struct mspPort_s { uint8_t cmdMSP; } mspPort_t; -void mspInit(serialConfig_t *serialConfig); +struct serialConfig_s; +void mspInit(struct serialConfig_s *serialConfig); void mspProcess(void); -void mspAllocateSerialPorts(serialConfig_t *serialConfig); -void mspReleasePortIfAllocated(serialPort_t *serialPort); +void mspAllocateSerialPorts(struct serialConfig_s *serialConfig); +struct serialPort_s; +void mspReleasePortIfAllocated(struct serialPort_s *serialPort); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index a7a09a44b7..bcba5bb86e 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -32,15 +32,16 @@ #include "drivers/serial.h" #include "drivers/adc.h" -#include "io/serial.h" -#include "io/rc_controls.h" - -#include "flight/failsafe.h" - -#include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/system.h" + +#include "flight/failsafe.h" + +#include "io/serial.h" +#include "io/rc_controls.h" + +#include "rx/rx.h" #include "rx/pwm.h" #include "rx/sbus.h" #include "rx/spektrum.h" @@ -51,8 +52,6 @@ #include "rx/ibus.h" #include "rx/jetiexbus.h" -#include "rx/rx.h" - //#define DEBUG_RX_SIGNAL_LOSS diff --git a/src/main/rx/xbus.h b/src/main/rx/xbus.h index bc76f56786..cfaa920ffd 100644 --- a/src/main/rx/xbus.h +++ b/src/main/rx/xbus.h @@ -17,7 +17,7 @@ #pragma once -#include "rx/rx.h" - -bool xBusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback); +struct rxConfig_s; +struct rxRuntimeConfig_s; +bool xBusInit(struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig, rcReadRawDataPtr *callback); uint8_t xBusFrameStatus(void); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 75aac6cf0a..6d165cbc57 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -35,6 +35,8 @@ #include "io/rc_controls.h" #include "io/beeper.h" +#include "rx/rx.h" + #define VBATT_PRESENT_THRESHOLD_MV 10 #define VBATT_LPF_FREQ 0.4f diff --git a/src/main/telemetry/frsky.h b/src/main/telemetry/frsky.h index 555eb43e83..89ba617c8f 100644 --- a/src/main/telemetry/frsky.h +++ b/src/main/telemetry/frsky.h @@ -17,17 +17,17 @@ #pragma once -#include "rx/rx.h" - typedef enum { FRSKY_VFAS_PRECISION_LOW = 0, FRSKY_VFAS_PRECISION_HIGH } frskyVFasPrecision_e; -void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); +struct rxConfig_s; +void handleFrSkyTelemetry(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); void checkFrSkyTelemetryState(void); -void initFrSkyTelemetry(telemetryConfig_t *telemetryConfig); +struct telemetryConfig_s; +void initFrSkyTelemetry(struct telemetryConfig_s *telemetryConfig); void configureFrSkyTelemetryPort(void); void freeFrSkyTelemetryPort(void); From 58014f416e255819d87fff82f08ccd62d4006464 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 4 Aug 2016 19:45:43 +0100 Subject: [PATCH 252/456] Removed #includes from header files --- src/main/config/config.c | 1 + src/main/drivers/accgyro_mpu.c | 2 +- src/main/drivers/accgyro_mpu.h | 1 + src/main/drivers/bus_spi.c | 1 + src/main/drivers/exti.h | 2 +- src/main/drivers/rcc.h | 2 -- src/main/drivers/serial_usb_vcp.h | 5 ++--- src/main/drivers/system_stm32f4xx.c | 2 +- src/main/drivers/timer_stm32f10x.c | 2 ++ src/main/drivers/timer_stm32f30x.c | 2 ++ src/main/drivers/timer_stm32f4xx.c | 2 ++ src/main/io/beeper.c | 4 ++-- src/main/rx/jetiexbus.c | 10 ++++++---- src/main/sensors/battery.h | 2 +- src/main/target/ALIENFLIGHTF3/hardware_revision.h | 2 +- 15 files changed, 24 insertions(+), 16 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 1af50acbc8..da0aaea2cf 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -36,6 +36,7 @@ #include "drivers/accgyro.h" #include "drivers/compass.h" #include "drivers/system.h" +#include "drivers/io.h" #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 1c705ef137..4284697217 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -29,7 +29,7 @@ #include "nvic.h" #include "system.h" -#include "gpio.h" +#include "io.h" #include "exti.h" #include "bus_i2c.h" diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 132c9e13a0..0b70ebb14a 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -17,6 +17,7 @@ #pragma once +#include "io_types.h" #include "exti.h" // MPU6050 diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 690f25b52e..c86fdbdb48 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -21,6 +21,7 @@ #include #include "bus_spi.h" +#include "exti.h" #include "io.h" #include "io_impl.h" #include "rcc.h" diff --git a/src/main/drivers/exti.h b/src/main/drivers/exti.h index 8071c65e33..adc95f13ab 100644 --- a/src/main/drivers/exti.h +++ b/src/main/drivers/exti.h @@ -18,7 +18,7 @@ #pragma once -#include "drivers/io.h" +#include "io_types.h" // old EXTI interface, to be replaced typedef struct extiConfig_s { diff --git a/src/main/drivers/rcc.h b/src/main/drivers/rcc.h index 69dce965f9..4dee74ee79 100644 --- a/src/main/drivers/rcc.h +++ b/src/main/drivers/rcc.h @@ -1,7 +1,5 @@ #pragma once -#include "platform.h" -#include "common/utils.h" #include "rcc_types.h" enum rcc_reg { diff --git a/src/main/drivers/serial_usb_vcp.h b/src/main/drivers/serial_usb_vcp.h index 7d1b88be1f..26ef11215f 100644 --- a/src/main/drivers/serial_usb_vcp.h +++ b/src/main/drivers/serial_usb_vcp.h @@ -17,8 +17,6 @@ #pragma once -#include "serial.h" - typedef struct { serialPort_t port; @@ -30,4 +28,5 @@ typedef struct { } vcpPort_t; serialPort_t *usbVcpOpen(void); -uint32_t usbVcpGetBaudRate(serialPort_t *instance); +struct serialPort_s; +uint32_t usbVcpGetBaudRate(struct serialPort_s *instance); diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 5c09ebdf6a..8b48f82f5e 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -22,7 +22,7 @@ #include "platform.h" #include "accgyro_mpu.h" -#include "gpio.h" +#include "exti.h" #include "nvic.h" #include "system.h" diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index 49106818f0..d918b07112 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -10,6 +10,8 @@ #include "platform.h" +#include "common/utils.h" + #include "stm32f10x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 99111bad77..871cfb6d78 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -10,6 +10,8 @@ #include "platform.h" +#include "common/utils.h" + #include "stm32f30x.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index ccce10350a..750324e94a 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -10,6 +10,8 @@ #include "platform.h" +#include "common/utils.h" + #include "stm32f4xx.h" #include "rcc.h" #include "timer.h" diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 716cf84b36..cd616044d2 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -22,14 +22,14 @@ #include #include "build_config.h" -#include "io/rc_controls.h" - #include "drivers/gpio.h" #include "drivers/sound_beeper.h" #include "drivers/system.h" #include "sensors/battery.h" #include "sensors/sensors.h" +#include "io/rc_controls.h" + #include "io/statusindicator.h" #include "io/vtx.h" diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 2836d5ebca..d4050b8c84 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -39,12 +39,17 @@ #include "platform.h" -#include "common/utils.h" #include "build_config.h" +#include "debug.h" + +#include "common/utils.h" + #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/serial_uart.h" + #include "io/serial.h" + #include "rx/rx.h" #include "rx/jetiexbus.h" @@ -61,9 +66,6 @@ #endif //TELEMETRY -#include "debug.h" -#include "rx/rx.h" - // // Serial driver for Jeti EX Bus receiver // diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index f459a3bcc4..3cfc2702d3 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -17,7 +17,7 @@ #pragma once -#include "common/maths.h" +#include "common/maths.h" // for fix12_t #ifndef VBAT_SCALE_DEFAULT #define VBAT_SCALE_DEFAULT 110 diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.h b/src/main/target/ALIENFLIGHTF3/hardware_revision.h index 9b23c614d1..48d56a86a3 100644 --- a/src/main/target/ALIENFLIGHTF3/hardware_revision.h +++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.h @@ -29,4 +29,4 @@ extern uint8_t hardwareRevision; void updateHardwareRevision(void); void detectHardwareRevision(void); -const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void); \ No newline at end of file +const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void); From 4f6e0fc114a16cb65d43cd9067652f7040558029 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 5 Aug 2016 22:18:02 +0200 Subject: [PATCH 253/456] Sparky I2C initialization fix --- src/main/target/SPARKY/target.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 582b82bf76..6e056b7269 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -72,6 +72,9 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) +#define I2C2_SCL PA9 +#define I2C2_SDA PA10 + #define USE_ADC #define ADC_INSTANCE ADC2 #define VBAT_ADC_PIN PA4 From 9aa57eb6b0faf9aefec0e67da51c25eb7de05a13 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 7 Aug 2016 01:33:21 +0200 Subject: [PATCH 254/456] Fix rebase error --- src/main/flight/pid_betaflight.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index a2dc204d98..39616552e0 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -126,8 +126,8 @@ void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; b[axis] = pidProfile->ptermSetpointWeight / 100.0f; c[axis] = pidProfile->dtermSetpointWeight / 100.0f; - yawMaxVelocity = pidProfile->pidMaxVelocityYaw * 1000 * getdT(); - rollPitchMaxVelocity = pidProfile->pidMaxVelocityRollPitch * 1000 * getdT(); + yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT(); + rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT(); configP[axis] = pidProfile->P8[axis]; configI[axis] = pidProfile->I8[axis]; From 0dcc2bbeaa0f8738fecb7115e73ecb1b0de14680 Mon Sep 17 00:00:00 2001 From: kc10kevin Date: Sat, 6 Aug 2016 19:40:07 -0500 Subject: [PATCH 255/456] FuryF4 Target Changes/Cleanup --- src/main/target/FURYF4/config.c | 86 +++++++++++++++++++++++++++++++++ src/main/target/FURYF4/target.h | 23 +++------ 2 files changed, 94 insertions(+), 15 deletions(-) create mode 100644 src/main/target/FURYF4/config.c diff --git a/src/main/target/FURYF4/config.c b/src/main/target/FURYF4/config.c new file mode 100644 index 0000000000..0a12258f2d --- /dev/null +++ b/src/main/target/FURYF4/config.c @@ -0,0 +1,86 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include + +#include "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +// alternative defaults settings for FURYF4 targets +void targetConfiguration(void) { + masterConfig.mag_hardware = MAG_NONE; // disabled by default + masterConfig.motor_pwm_rate = 32000; + masterConfig.failsafeConfig.failsafe_delay = 2; + masterConfig.failsafeConfig.failsafe_off_delay = 0; + masterConfig.gyro_sync_denom = 1; + masterConfig.pid_process_denom = 1; +} diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 8f69378592..4006d9d665 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -18,6 +18,8 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "FYF4" +#define TARGET_CONFIG + #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) #define USBD_PRODUCT_STRING "FuryF4" @@ -36,6 +38,7 @@ #define MPU_INT_EXTI PC4 #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready (mag disabled) #define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 @@ -43,15 +46,18 @@ #define MPU6500_CS_PIN PA4 #define MPU6500_SPI_INSTANCE SPI1 +#define GYRO #define USE_GYRO_SPI_MPU6000 #define GYRO_MPU6000_ALIGN CW180_DEG -#define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW180_DEG #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define GYRO_MPU6500_ALIGN CW180_DEG +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG + #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 #define ACC_MPU6500_ALIGN CW180_DEG @@ -68,17 +74,6 @@ #define SDCARD_SPI_INSTANCE SPI2 #define SDCARD_SPI_CS_PIN PB12 -/* -#define SDCARD_DETECT_PIN PD2 -#define SDCARD_DETECT_EXTI_LINE EXTI_Line2 -#define SDCARD_DETECT_EXTI_PIN_SOURCE EXTI_PinSource2 -#define SDCARD_DETECT_EXTI_PORT_SOURCE EXTI_PortSourceGPIOD -#define SDCARD_DETECT_EXTI_IRQn EXTI2_IRQn - -#define SDCARD_SPI_INSTANCE SPI3 -#define SDCARD_SPI_CS_PIN PB3 -*/ - // SPI2 is on the APB1 bus whose clock runs at 84MHz. Divide to under 400kHz for init: #define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 256 // 328kHz // Divide to under 25MHz for normal operation: @@ -95,7 +90,6 @@ #define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 #define SDCARD_DMA_CHANNEL DMA_Channel_0 - #define USE_FLASHFS #define USE_FLASH_M25P16 #define M25P16_CS_PIN PB3 @@ -110,7 +104,6 @@ #define USE_UART1 #define UART1_RX_PIN PA10 #define UART1_TX_PIN PA9 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART3 #define UART3_RX_PIN PB11 From 4cc57c15f713d3e47ba2cbaf5bc5644bd35f9a1c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 7 Aug 2016 11:57:52 +0100 Subject: [PATCH 256/456] Fixed Colibri build --- src/main/target/COLIBRI/target.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c index dd87343119..63dd92cf80 100644 --- a/src/main/target/COLIBRI/target.c +++ b/src/main/target/COLIBRI/target.c @@ -19,7 +19,9 @@ #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input From 541f4d4018fbc6aef9eae99603f13f5a4de9b954 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 7 Aug 2016 11:46:47 +0100 Subject: [PATCH 257/456] Directory rearrangement to reflect cleanflight changes --- Makefile | 20 +++++++++---------- src/main/blackbox/blackbox.c | 10 ++++++---- src/main/blackbox/blackbox_io.c | 9 +++++---- src/main/{common => build}/atomic.h | 0 src/main/{ => build}/build_config.c | 0 src/main/{ => build}/build_config.h | 0 src/main/{ => build}/debug.c | 0 src/main/{ => build}/debug.h | 0 src/main/{ => build}/version.c | 0 src/main/{ => build}/version.h | 0 src/main/common/printf.c | 3 +-- src/main/common/typeconversion.c | 2 +- src/main/config/config.c | 12 +++++------ src/main/drivers/accgyro_l3gd20.c | 4 ++-- src/main/drivers/accgyro_lsm303dlhc.c | 2 +- src/main/drivers/accgyro_mpu.c | 5 +++-- src/main/drivers/accgyro_mpu6050.c | 5 +++-- src/main/drivers/accgyro_spi_mpu9250.c | 5 +++-- src/main/drivers/adc.c | 5 +++-- src/main/drivers/adc_stm32f10x.c | 2 +- src/main/drivers/barometer_bmp085.c | 2 +- src/main/drivers/barometer_bmp280.c | 2 +- src/main/drivers/barometer_ms5611.c | 4 ++-- src/main/drivers/bus_i2c_soft.c | 3 ++- src/main/drivers/compass_ak8963.c | 5 ++--- src/main/drivers/compass_ak8975.c | 2 +- src/main/drivers/compass_hmc5883l.c | 2 +- src/main/drivers/gpio_stm32f4xx.c | 2 +- src/main/drivers/light_ws2811strip.c | 2 +- src/main/drivers/max7456.c | 2 -- src/main/drivers/pwm_rx.c | 4 ++-- src/main/drivers/serial_softserial.c | 4 ++-- src/main/drivers/serial_uart.c | 2 +- src/main/drivers/serial_usb_vcp.c | 3 ++- src/main/drivers/sonar_hcsr04.c | 3 ++- src/main/drivers/timer.c | 4 +++- src/main/{ => fc}/mw.c | 9 +++++---- src/main/{ => fc}/mw.h | 0 src/main/{io => fc}/rc_controls.c | 11 +++++----- src/main/{io => fc}/rc_controls.h | 0 src/main/{io => fc}/rc_curves.c | 7 +++++-- src/main/{io => fc}/rc_curves.h | 0 src/main/{config => fc}/runtime_config.c | 4 +++- src/main/{config => fc}/runtime_config.h | 0 src/main/flight/altitudehold.c | 7 ++++--- src/main/flight/failsafe.c | 7 ++++--- src/main/flight/gtune.c | 5 +++-- src/main/flight/imu.c | 8 +++++--- src/main/flight/mixer.c | 9 +++++---- src/main/flight/navigation.c | 8 +++++--- src/main/flight/pid.c | 9 +++++---- src/main/flight/pid_betaflight.c | 10 +++++----- src/main/flight/pid_legacy.c | 9 +++++---- src/main/io/beeper.c | 8 +++++--- src/main/io/display.c | 11 +++++----- src/main/io/gps.c | 6 +++--- src/main/io/ledstrip.c | 15 +++++++------- src/main/io/osd.c | 20 ++++++++++--------- src/main/io/serial.c | 2 +- src/main/io/serial_cli.c | 11 +++++----- src/main/io/serial_msp.c | 14 +++++++------ src/main/io/transponder_ir.c | 2 +- src/main/io/vtx.c | 7 ++++--- src/main/main.c | 9 +++++---- src/main/rx/ibus.c | 4 ++-- src/main/rx/jetiexbus.c | 4 ++-- src/main/rx/msp.c | 2 +- src/main/rx/pwm.c | 2 +- src/main/rx/rx.c | 8 +++++--- src/main/rx/sbus.c | 2 +- src/main/rx/spektrum.c | 3 ++- src/main/rx/sumd.c | 2 +- src/main/rx/sumh.c | 2 +- src/main/scheduler/scheduler.c | 4 ++-- src/main/sensors/acceleration.c | 3 ++- src/main/sensors/battery.c | 8 +++++--- src/main/sensors/compass.c | 3 ++- src/main/sensors/gyro.c | 3 ++- src/main/sensors/initialisation.c | 4 ++-- src/main/sensors/sonar.c | 6 ++++-- src/main/target/ALIENFLIGHTF1/config.c | 9 +++++---- src/main/target/ALIENFLIGHTF3/config.c | 9 +++++---- .../target/ALIENFLIGHTF3/hardware_revision.c | 3 ++- src/main/target/ALIENFLIGHTF4/config.c | 9 +++++---- src/main/target/BLUEJAYF4/config.c | 9 +++++---- src/main/target/BLUEJAYF4/hardware_revision.c | 3 ++- src/main/target/CJMCU/hardware_revision.c | 2 +- src/main/target/COLIBRI/config.c | 10 +++++----- .../target/COLIBRI_RACE/bus_bst_stm32f30x.c | 2 +- src/main/target/COLIBRI_RACE/config.c | 9 +++++---- src/main/target/COLIBRI_RACE/i2c_bst.c | 12 +++++------ src/main/target/MOTOLAB/config.c | 9 +++++---- src/main/target/NAZE/hardware_revision.c | 2 +- src/main/telemetry/frsky.c | 5 +++-- src/main/telemetry/hott.c | 6 +++--- src/main/telemetry/ltm.c | 7 ++++--- src/main/telemetry/smartport.c | 5 +++-- src/main/telemetry/telemetry.c | 5 +++-- src/main/vcp/hw_config.c | 2 +- 99 files changed, 287 insertions(+), 229 deletions(-) rename src/main/{common => build}/atomic.h (100%) rename src/main/{ => build}/build_config.c (100%) rename src/main/{ => build}/build_config.h (100%) rename src/main/{ => build}/debug.c (100%) rename src/main/{ => build}/debug.h (100%) rename src/main/{ => build}/version.c (100%) rename src/main/{ => build}/version.h (100%) rename src/main/{ => fc}/mw.c (99%) rename src/main/{ => fc}/mw.h (100%) rename src/main/{io => fc}/rc_controls.c (99%) rename src/main/{io => fc}/rc_controls.h (100%) rename src/main/{io => fc}/rc_curves.c (96%) rename src/main/{io => fc}/rc_curves.h (100%) rename src/main/{config => fc}/runtime_config.c (97%) rename src/main/{config => fc}/runtime_config.h (100%) diff --git a/Makefile b/Makefile index 68364d7b7c..c65876bcac 100644 --- a/Makefile +++ b/Makefile @@ -122,9 +122,9 @@ endif REVISION = $(shell git log -1 --format="%h") -FC_VER_MAJOR := $(shell grep " FC_VERSION_MAJOR" src/main/version.h | awk '{print $$3}' ) -FC_VER_MINOR := $(shell grep " FC_VERSION_MINOR" src/main/version.h | awk '{print $$3}' ) -FC_VER_PATCH := $(shell grep " FC_VERSION_PATCH" src/main/version.h | awk '{print $$3}' ) +FC_VER_MAJOR := $(shell grep " FC_VERSION_MAJOR" src/main/build/version.h | awk '{print $$3}' ) +FC_VER_MINOR := $(shell grep " FC_VERSION_MINOR" src/main/build/version.h | awk '{print $$3}' ) +FC_VER_PATCH := $(shell grep " FC_VERSION_PATCH" src/main/build/version.h | awk '{print $$3}' ) FC_VER := $(FC_VER_MAJOR).$(FC_VER_MINOR).$(FC_VER_PATCH) @@ -357,19 +357,19 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \ VPATH := $(VPATH):$(TARGET_DIR) COMMON_SRC = \ - build_config.c \ - debug.c \ - version.c \ + build/build_config.c \ + build/debug.c \ + build/version.c \ $(TARGET_DIR_SRC) \ main.c \ - mw.c \ + fc/mw.c \ common/encoding.c \ common/filter.c \ common/maths.c \ common/printf.c \ common/typeconversion.c \ config/config.c \ - config/runtime_config.c \ + fc/runtime_config.c \ drivers/adc.c \ drivers/buf_writer.c \ drivers/bus_i2c_soft.c \ @@ -395,8 +395,8 @@ COMMON_SRC = \ flight/pid_legacy.c \ flight/pid_betaflight.c \ io/beeper.c \ - io/rc_controls.c \ - io/rc_curves.c \ + fc/rc_controls.c \ + fc/rc_curves.c \ io/serial.c \ io/serial_4way.c \ io/serial_4way_avrootloader.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index d2fcb40a5a..0ac0b0ff78 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -19,11 +19,12 @@ #include #include "platform.h" -#include "version.h" -#include "debug.h" #ifdef BLACKBOX +#include "build/version.h" +#include "build/debug.h" + #include "common/maths.h" #include "common/axis.h" #include "common/color.h" @@ -52,7 +53,7 @@ #include "io/beeper.h" #include "io/display.h" #include "io/escservo.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/gimbal.h" #include "io/gps.h" #include "io/ledstrip.h" @@ -75,7 +76,8 @@ #include "flight/pid.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 88306767c6..8f7d81ac24 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -4,8 +4,8 @@ #include "blackbox_io.h" -#include "version.h" -#include "build_config.h" +#include "build/version.h" +#include "build/build_config.h" #include "common/maths.h" #include "common/axis.h" @@ -34,7 +34,7 @@ #include "io/display.h" #include "io/escservo.h" #include "rx/rx.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/osd.h" #include "io/vtx.h" @@ -56,7 +56,8 @@ #include "flight/pid.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" diff --git a/src/main/common/atomic.h b/src/main/build/atomic.h similarity index 100% rename from src/main/common/atomic.h rename to src/main/build/atomic.h diff --git a/src/main/build_config.c b/src/main/build/build_config.c similarity index 100% rename from src/main/build_config.c rename to src/main/build/build_config.c diff --git a/src/main/build_config.h b/src/main/build/build_config.h similarity index 100% rename from src/main/build_config.h rename to src/main/build/build_config.h diff --git a/src/main/debug.c b/src/main/build/debug.c similarity index 100% rename from src/main/debug.c rename to src/main/build/debug.c diff --git a/src/main/debug.h b/src/main/build/debug.h similarity index 100% rename from src/main/debug.h rename to src/main/build/debug.h diff --git a/src/main/version.c b/src/main/build/version.c similarity index 100% rename from src/main/version.c rename to src/main/build/version.c diff --git a/src/main/version.h b/src/main/build/version.h similarity index 100% rename from src/main/version.h rename to src/main/build/version.h diff --git a/src/main/common/printf.c b/src/main/common/printf.c index e11d49d151..39d072dbe6 100644 --- a/src/main/common/printf.c +++ b/src/main/common/printf.c @@ -36,12 +36,11 @@ #include "platform.h" -#include "build_config.h" +#include "build/build_config.h" #include "drivers/serial.h" #include "io/serial.h" -#include "build_config.h" #include "printf.h" #ifdef REQUIRE_PRINTF_LONG_SUPPORT diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c index 4e3f8b4c2e..469d0b725a 100644 --- a/src/main/common/typeconversion.c +++ b/src/main/common/typeconversion.c @@ -17,7 +17,7 @@ #include #include #include -#include "build_config.h" +#include "build/build_config.h" #include "maths.h" #ifdef REQUIRE_PRINTF_LONG_SUPPORT diff --git a/src/main/config/config.c b/src/main/config/config.c index da0aaea2cf..1db98cec84 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -20,10 +20,9 @@ #include #include "platform.h" -#include "debug.h" -#include "build_config.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" #include "blackbox/blackbox_io.h" @@ -56,8 +55,8 @@ #include "io/serial.h" #include "io/gimbal.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "io/rc_curves.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" #include "io/osd.h" @@ -74,7 +73,8 @@ #include "flight/altitudehold.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index e227d7f8d4..a9323004fa 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -19,8 +19,8 @@ #include #include "platform.h" -#include "build_config.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" #include "common/maths.h" diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c index 80a270b9f1..43b75d585d 100644 --- a/src/main/drivers/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_lsm303dlhc.c @@ -22,7 +22,7 @@ #ifdef USE_ACC_LSM303DLHC -#include "debug.h" +#include "build/debug.h" #include "common/maths.h" #include "common/axis.h" diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 4284697217..b8d7d176ae 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -21,8 +21,9 @@ #include #include "platform.h" -#include "build_config.h" -#include "debug.h" + +#include "build/build_config.h" +#include "build/debug.h" #include "common/maths.h" diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 78e8b8e801..5d2198524b 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -20,8 +20,9 @@ #include #include "platform.h" -#include "build_config.h" -#include "debug.h" + +#include "build/build_config.h" +#include "build/debug.h" #include "common/maths.h" diff --git a/src/main/drivers/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro_spi_mpu9250.c index 0406e6023c..c1c1169ae9 100644 --- a/src/main/drivers/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro_spi_mpu9250.c @@ -27,7 +27,8 @@ #include #include "platform.h" -#include "light_led.h" + +#include "build/debug.h" #include "common/axis.h" #include "common/maths.h" @@ -38,7 +39,7 @@ #include "exti.h" #include "bus_spi.h" #include "gyro_sync.h" -#include "debug.h" +#include "light_led.h" #include "sensor.h" #include "accgyro.h" diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 6bba109396..16b70f6c64 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -19,8 +19,9 @@ #include #include "platform.h" -#include "build_config.h" -#include "debug.h" + +#include "build/build_config.h" +#include "build/debug.h" #include "system.h" diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 1ac85621d7..19d7edf6f9 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -23,7 +23,7 @@ #ifdef USE_ADC -#include "build_config.h" +#include "build/build_config.h" #include "system.h" #include "sensor.h" diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c index 9b35646b6b..9f022f6fe4 100644 --- a/src/main/drivers/barometer_bmp085.c +++ b/src/main/drivers/barometer_bmp085.c @@ -20,7 +20,7 @@ #include -#include "build_config.h" +#include "build/build_config.h" #include "barometer.h" diff --git a/src/main/drivers/barometer_bmp280.c b/src/main/drivers/barometer_bmp280.c index 6d42458486..bebf45ac3b 100644 --- a/src/main/drivers/barometer_bmp280.c +++ b/src/main/drivers/barometer_bmp280.c @@ -20,7 +20,7 @@ #include -#include "build_config.h" +#include "build/build_config.h" #include "barometer.h" diff --git a/src/main/drivers/barometer_ms5611.c b/src/main/drivers/barometer_ms5611.c index 468f86046f..4864324de9 100644 --- a/src/main/drivers/barometer_ms5611.c +++ b/src/main/drivers/barometer_ms5611.c @@ -20,14 +20,14 @@ #include +#include "build/build_config.h" + #include "barometer.h" #include "gpio.h" #include "system.h" #include "bus_i2c.h" -#include "build_config.h" - // MS5611, Standard address 0x77 #define MS5611_ADDR 0x77 diff --git a/src/main/drivers/bus_i2c_soft.c b/src/main/drivers/bus_i2c_soft.c index 42ea8cadc9..540a139427 100644 --- a/src/main/drivers/bus_i2c_soft.c +++ b/src/main/drivers/bus_i2c_soft.c @@ -20,7 +20,8 @@ #include -#include "build_config.h" +#include "build/build_config.h" + #include "bus_i2c.h" #include "io.h" diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c index d5b9a59dec..d8e8fa6d52 100644 --- a/src/main/drivers/compass_ak8963.c +++ b/src/main/drivers/compass_ak8963.c @@ -20,11 +20,10 @@ #include -#include "build_config.h" - #include "platform.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" #include "common/axis.h" #include "common/maths.h" diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c index 0e4d3677c5..df6cfd4f10 100644 --- a/src/main/drivers/compass_ak8975.c +++ b/src/main/drivers/compass_ak8975.c @@ -24,7 +24,7 @@ #ifdef USE_MAG_AK8975 -#include "build_config.h" +#include "build/build_config.h" #include "common/axis.h" #include "common/maths.h" diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index 10fa9b3c6d..6d91614540 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -24,7 +24,7 @@ #ifdef USE_MAG_HMC5883 -#include "debug.h" +#include "build/debug.h" #include "common/axis.h" #include "common/maths.h" diff --git a/src/main/drivers/gpio_stm32f4xx.c b/src/main/drivers/gpio_stm32f4xx.c index 0129bf68f4..7c5281191e 100644 --- a/src/main/drivers/gpio_stm32f4xx.c +++ b/src/main/drivers/gpio_stm32f4xx.c @@ -20,7 +20,7 @@ #include "platform.h" -#include "build_config.h" +#include "build/build_config.h" #include "gpio.h" diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index d9c29af370..3c1b5d4478 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -32,7 +32,7 @@ #ifdef LED_STRIP -#include "build_config.h" +#include "build/build_config.h" #include "common/color.h" #include "common/colorconversion.h" diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 7ddc2a058b..bea03d8c07 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -22,8 +22,6 @@ #ifdef USE_MAX7456 -#include "version.h" - #include "common/printf.h" #include "bus_spi.h" diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 60af85e97b..cb40782c9a 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -22,8 +22,8 @@ #ifndef SKIP_RX_PWM_PPM -#include "build_config.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" #include "common/utils.h" diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index d8fafdf78e..a5e41c8ec7 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -22,10 +22,10 @@ #if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2) -#include "build_config.h" +#include "build/build_config.h" +#include "build/atomic.h" #include "common/utils.h" -#include "common/atomic.h" #include "nvic.h" #include "system.h" diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 4663701179..68a43289e2 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -25,7 +25,7 @@ #include "platform.h" -#include "build_config.h" +#include "build/build_config.h" #include "common/utils.h" #include "gpio.h" diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 50c6055ddd..81d71a7ed3 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -20,7 +20,8 @@ #include "platform.h" -#include "build_config.h" +#include "build/build_config.h" + #include "common/utils.h" #include "io.h" diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index cbfcf80a1a..dd0e9b6847 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/src/main/drivers/sonar_hcsr04.c @@ -19,7 +19,8 @@ #include #include "platform.h" -#include "build_config.h" + +#include "build/build_config.h" #include "system.h" #include "nvic.h" diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 5051588c99..da2889ac97 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -20,8 +20,10 @@ #include #include "platform.h" + +#include "build/atomic.h" + #include "common/utils.h" -#include "common/atomic.h" #include "nvic.h" diff --git a/src/main/mw.c b/src/main/fc/mw.c similarity index 99% rename from src/main/mw.c rename to src/main/fc/mw.c index b2c569e98b..98ad21ba70 100644 --- a/src/main/mw.c +++ b/src/main/fc/mw.c @@ -21,7 +21,8 @@ #include #include "platform.h" -#include "debug.h" + +#include "build/debug.h" #include "common/maths.h" #include "common/axis.h" @@ -53,8 +54,8 @@ #include "io/beeper.h" #include "io/display.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "io/rc_curves.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" #include "io/gimbal.h" #include "io/gps.h" #include "io/ledstrip.h" @@ -82,7 +83,7 @@ #include "flight/gtune.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" diff --git a/src/main/mw.h b/src/main/fc/mw.h similarity index 100% rename from src/main/mw.h rename to src/main/fc/mw.h diff --git a/src/main/io/rc_controls.c b/src/main/fc/rc_controls.c similarity index 99% rename from src/main/io/rc_controls.c rename to src/main/fc/rc_controls.c index c3ba253b87..71307b5e50 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -23,13 +23,14 @@ #include "platform.h" -#include "build_config.h" +#include "build/build_config.h" #include "common/axis.h" #include "common/maths.h" #include "config/config.h" -#include "config/runtime_config.h" + +#include "fc/runtime_config.h" #include "drivers/system.h" #include "drivers/sensor.h" @@ -46,8 +47,8 @@ #include "io/gps.h" #include "io/beeper.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "io/rc_curves.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" #include "io/vtx.h" #include "io/display.h" @@ -58,7 +59,7 @@ #include "blackbox/blackbox.h" -#include "mw.h" +#include "fc/mw.h" static escAndServoConfig_t *escAndServoConfig; static pidProfile_t *pidProfile; diff --git a/src/main/io/rc_controls.h b/src/main/fc/rc_controls.h similarity index 100% rename from src/main/io/rc_controls.h rename to src/main/fc/rc_controls.h diff --git a/src/main/io/rc_curves.c b/src/main/fc/rc_curves.c similarity index 96% rename from src/main/io/rc_curves.c rename to src/main/fc/rc_curves.c index f96426bfdf..534676e6d8 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -18,11 +18,14 @@ #include #include +#include "platform.h" + #include "config/config.h" #include "io/escservo.h" -#include "io/rc_curves.h" -#include "io/rc_controls.h" + +#include "fc/rc_curves.h" +#include "fc/rc_controls.h" #include "rx/rx.h" diff --git a/src/main/io/rc_curves.h b/src/main/fc/rc_curves.h similarity index 100% rename from src/main/io/rc_curves.h rename to src/main/fc/rc_curves.h diff --git a/src/main/config/runtime_config.c b/src/main/fc/runtime_config.c similarity index 97% rename from src/main/config/runtime_config.c rename to src/main/fc/runtime_config.c index 90304c975c..889f612e15 100644 --- a/src/main/config/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -18,7 +18,9 @@ #include #include -#include "config/runtime_config.h" +#include "platform.h" + +#include "fc/runtime_config.h" #include "io/beeper.h" uint8_t armingFlags = 0; diff --git a/src/main/config/runtime_config.h b/src/main/fc/runtime_config.h similarity index 100% rename from src/main/config/runtime_config.h rename to src/main/fc/runtime_config.h diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 1d67ba3d9b..ee2a2322a4 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -23,7 +23,7 @@ #include "platform.h" -#include "debug.h" +#include "build/debug.h" #include "common/maths.h" #include "common/axis.h" @@ -39,14 +39,15 @@ #include "rx/rx.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/escservo.h" #include "flight/mixer.h" #include "flight/pid.h" #include "flight/imu.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + int32_t setVelocity = 0; uint8_t velocityControl = 0; diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index a553cb58f9..86c8aef763 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -20,7 +20,7 @@ #include -#include "debug.h" +#include "build/debug.h" #include "common/axis.h" @@ -28,8 +28,9 @@ #include "drivers/system.h" #include "io/beeper.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "config/runtime_config.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "flight/failsafe.h" diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c index afef499d59..17c5027541 100644 --- a/src/main/flight/gtune.c +++ b/src/main/flight/gtune.c @@ -41,9 +41,10 @@ #include "config/config.h" #include "blackbox/blackbox.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" + +#include "fc/runtime_config.h" -#include "config/runtime_config.h" extern uint16_t cycleTime; extern uint8_t motorCount; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 8945e765e6..f07ec47823 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -23,9 +23,10 @@ #include "common/maths.h" -#include "build_config.h" #include "platform.h" -#include "debug.h" + +#include "build/build_config.h" +#include "build/debug.h" #include "common/axis.h" @@ -47,7 +48,8 @@ #include "io/gps.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + // the limit (in degrees/second) beyond which we stop integrating // omega_I. At larger spin rates the DCM PI controller can get 'dizzy' diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 303f6960bc..0ed25a2eac 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -22,9 +22,9 @@ #include #include "platform.h" -#include "debug.h" -#include "build_config.h" +#include "build/build_config.h" +#include "build/debug.h" #include "common/axis.h" #include "common/maths.h" @@ -41,7 +41,7 @@ #include "io/gimbal.h" #include "io/escservo.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "sensors/sensors.h" #include "sensors/acceleration.h" @@ -52,7 +52,8 @@ #include "flight/pid.h" #include "flight/imu.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" uint8_t motorCount; diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 057c867a67..6b77ca61fe 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -23,7 +23,8 @@ #include #include "platform.h" -#include "debug.h" + +#include "build/debug.h" #include "common/maths.h" #include "common/axis.h" @@ -43,7 +44,7 @@ #include "io/beeper.h" #include "io/serial.h" #include "io/gps.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "flight/pid.h" #include "flight/navigation.h" @@ -54,7 +55,8 @@ #include "config/config.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + extern int16_t magHold; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index be1bcf2034..05969e4f1e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -21,8 +21,8 @@ #include -#include "build_config.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" #include "common/axis.h" #include "common/maths.h" @@ -37,7 +37,7 @@ #include "rx/rx.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/gps.h" #include "flight/pid.h" @@ -45,7 +45,8 @@ #include "flight/navigation.h" #include "flight/gtune.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + uint32_t targetPidLooptime; diff --git a/src/main/flight/pid_betaflight.c b/src/main/flight/pid_betaflight.c index 39616552e0..3ddb38e871 100644 --- a/src/main/flight/pid_betaflight.c +++ b/src/main/flight/pid_betaflight.c @@ -23,8 +23,8 @@ #ifndef SKIP_PID_FLOAT -#include "build_config.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" #include "common/axis.h" #include "common/maths.h" @@ -39,16 +39,16 @@ #include "rx/rx.h" -#include "io/rc_controls.h" #include "io/gps.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + #include "flight/pid.h" #include "flight/imu.h" #include "flight/navigation.h" #include "flight/gtune.h" -#include "config/runtime_config.h" - extern float rcInput[3]; extern float setpointRate[3]; diff --git a/src/main/flight/pid_legacy.c b/src/main/flight/pid_legacy.c index 0424f5461f..24c3aac56d 100644 --- a/src/main/flight/pid_legacy.c +++ b/src/main/flight/pid_legacy.c @@ -21,8 +21,8 @@ #include -#include "build_config.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" #include "common/axis.h" #include "common/maths.h" @@ -38,15 +38,16 @@ #include "rx/rx.h" -#include "io/rc_controls.h" #include "io/gps.h" +#include "fc/runtime_config.h" +#include "fc/rc_controls.h" + #include "flight/pid.h" #include "flight/imu.h" #include "flight/navigation.h" #include "flight/gtune.h" -#include "config/runtime_config.h" extern uint8_t motorCount; extern uint8_t PIDweight[3]; diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index cd616044d2..e015172be7 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -20,7 +20,8 @@ #include "stdlib.h" #include -#include "build_config.h" + +#include "build/build_config.h" #include "drivers/gpio.h" #include "drivers/sound_beeper.h" @@ -28,7 +29,7 @@ #include "sensors/battery.h" #include "sensors/sensors.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/statusindicator.h" #include "io/vtx.h" @@ -37,7 +38,8 @@ #include "io/gps.h" #endif -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "io/beeper.h" diff --git a/src/main/io/display.c b/src/main/io/display.c index a22ca88023..ce18ee3704 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -23,10 +23,10 @@ #ifdef DISPLAY -#include "version.h" -#include "debug.h" +#include "build/version.h" +#include "build/debug.h" -#include "build_config.h" +#include "build/build_config.h" #include "drivers/serial.h" #include "drivers/system.h" @@ -46,7 +46,7 @@ #include "sensors/acceleration.h" #include "sensors/gyro.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "flight/pid.h" #include "flight/imu.h" @@ -58,7 +58,8 @@ #endif #include "config/config.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config_profile.h" #include "io/display.h" diff --git a/src/main/io/gps.c b/src/main/io/gps.c index af963adcdc..11a9f0873e 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -26,8 +26,8 @@ #ifdef GPS -#include "build_config.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" #include "common/maths.h" #include "common/axis.h" @@ -50,7 +50,7 @@ #include "flight/navigation.h" #include "config/config.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" #define LOG_ERROR '?' diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 573d562374..967dd0dd57 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -23,13 +23,13 @@ #include -#include - #ifdef LED_STRIP -#include -#include -#include +#include "build/build_config.h" + +#include "common/color.h" +#include "common/maths.h" +#include "common/typeconversion.h" #include "drivers/light_ws2811strip.h" #include "drivers/system.h" @@ -44,7 +44,7 @@ #include "common/axis.h" #include "common/utils.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "sensors/battery.h" #include "sensors/sensors.h" @@ -72,7 +72,8 @@ #include "telemetry/telemetry.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index abd862dd13..d956df4832 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -23,12 +23,18 @@ #include #include "platform.h" -#include "version.h" + +#ifdef OSD + +#include "build/atomic.h" +#include "build/build_config.h" +#include "build/debug.h" +#include "build/version.h" + #include "scheduler/scheduler.h" #include "common/axis.h" #include "common/color.h" -#include "common/atomic.h" #include "common/maths.h" #include "common/typeconversion.h" @@ -64,7 +70,7 @@ #include "io/flashfs.h" #include "io/gps.h" #include "io/escservo.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/gimbal.h" #include "io/ledstrip.h" #include "io/display.h" @@ -91,7 +97,8 @@ #include "flight/failsafe.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" @@ -100,11 +107,6 @@ #include "hardware_revision.h" #endif -#include "build_config.h" -#include "debug.h" - -#ifdef OSD - #include "drivers/max7456.h" #include "drivers/max7456_symbols.h" diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 2e8e2eab44..fdbff97630 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -21,7 +21,7 @@ #include "platform.h" -#include "build_config.h" +#include "build/build_config.h" #include "common/utils.h" diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 463ffb72da..958a602c39 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -24,10 +24,10 @@ #include #include "platform.h" -#include "version.h" -#include "debug.h" -#include "build_config.h" +#include "build/version.h" +#include "build/debug.h" +#include "build/build_config.h" #include "common/axis.h" #include "common/maths.h" @@ -54,7 +54,7 @@ #include "io/escservo.h" #include "io/gps.h" #include "io/gimbal.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/serial.h" #include "io/ledstrip.h" #include "io/flashfs.h" @@ -83,7 +83,8 @@ #include "telemetry/telemetry.h" #include "telemetry/frsky.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 031d0ddf6b..c625ee847d 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -21,10 +21,12 @@ #include #include -#include "build_config.h" -#include "debug.h" #include "platform.h" +#include "build/build_config.h" +#include "build/debug.h" +#include "build/version.h" + #include "common/axis.h" #include "common/color.h" #include "common/maths.h" @@ -50,7 +52,7 @@ #include "io/beeper.h" #include "io/escservo.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/gps.h" #include "io/gimbal.h" #include "io/serial.h" @@ -84,14 +86,14 @@ #include "blackbox/blackbox.h" -#include "mw.h" +#include "fc/mw.h" + +#include "fc/runtime_config.h" -#include "config/runtime_config.h" #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" -#include "version.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif diff --git a/src/main/io/transponder_ir.c b/src/main/io/transponder_ir.c index fa4f66118a..633e56e068 100644 --- a/src/main/io/transponder_ir.c +++ b/src/main/io/transponder_ir.c @@ -23,7 +23,7 @@ #include -#include +#include #include "drivers/transponder_ir.h" #include "drivers/system.h" diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index 15a3de3347..6c6f6ab94e 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -50,8 +50,8 @@ #include "io/serial.h" #include "io/gimbal.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "io/rc_curves.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" #include "io/vtx.h" @@ -70,7 +70,8 @@ #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + static uint8_t locked = 0; diff --git a/src/main/main.c b/src/main/main.c index 436f5b5798..c642eef763 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -70,7 +70,7 @@ #include "io/flashfs.h" #include "io/gps.h" #include "io/escservo.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/gimbal.h" #include "io/ledstrip.h" #include "io/display.h" @@ -102,7 +102,8 @@ #include "flight/failsafe.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" @@ -111,8 +112,8 @@ #include "hardware_revision.h" #endif -#include "build_config.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" extern uint8_t motorControlEnable; diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index 507df1eff3..562999ccf1 100755 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -16,7 +16,7 @@ * * * Driver for IBUS (Flysky) receiver - * - initial implementation for MultiWii by Cesco/Plüschi + * - initial implementation for MultiWii by Cesco/Pl¸schi * - implementation for BaseFlight by Andreas (fiendie) Tacke * - ported to CleanFlight by Konstantin (digitalentity) Sharlaimov */ @@ -27,7 +27,7 @@ #include -#include "build_config.h" +#include "build/build_config.h" #include "drivers/system.h" diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index d4050b8c84..2c8d2a72ab 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -39,8 +39,8 @@ #include "platform.h" -#include "build_config.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" #include "common/utils.h" diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c index 141d37a773..b850b9cab6 100644 --- a/src/main/rx/msp.c +++ b/src/main/rx/msp.c @@ -22,7 +22,7 @@ #ifndef SKIP_RX_MSP -#include "build_config.h" +#include "build/build_config.h" #include "drivers/system.h" diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c index 517a361e6b..a85fd94759 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -25,7 +25,7 @@ #ifndef SKIP_RX_PWM_PPM -#include "build_config.h" +#include "build/build_config.h" #include "drivers/gpio.h" #include "drivers/timer.h" diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index bcba5bb86e..d5629bfae3 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -22,9 +22,9 @@ #include #include "platform.h" -#include "build_config.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" #include "common/maths.h" @@ -32,14 +32,16 @@ #include "drivers/serial.h" #include "drivers/adc.h" +#include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" #include "drivers/system.h" +#include "fc/rc_controls.h" + #include "flight/failsafe.h" #include "io/serial.h" -#include "io/rc_controls.h" #include "rx/rx.h" #include "rx/pwm.h" diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 8613a28fdd..742f783406 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -21,7 +21,7 @@ #include "platform.h" -#include "build_config.h" +#include "build/build_config.h" #include "drivers/system.h" diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index fb211bfcf7..16fc166e71 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -20,7 +20,8 @@ #include #include "platform.h" -#include "debug.h" + +#include "build/debug.h" #include "drivers/io.h" #include "drivers/io_impl.h" diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 8391b8fa0d..70457d1110 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -21,7 +21,7 @@ #include "platform.h" -#include "build_config.h" +#include "build/build_config.h" #include "drivers/system.h" diff --git a/src/main/rx/sumh.c b/src/main/rx/sumh.c index 87a49fbb72..3b608a5d39 100644 --- a/src/main/rx/sumh.c +++ b/src/main/rx/sumh.c @@ -27,7 +27,7 @@ #include "platform.h" -#include "build_config.h" +#include "build/build_config.h" #include "drivers/system.h" diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index cff97d77e4..aa374072b9 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -23,8 +23,8 @@ #include "platform.h" -#include "debug.h" -#include "build_config.h" +#include "build/build_config.h" +#include "build/debug.h" #include "scheduler/scheduler.h" #include "scheduler/scheduler_tasks.h" diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index d8cc423c1a..c8600d4307 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -20,7 +20,8 @@ #include #include "platform.h" -#include "debug.h" + +#include "build/debug.h" #include "common/axis.h" #include "common/filter.h" diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 6d165cbc57..532f74ce79 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -19,7 +19,8 @@ #include "stdint.h" #include "platform.h" -#include "debug.h" + +#include "build/debug.h" #include "common/maths.h" #include "common/filter.h" @@ -27,12 +28,13 @@ #include "drivers/adc.h" #include "drivers/system.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "sensors/battery.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/beeper.h" #include "rx/rx.h" diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 6f6735d0fe..355508e95c 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -28,7 +28,8 @@ #include "drivers/light_led.h" #include "sensors/boardalignment.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "sensors/sensors.h" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a17b61df20..c6d4c121a7 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -20,7 +20,8 @@ #include #include "platform.h" -#include "debug.h" + +#include "build/debug.h" #include "common/axis.h" #include "common/maths.h" diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index c3465b3934..3b25e0b564 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -20,7 +20,7 @@ #include "platform.h" -#include "build_config.h" +#include "build/build_config.h" #include "common/axis.h" @@ -60,7 +60,7 @@ #include "drivers/sonar_hcsr04.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" #include "sensors/sensors.h" #include "sensors/acceleration.h" diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 89be0464ea..88e2f2c47d 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -20,16 +20,18 @@ #include #include "platform.h" -#include "build_config.h" #ifdef SONAR +#include "build/build_config.h" + #include "common/maths.h" #include "common/axis.h" #include "drivers/sonar_hcsr04.h" #include "drivers/io.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "sensors/sensors.h" diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index b16a16444a..59dcf40e72 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -19,7 +19,7 @@ #include -#include "build_config.h" +#include "build/build_config.h" #include "blackbox/blackbox_io.h" @@ -48,8 +48,8 @@ #include "io/serial.h" #include "io/gimbal.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "io/rc_curves.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" #include "io/ledstrip.h" #include "rx/rx.h" @@ -63,7 +63,8 @@ #include "flight/altitudehold.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 3aed5a3ceb..8fdd4b7e57 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -19,7 +19,7 @@ #include -#include "build_config.h" +#include "build/build_config.h" #include "blackbox/blackbox_io.h" @@ -51,8 +51,8 @@ #include "io/serial.h" #include "io/gimbal.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "io/rc_curves.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" #include "io/osd.h" @@ -69,7 +69,8 @@ #include "flight/altitudehold.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/target/ALIENFLIGHTF3/hardware_revision.c b/src/main/target/ALIENFLIGHTF3/hardware_revision.c index a8f1ebdb7e..c535c7d9a2 100644 --- a/src/main/target/ALIENFLIGHTF3/hardware_revision.c +++ b/src/main/target/ALIENFLIGHTF3/hardware_revision.c @@ -20,7 +20,8 @@ #include #include "platform.h" -#include "build_config.h" + +#include "build/build_config.h" #include "drivers/system.h" #include "drivers/io.h" diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 5264a16735..3b23a96c69 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -19,7 +19,7 @@ #include -#include "build_config.h" +#include "build/build_config.h" #include "blackbox/blackbox_io.h" @@ -51,8 +51,8 @@ #include "io/serial.h" #include "io/gimbal.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "io/rc_curves.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" #include "io/osd.h" @@ -69,7 +69,8 @@ #include "flight/altitudehold.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c index 71007bb806..cd5d62e9a9 100644 --- a/src/main/target/BLUEJAYF4/config.c +++ b/src/main/target/BLUEJAYF4/config.c @@ -19,7 +19,7 @@ #include -#include "build_config.h" +#include "build/build_config.h" #include "blackbox/blackbox_io.h" @@ -51,8 +51,8 @@ #include "io/serial.h" #include "io/gimbal.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "io/rc_curves.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" #include "io/osd.h" @@ -69,7 +69,8 @@ #include "flight/altitudehold.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/target/BLUEJAYF4/hardware_revision.c b/src/main/target/BLUEJAYF4/hardware_revision.c index 3162e9f487..aa5f311326 100644 --- a/src/main/target/BLUEJAYF4/hardware_revision.c +++ b/src/main/target/BLUEJAYF4/hardware_revision.c @@ -20,7 +20,8 @@ #include #include "platform.h" -#include "build_config.h" + +#include "build/build_config.h" #include "drivers/system.h" #include "drivers/bus_spi.h" diff --git a/src/main/target/CJMCU/hardware_revision.c b/src/main/target/CJMCU/hardware_revision.c index 558e19a760..eacb851ec7 100755 --- a/src/main/target/CJMCU/hardware_revision.c +++ b/src/main/target/CJMCU/hardware_revision.c @@ -21,7 +21,7 @@ #include "platform.h" -#include "build_config.h" +#include "build/build_config.h" #include "drivers/system.h" #include "drivers/bus_spi.h" diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c index 7d20ab1d90..04bc7ffe83 100644 --- a/src/main/target/COLIBRI/config.c +++ b/src/main/target/COLIBRI/config.c @@ -20,9 +20,9 @@ #include #include "platform.h" -#include "debug.h" -#include "build_config.h" +#include "build/build_config.h" +#include "build/debug.h" #include "blackbox/blackbox_io.h" @@ -54,8 +54,8 @@ #include "io/serial.h" #include "io/gimbal.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "io/rc_curves.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" #include "io/osd.h" @@ -72,7 +72,7 @@ #include "flight/altitudehold.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c index 655f19cfe6..725e9076d0 100644 --- a/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c +++ b/src/main/target/COLIBRI_RACE/bus_bst_stm32f30x.c @@ -10,7 +10,7 @@ #include -#include +#include "build/build_config.h" #include "drivers/nvic.h" #include "bus_bst.h" diff --git a/src/main/target/COLIBRI_RACE/config.c b/src/main/target/COLIBRI_RACE/config.c index bbd5a49c9c..544d08b2ab 100644 --- a/src/main/target/COLIBRI_RACE/config.c +++ b/src/main/target/COLIBRI_RACE/config.c @@ -19,7 +19,7 @@ #include -#include "build_config.h" +#include "build/build_config.h" #include "blackbox/blackbox_io.h" @@ -51,8 +51,8 @@ #include "io/serial.h" #include "io/gimbal.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "io/rc_curves.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" #include "io/osd.h" @@ -69,7 +69,8 @@ #include "flight/altitudehold.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index c2cf9377dc..7c706f7ff0 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -6,8 +6,9 @@ #include #include -#include "build_config.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" +#include "build/version.h" #include "platform.h" @@ -31,7 +32,7 @@ #include "rx/msp.h" #include "io/escservo.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/gps.h" #include "io/gimbal.h" #include "io/serial.h" @@ -57,14 +58,13 @@ #include "flight/navigation.h" #include "flight/altitudehold.h" -#include "mw.h" +#include "fc/mw.h" +#include "fc/runtime_config.h" -#include "config/runtime_config.h" #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" -#include "version.h" #ifdef NAZE #include "hardware_revision.h" #endif diff --git a/src/main/target/MOTOLAB/config.c b/src/main/target/MOTOLAB/config.c index 490236ce04..8afb1e3cf3 100755 --- a/src/main/target/MOTOLAB/config.c +++ b/src/main/target/MOTOLAB/config.c @@ -19,7 +19,7 @@ #include -#include "build_config.h" +#include "build/build_config.h" #include "blackbox/blackbox_io.h" @@ -51,8 +51,8 @@ #include "io/serial.h" #include "io/gimbal.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "io/rc_curves.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" #include "io/osd.h" @@ -69,7 +69,8 @@ #include "flight/altitudehold.h" #include "flight/navigation.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/target/NAZE/hardware_revision.c b/src/main/target/NAZE/hardware_revision.c index 3f87237dbb..1c55298f31 100755 --- a/src/main/target/NAZE/hardware_revision.c +++ b/src/main/target/NAZE/hardware_revision.c @@ -21,7 +21,7 @@ #include "platform.h" -#include "build_config.h" +#include "build/build_config.h" #include "drivers/system.h" #include "drivers/bus_spi.h" diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index d698b958a0..20552b01a4 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -46,7 +46,7 @@ #include "sensors/battery.h" #include "io/serial.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/gps.h" #include "rx/rx.h" @@ -56,7 +56,8 @@ #include "flight/imu.h" #include "flight/altitudehold.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" #include "telemetry/telemetry.h" diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index dcab855ae0..bee5162b11 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -60,8 +60,8 @@ #ifdef TELEMETRY -#include "build_config.h" -#include "debug.h" +#include "build/build_config.h" +#include "build/debug.h" #include "common/axis.h" @@ -70,7 +70,7 @@ #include "drivers/serial.h" #include "io/serial.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" #include "sensors/sensors.h" #include "sensors/battery.h" diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index a7b7556e10..99cb4b2503 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -35,7 +35,7 @@ #ifdef TELEMETRY -#include "build_config.h" +#include "build/build_config.h" #include "common/maths.h" #include "common/axis.h" @@ -57,7 +57,7 @@ #include "sensors/battery.h" #include "io/serial.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/gimbal.h" #include "io/gps.h" #include "io/ledstrip.h" @@ -78,7 +78,8 @@ #include "telemetry/ltm.h" #include "config/config.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #define TELEMETRY_LTM_INITIAL_PORT_MODE MODE_TX #define LTM_CYCLETIME 100 diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 8d167dd2ca..4396b0c0a6 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -32,7 +32,7 @@ #include "io/beeper.h" #include "io/escservo.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" #include "io/gps.h" #include "io/gimbal.h" #include "io/serial.h" @@ -58,7 +58,8 @@ #include "telemetry/telemetry.h" #include "telemetry/smartport.h" -#include "config/runtime_config.h" +#include "fc/runtime_config.h" + #include "config/config.h" enum diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 6373d166eb..c0058839d4 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -29,9 +29,10 @@ #include "io/serial.h" #include "rx/rx.h" -#include "io/rc_controls.h" +#include "fc/rc_controls.h" + +#include "fc/runtime_config.h" -#include "config/runtime_config.h" #include "config/config.h" #include "telemetry/telemetry.h" diff --git a/src/main/vcp/hw_config.c b/src/main/vcp/hw_config.c index d7552ea0fb..1f7f471eee 100644 --- a/src/main/vcp/hw_config.c +++ b/src/main/vcp/hw_config.c @@ -39,7 +39,7 @@ #include "drivers/system.h" #include "drivers/nvic.h" -#include "build_config.h" +#include "build/build_config.h" /* Private typedef -----------------------------------------------------------*/ From 4cd7375cc32653d539c907370bdc8ab1eb8cb9ec Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Sun, 7 Aug 2016 17:13:56 +0200 Subject: [PATCH 258/456] Fix compile warnigs when only USE_VCP is defined. --- src/main/io/serial.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 2e8e2eab44..3cc38a5ae7 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -270,7 +270,7 @@ serialPort_t *openSerialPort( portMode_t mode, portOptions_t options) { -#if (!defined(USE_VCP) && !defined(USE_UART1) && !defined(USE_UART2) && !defined(USE_UART3) && !defined(USE_UART4) && !defined(USE_UART5) && !defined(USE_UART6) && !defined(USE_SOFTSERIAL1) && !defined(USE_SOFTSERIAL2)) +#if (!defined(USE_UART1) && !defined(USE_UART2) && !defined(USE_UART3) && !defined(USE_UART4) && !defined(USE_UART5) && !defined(USE_UART6) && !defined(USE_SOFTSERIAL1) && !defined(USE_SOFTSERIAL2)) UNUSED(callback); UNUSED(baudRate); UNUSED(mode); From 4785bf5471fdb70b967d220b6a47e62407bbd7ac Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 8 Aug 2016 09:27:15 +1200 Subject: [PATCH 259/456] Removed unneeded define for 'ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS'. --- src/main/config/config.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/config/config.h b/src/main/config/config.h index cc239da923..e1307b0159 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -23,7 +23,6 @@ #define MAX_PROFILE_COUNT 3 #endif #define MAX_RATEPROFILES 3 -#define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 #define MAX_NAME_LENGTH 16 From 8e8e6d4d49f3ef83e6b7cea025432cdc385f8f2e Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 8 Aug 2016 09:44:47 +1200 Subject: [PATCH 260/456] Changed LEDSTRIP default config to all LEDs undefined. --- src/main/io/ledstrip.c | 159 ----------------------------------------- 1 file changed, 159 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 573d562374..2458260b6f 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -90,7 +90,6 @@ static bool ledStripEnabled = true; static void ledStripDisable(void); //#define USE_LED_ANIMATION -//#define USE_LED_RING_DEFAULT_CONFIG #define LED_STRIP_HZ(hz) ((int32_t)((1000 * 1000) / (hz))) #define LED_STRIP_MS(ms) ((int32_t)(1000 * (ms))) @@ -146,73 +145,6 @@ STATIC_UNIT_TESTED uint8_t lowestXValueForEast; STATIC_UNIT_TESTED ledCounts_t ledCounts; -// macro for initializer -#define LF(name) LED_FUNCTION_ ## name -#define LO(name) LED_FLAG_OVERLAY(LED_OVERLAY_ ## name) -#define LD(name) LED_FLAG_DIRECTION(LED_DIRECTION_ ## name) - -#ifdef USE_LED_RING_DEFAULT_CONFIG -static const ledConfig_t defaultLedStripConfig[] = { - DEFINE_LED( 2, 2, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 2, 1, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 2, 0, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 1, 0, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 0, 0, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 0, 1, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 0, 2, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 1, 2, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 1, 1, 3, 0, LF(THRUST_RING), 0, 0), -}; -#else -static const ledConfig_t defaultLedStripConfig[] = { - DEFINE_LED(15, 15, 0, LD(SOUTH) | LD(EAST), LF(ARM_STATE), LO(INDICATOR), 0), - - DEFINE_LED(15, 8, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0), - DEFINE_LED(15, 7, 0, LD(EAST) , LF(FLIGHT_MODE), LO(WARNING), 0), - - DEFINE_LED(15, 0, 0, LD(NORTH) | LD(EAST), LF(ARM_STATE) , LO(INDICATOR), 0), - - DEFINE_LED( 8, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0), - DEFINE_LED( 7, 0, 0, LD(NORTH) , LF(FLIGHT_MODE), 0, 0), - - DEFINE_LED( 0, 0, 0, LD(NORTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0), - - DEFINE_LED( 0, 7, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0), - DEFINE_LED( 0, 8, 0, LD(WEST) , LF(FLIGHT_MODE), LO(WARNING), 0), - - DEFINE_LED( 0, 15, 0, LD(SOUTH) | LD(WEST), LF(ARM_STATE) , LO(INDICATOR), 0), - - DEFINE_LED( 7, 15, 0, LD(SOUTH) , LF(FLIGHT_MODE), LO(WARNING), 0), - DEFINE_LED( 8, 15, 0, LD(SOUTH) , LF(FLIGHT_MODE), LO(WARNING), 0), - - DEFINE_LED( 7, 7, 0, LD(UP) , LF(FLIGHT_MODE), LO(WARNING), 0), - DEFINE_LED( 8, 7, 0, LD(UP) , LF(FLIGHT_MODE), LO(WARNING), 0), - DEFINE_LED( 7, 8, 0, LD(DOWN) , LF(FLIGHT_MODE), LO(WARNING), 0), - DEFINE_LED( 8, 8, 0, LD(DOWN) , LF(FLIGHT_MODE), LO(WARNING), 0), - - DEFINE_LED( 8, 9, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 9, 10, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED(10, 11, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED(10, 12, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 9, 13, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 8, 14, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 7, 14, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 6, 13, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 5, 12, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 5, 11, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 6, 10, 3, 0, LF(THRUST_RING), 0, 0), - DEFINE_LED( 7, 9, 3, 0, LF(THRUST_RING), 0, 0), - -}; -#endif - -#undef LD -#undef LF -#undef LO - static const modeColorIndexes_t defaultModeColors[] = { // NORTH EAST SOUTH WEST UP DOWN [LED_MODE_ORIENTATION] = {{ COLOR_WHITE, COLOR_DARK_VIOLET, COLOR_RED, COLOR_DEEP_PINK, COLOR_BLUE, COLOR_ORANGE }}, @@ -235,88 +167,8 @@ static const specialColorIndexes_t defaultSpecialColors[] = { }} }; - static int scaledThrottle; - - - -/* -#ifdef USE_LED_RING_DEFAULT_CONFIG -const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 2, 2), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 2, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 2, 0), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 0), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 0, 0), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 0, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 0, 2), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 2), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, -}; -#elif defined(USE_COLIBTI_RACE_LED_DEFAULT_CONFIG) -const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, -}; -#else -const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY(15, 15), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { CALCULATE_LED_XY(15, 8), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY(15, 7), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - - { CALCULATE_LED_XY(15, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { CALCULATE_LED_XY( 8, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 7, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - - { CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { CALCULATE_LED_XY( 0, 7), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 8), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - - { CALCULATE_LED_XY( 0, 15), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { CALCULATE_LED_XY( 7, 15), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 8, 15), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - - { CALCULATE_LED_XY( 7, 7), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 8, 7), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 7, 8), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 8, 8), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - - { CALCULATE_LED_XY( 8, 9), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 9, 10), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY(10, 11), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY(10, 12), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 9, 13), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 8, 14), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 7, 14), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 6, 13), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 5, 12), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 5, 11), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 6, 10), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 7, 9), 3, LED_FUNCTION_THRUST_RING}, - -}; -#endif -*/ - - - - static void updateLedRingCounts(void); STATIC_UNIT_TESTED void determineLedStripDimensions(void) @@ -504,7 +356,6 @@ void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize) sprintf(ledConfigBuffer, "%u,%u:%s:%s:%u", ledGetX(ledConfig), ledGetY(ledConfig), directions, baseFunctionOverlays, ledGetColor(ledConfig)); } - typedef enum { // the ordering is important, see below how NSEW is mapped to NE/SE/NW/SW QUADRANT_NORTH = 1 << 0, @@ -578,7 +429,6 @@ static hsvColor_t* getDirectionalModeColor(const int ledIndex, const modeColorIn return NULL; } - // map flight mode to led mode, in order of priority // flightMode == 0 is always active static const struct { @@ -663,7 +513,6 @@ typedef enum { WARNING_FAILSAFE, } warningFlags_e; - static void applyLedWarningLayer(bool updateNow, uint32_t *timer) { static uint8_t warningFlashCounter = 0; @@ -811,7 +660,6 @@ static void applyLedGpsLayer(bool updateNow, uint32_t *timer) #endif - #define INDICATOR_DEADBAND 25 static void applyLedIndicatorLayer(bool updateNow, uint32_t *timer) @@ -999,9 +847,7 @@ static void applyLedBlinkLayer(bool updateNow, uint32_t *timer) } } - #ifdef USE_LED_ANIMATION - static void applyLedAnimationLayer(bool updateNow, uint32_t *timer) { static uint8_t frameCounter = 0; @@ -1052,7 +898,6 @@ typedef enum { static uint32_t timerVal[timTimerCount]; - // function to apply layer. // function must replan self using timer pointer // when updateNow is true (timer triggered), state must be updated first, @@ -1060,7 +905,6 @@ static uint32_t timerVal[timTimerCount]; // may modify LED state. typedef void applyLayerFn_timed(bool updateNow, uint32_t *timer); - static applyLayerFn_timed* layerTable[] = { [timBlink] = &applyLedBlinkLayer, [timLarson] = &applyLarsonScannerLayer, @@ -1223,7 +1067,6 @@ void pgResetFn_specialColors(specialColorIndexes_t *instance) void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) { memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); - memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); reevaluateLedConfig(); } @@ -1247,8 +1090,6 @@ void applyDefaultSpecialColors(specialColorIndexes_t *specialColors) memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors)); } - - void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse) { ledConfigs = ledConfigsToUse; From 3548ea45087acbbbde99394099f35d3a7d26580d Mon Sep 17 00:00:00 2001 From: kc10kevin Date: Mon, 8 Aug 2016 01:44:54 -0500 Subject: [PATCH 261/456] FuryF3 I2C Fix --- src/main/target/FURYF3/target.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 1cc93b6f21..bc8e0d726e 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -123,8 +123,8 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_1) // SDA (PB9/AF4), SCL (PB8/AF4) -#define I2C1_SCL_PIN PB8 -#define I2C1_SDA_PIN PB9 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 #define BOARD_HAS_VOLTAGE_DIVIDER #define USE_ADC From f6cc56514a4fd784c96f5e2089c45f2475934154 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 8 Aug 2016 11:13:50 +0200 Subject: [PATCH 262/456] Corrected the usage of TARGET_FLAGS in main Makefile. --- Makefile | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index 281b169c63..777c04026d 100644 --- a/Makefile +++ b/Makefile @@ -179,7 +179,6 @@ LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_$(FLASH_SIZE)k.ld ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion DEVICE_FLAGS = -DSTM32F303xC -DSTM32F303 -TARGET_FLAGS = -D$(TARGET) # End F3 targets # # Start F4 targets @@ -273,7 +272,6 @@ $(error Unknown MCU for F4 target) endif DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) -TARGET_FLAGS = -D$(TARGET) # End F4 targets # # Start F1 targets @@ -313,7 +311,6 @@ endif LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f103_$(FLASH_SIZE)k.ld ARCH_FLAGS = -mthumb -mcpu=cortex-m3 -TARGET_FLAGS := -D$(TARGET) -pedantic $(TARGET_FLAGS) ifeq ($(DEVICE_FLAGS),) DEVICE_FLAGS = -DSTM32F10X_MD @@ -581,8 +578,10 @@ CFLAGS += $(ARCH_FLAGS) \ -Wall -Wextra -Wunsafe-loop-optimizations -Wdouble-promotion \ -ffunction-sections \ -fdata-sections \ + -pedantic \ $(DEVICE_FLAGS) \ -DUSE_STDPERIPH_DRIVER \ + -D$(TARGET) \ $(TARGET_FLAGS) \ -D'__FORKNAME__="$(FORKNAME)"' \ -D'__TARGET__="$(TARGET)"' \ From d14569d271c23c881b760e85cd2ea2486de7b26c Mon Sep 17 00:00:00 2001 From: NightHawk32 Date: Mon, 8 Aug 2016 07:13:53 -0400 Subject: [PATCH 263/456] fixing 3D deadband in MSP: inconsistency between reading and setting the parameter --- src/main/io/serial_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index a2f472f45d..f4646c3905 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1501,13 +1501,13 @@ static bool processInCommand(void) masterConfig.flight3DConfig.deadband3d_low = read16(); masterConfig.flight3DConfig.deadband3d_high = read16(); masterConfig.flight3DConfig.neutral3d = read16(); - masterConfig.flight3DConfig.deadband3d_throttle = read16(); break; case MSP_SET_RC_DEADBAND: masterConfig.rcControlsConfig.deadband = read8(); masterConfig.rcControlsConfig.yaw_deadband = read8(); masterConfig.rcControlsConfig.alt_hold_deadband = read8(); + masterConfig.flight3DConfig.deadband3d_throttle = read16(); break; case MSP_SET_RESET_CURR_PID: From 909fee420566f43468f26c56ce87d2d49bdeddeb Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Mon, 8 Aug 2016 14:26:11 +0200 Subject: [PATCH 264/456] Add Yaw RC Rate as adjustment function --- src/main/io/rc_controls.c | 10 ++++++++++ src/main/io/rc_controls.h | 1 + 2 files changed, 11 insertions(+) diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index c3ba253b87..697ccf2357 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -470,6 +470,11 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_ROLL_D, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} + }, + { + .adjustmentFunction = ADJUSTMENT_RC_RATE_YAW, + .mode = ADJUSTMENT_MODE_STEP, + .data = { .stepConfig = { .step = 1 }} } }; @@ -595,6 +600,11 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t pidProfile->D8[PIDYAW] = newValue; blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue); break; + case ADJUSTMENT_RC_RATE_YAW: + newValue = constrain((int)controlRateConfig->rcYawRate8 + delta, 0, 300); // FIXME magic numbers repeated in serial_cli.c + controlRateConfig->rcYawRate8 = newValue; + blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue); + break; default: break; }; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index a3622aacab..339dea94c6 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -195,6 +195,7 @@ typedef enum { ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, + ADJUSTMENT_RC_RATE_YAW, ADJUSTMENT_FUNCTION_COUNT, } adjustmentFunction_e; From cba6c508fadaa392ac9c7b6c4a263e3ac8d0e1a5 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 8 Aug 2016 14:47:45 +0200 Subject: [PATCH 265/456] First try with Travis on a few targets. --- .travis.sh | 2 +- .travis.yml | 69 ++++++++++++++++++++++++++++++++++++----------------- 2 files changed, 48 insertions(+), 23 deletions(-) diff --git a/.travis.sh b/.travis.sh index f848242d32..ee310af88b 100755 --- a/.travis.sh +++ b/.travis.sh @@ -4,7 +4,7 @@ REVISION=$(git rev-parse --short HEAD) BRANCH=$(git rev-parse --abbrev-ref HEAD) REVISION=$(git rev-parse --short HEAD) LAST_COMMIT_DATE=$(git log -1 --date=short --format="%cd") -TARGET_FILE=obj/cleanflight_${TARGET} +TARGET_FILE=obj/betaflight_${TARGET} TRAVIS_REPO_SLUG=${TRAVIS_REPO_SLUG:=$USER/undefined} BUILDNAME=${BUILDNAME:=travis} TRAVIS_BUILD_NUMBER=${TRAVIS_BUILD_NUMBER:=undefined} diff --git a/.travis.yml b/.travis.yml index 9efb7f246d..661522e0d1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,27 +1,52 @@ env: - - RUNTESTS=True +# - RUNTESTS=True - PUBLISHMETA=True - - PUBLISHDOCS=True +# - PUBLISHDOCS=True +# - TARGET=AIORACERF3 + - TARGET=AFROMINI - TARGET=CC3D - TARGET=CC3D_OPBL - - TARGET=CC3D_BP6 - - TARGET=CC3D_OPBL_BP6 - - TARGET=COLIBRI_RACE - - TARGET=LUX_RACE - - TARGET=CHEBUZZF3 +# - TARGET=ALIENFLIGHTF1 +# - TARGET=ALIENFLIGHTF3 +# - TARGET=ALIENFLIGHTF4 +# - TARGET=AIR32 +# - TARGET=BLUEJAYF4 +# - TARGET=CHEBUZZF3 - TARGET=CJMCU - - TARGET=EUSTM32F103RC - - TARGET=SPRACINGF3 + - TARGET=COLIBRI + - TARGET=COLIBRI_RACE + - TARGET=DOGE +# - TARGET=EUSTM32F103RC +# - TARGET=F4BY +# - TARGET=FURYF3 +# - TARGET=FURYF4 +# - TARGET=IRCFUSIONF3 +# - TARGET=KISSFC +# - TARGET=LUX_RACE +# - TARGET=MICROSCISKY +# - TARGET=MOTOLAB - TARGET=NAZE - - TARGET=NAZE32PRO - - TARGET=OLIMEXINO - - TARGET=RMDO - - TARGET=PORT103R - - TARGET=SPARKY - - TARGET=STM32F3DISCOVERY - - TARGET=ALIENFLIGHTF1 - - TARGET=ALIENFLIGHTF3 - +# - TARGET=OLIMEXINO +# - TARGET=OMNIBUS +# - TARGET=OMNIBUSF4 +# - TARGET=PIKOBLX +# - TARGET=PORT103R + - TARGET=REVO +# - TARGET=REVONANO +# - TARGET=REVO_OPBL +# - TARGET=RMDO +# - TARGET=SINGULARITY +# - TARGET=SIRINFPV +# - TARGET=SPARKY +# - TARGET=SPARKY2 +# - TARGET=SPARKY_OPBL + - TARGET=SPRACINGF3 +# - TARGET=SPRACINGF3EVO +# - TARGET=SPRACINGF3MINI +# - TARGET=STM32F3DISCOVERY +# - TARGET=VRRACE +# - TARGET=X_RACERSPI +# - TARGET=ZCOREF3 # use new docker environment sudo: false @@ -52,7 +77,7 @@ script: ./.travis.sh cache: apt -notifications: - irc: "chat.freenode.net#cleanflight" - use_notice: true - skip_join: true +#notifications: +# irc: "chat.freenode.net#cleanflight" +# use_notice: true +# skip_join: true From 98d634231e476245def36e9f6f9370aa43b43403 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 8 Aug 2016 15:19:47 +0200 Subject: [PATCH 266/456] Second TRavis trail with all 45 targets. --- .travis.yml | 72 ++++++++++++++++++++++++++--------------------------- 1 file changed, 36 insertions(+), 36 deletions(-) diff --git a/.travis.yml b/.travis.yml index 661522e0d1..ca73f8c14e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,52 +1,52 @@ env: # - RUNTESTS=True - - PUBLISHMETA=True +# - PUBLISHMETA=True # - PUBLISHDOCS=True -# - TARGET=AIORACERF3 - TARGET=AFROMINI + - TARGET=AIORACERF3 + - TARGET=AIR32 + - TARGET=ALIENFLIGHTF1 + - TARGET=ALIENFLIGHTF3 + - TARGET=ALIENFLIGHTF4 + - TARGET=BLUEJAYF4 - TARGET=CC3D - TARGET=CC3D_OPBL -# - TARGET=ALIENFLIGHTF1 -# - TARGET=ALIENFLIGHTF3 -# - TARGET=ALIENFLIGHTF4 -# - TARGET=AIR32 -# - TARGET=BLUEJAYF4 -# - TARGET=CHEBUZZF3 + - TARGET=CHEBUZZF3 - TARGET=CJMCU - TARGET=COLIBRI - TARGET=COLIBRI_RACE - TARGET=DOGE -# - TARGET=EUSTM32F103RC -# - TARGET=F4BY -# - TARGET=FURYF3 -# - TARGET=FURYF4 -# - TARGET=IRCFUSIONF3 -# - TARGET=KISSFC -# - TARGET=LUX_RACE -# - TARGET=MICROSCISKY -# - TARGET=MOTOLAB + - TARGET=EUSTM32F103RC + - TARGET=F4BY + - TARGET=FURYF3 + - TARGET=FURYF4 + - TARGET=IRCFUSIONF3 + - TARGET=KISSFC + - TARGET=LUX_RACE + - TARGET=MICROSCISKY + - TARGET=MOTOLAB - TARGET=NAZE -# - TARGET=OLIMEXINO -# - TARGET=OMNIBUS -# - TARGET=OMNIBUSF4 -# - TARGET=PIKOBLX -# - TARGET=PORT103R + - TARGET=OLIMEXINO + - TARGET=OMNIBUS + - TARGET=OMNIBUSF4 + - TARGET=PIKOBLX + - TARGET=PORT103R - TARGET=REVO -# - TARGET=REVONANO -# - TARGET=REVO_OPBL -# - TARGET=RMDO -# - TARGET=SINGULARITY -# - TARGET=SIRINFPV -# - TARGET=SPARKY -# - TARGET=SPARKY2 -# - TARGET=SPARKY_OPBL + - TARGET=REVONANO + - TARGET=REVO_OPBL + - TARGET=RMDO + - TARGET=SINGULARITY + - TARGET=SIRINFPV + - TARGET=SPARKY + - TARGET=SPARKY2 + - TARGET=SPARKY_OPBL - TARGET=SPRACINGF3 -# - TARGET=SPRACINGF3EVO -# - TARGET=SPRACINGF3MINI -# - TARGET=STM32F3DISCOVERY -# - TARGET=VRRACE -# - TARGET=X_RACERSPI -# - TARGET=ZCOREF3 + - TARGET=SPRACINGF3EVO + - TARGET=SPRACINGF3MINI + - TARGET=STM32F3DISCOVERY + - TARGET=VRRACE + - TARGET=X_RACERSPI + - TARGET=ZCOREF3 # use new docker environment sudo: false From 12a7c13ed0d33bb833697b491c5e3dabb116b6d7 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 8 Aug 2016 16:49:58 +0200 Subject: [PATCH 267/456] Travis updates for development branch. --- .travis.sh | 2 +- .travis.yml | 75 +++++++++++++++++++++++++++++++++++------------------ 2 files changed, 51 insertions(+), 26 deletions(-) diff --git a/.travis.sh b/.travis.sh index f848242d32..ee310af88b 100755 --- a/.travis.sh +++ b/.travis.sh @@ -4,7 +4,7 @@ REVISION=$(git rev-parse --short HEAD) BRANCH=$(git rev-parse --abbrev-ref HEAD) REVISION=$(git rev-parse --short HEAD) LAST_COMMIT_DATE=$(git log -1 --date=short --format="%cd") -TARGET_FILE=obj/cleanflight_${TARGET} +TARGET_FILE=obj/betaflight_${TARGET} TRAVIS_REPO_SLUG=${TRAVIS_REPO_SLUG:=$USER/undefined} BUILDNAME=${BUILDNAME:=travis} TRAVIS_BUILD_NUMBER=${TRAVIS_BUILD_NUMBER:=undefined} diff --git a/.travis.yml b/.travis.yml index 9efb7f246d..ca73f8c14e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,27 +1,52 @@ env: - - RUNTESTS=True - - PUBLISHMETA=True - - PUBLISHDOCS=True - - TARGET=CC3D - - TARGET=CC3D_OPBL - - TARGET=CC3D_BP6 - - TARGET=CC3D_OPBL_BP6 - - TARGET=COLIBRI_RACE - - TARGET=LUX_RACE - - TARGET=CHEBUZZF3 - - TARGET=CJMCU - - TARGET=EUSTM32F103RC - - TARGET=SPRACINGF3 - - TARGET=NAZE - - TARGET=NAZE32PRO - - TARGET=OLIMEXINO - - TARGET=RMDO - - TARGET=PORT103R - - TARGET=SPARKY - - TARGET=STM32F3DISCOVERY +# - RUNTESTS=True +# - PUBLISHMETA=True +# - PUBLISHDOCS=True + - TARGET=AFROMINI + - TARGET=AIORACERF3 + - TARGET=AIR32 - TARGET=ALIENFLIGHTF1 - TARGET=ALIENFLIGHTF3 - + - TARGET=ALIENFLIGHTF4 + - TARGET=BLUEJAYF4 + - TARGET=CC3D + - TARGET=CC3D_OPBL + - TARGET=CHEBUZZF3 + - TARGET=CJMCU + - TARGET=COLIBRI + - TARGET=COLIBRI_RACE + - TARGET=DOGE + - TARGET=EUSTM32F103RC + - TARGET=F4BY + - TARGET=FURYF3 + - TARGET=FURYF4 + - TARGET=IRCFUSIONF3 + - TARGET=KISSFC + - TARGET=LUX_RACE + - TARGET=MICROSCISKY + - TARGET=MOTOLAB + - TARGET=NAZE + - TARGET=OLIMEXINO + - TARGET=OMNIBUS + - TARGET=OMNIBUSF4 + - TARGET=PIKOBLX + - TARGET=PORT103R + - TARGET=REVO + - TARGET=REVONANO + - TARGET=REVO_OPBL + - TARGET=RMDO + - TARGET=SINGULARITY + - TARGET=SIRINFPV + - TARGET=SPARKY + - TARGET=SPARKY2 + - TARGET=SPARKY_OPBL + - TARGET=SPRACINGF3 + - TARGET=SPRACINGF3EVO + - TARGET=SPRACINGF3MINI + - TARGET=STM32F3DISCOVERY + - TARGET=VRRACE + - TARGET=X_RACERSPI + - TARGET=ZCOREF3 # use new docker environment sudo: false @@ -52,7 +77,7 @@ script: ./.travis.sh cache: apt -notifications: - irc: "chat.freenode.net#cleanflight" - use_notice: true - skip_join: true +#notifications: +# irc: "chat.freenode.net#cleanflight" +# use_notice: true +# skip_join: true From 9c1ec6aec9971701d7510b152cf8442eafacf4f7 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 8 Aug 2016 17:31:22 +0200 Subject: [PATCH 268/456] Doc updates with betaflight Travis links.s --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 0253334eff..cb5d2b55a9 100644 --- a/README.md +++ b/README.md @@ -110,11 +110,11 @@ Please refer to the development section in the `docs/development` folder. TravisCI is used to run automatic builds -https://travis-ci.org/cleanflight/cleanflight +https://travis-ci.org/betaflight/betaflight -[![Build Status](https://travis-ci.org/cleanflight/cleanflight.svg?branch=master)](https://travis-ci.org/cleanflight/cleanflight) +[![Build Status](https://travis-ci.org/betaflight/betaflight.svg?branch=master)](https://travis-ci.org/betaflight/betaflight) -## Cleanflight Releases -https://github.com/cleanflight/cleanflight/releases +## Betaflight Releases +https://github.com/betaflight/betaflight/releases From e76350a2e342bf09235e05020baca9d8806c2be1 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 8 Aug 2016 19:05:33 +0200 Subject: [PATCH 269/456] Cleaned up some more text and links to refer to betaflight. Some cleanflight remainders still there.... --- README.md | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index cb5d2b55a9..9b88d1fbe6 100644 --- a/README.md +++ b/README.md @@ -61,11 +61,9 @@ If what you need is not covered then refer to the baseflight documentation. If y ## IRC Support and Developers Channel -There's a dedicated IRC channel here: +There's a dedicated IRCgitter chat channel here: -irc://irc.freenode.net/#cleanflight - -If you are using windows and don't have an IRC client installed then take a look at HydraIRC - here: http://hydrairc.com/ +https://gitter.im/betaflight/betaflight Etiquette: Don't ask to ask and please wait around long enough for a reply - sometimes people are out flying, asleep or at work and can't answer immediately. @@ -79,13 +77,13 @@ Please subscribe and '+1' the videos if you find them useful. ## Configuration Tool -To configure Cleanflight you should use the Cleanflight-configurator GUI tool (Windows/OSX/Linux) that can be found here: +To configure Betaflight you should use the Betaflight-configurator GUI tool (Windows/OSX/Linux) that can be found here: -https://chrome.google.com/webstore/detail/cleanflight-configurator/enacoimjcgeinfnnnpajinjgmkahmfgb +https://chrome.google.com/webstore/detail/betaflight-configurator/kdaghagfopacdngbohiknlhcocjccjao The source for it is here: -https://github.com/cleanflight/cleanflight-configurator +https://github.com/betaflight/betaflight-configurator ## Contributing @@ -97,10 +95,10 @@ Contributions are welcome and encouraged. You can contribute in many ways: * New features. * Telling us your ideas and suggestions. -The best place to start is the IRC channel on freenode (see above), drop in, say hi. Next place is the github issue tracker: +The best place to start is the IRC channel on gitter (see above), drop in, say hi. Next place is the github issue tracker: -https://github.com/cleanflight/cleanflight/issues -https://github.com/cleanflight/cleanflight-configurator/issues +https://github.com/betaflight/betaflight/issues +https://github.com/betaflight/betaflight-configurator/issues Before creating new issues please check to see if there is an existing one, search first otherwise you waste peoples time when they could be coding instead! From 6a02cde5be09a11228621536b5618665a6eb9d28 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 8 Aug 2016 19:20:17 +0200 Subject: [PATCH 270/456] Fix Ident --- src/main/fc/rc_controls.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 339dea94c6..4d547931e9 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -195,7 +195,7 @@ typedef enum { ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, - ADJUSTMENT_RC_RATE_YAW, + ADJUSTMENT_RC_RATE_YAW, ADJUSTMENT_FUNCTION_COUNT, } adjustmentFunction_e; From 31c3790a5d1bd5de5ffb89de408e0fc637726384 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 7 Aug 2016 02:07:16 +1200 Subject: [PATCH 271/456] Added CLI 'diff' command. --- src/main/config/config.c | 306 +++++++------ src/main/config/config.h | 1 - src/main/config/config_master.h | 2 + src/main/io/ledstrip.c | 2 - src/main/io/osd.c | 34 +- src/main/io/osd.h | 2 +- src/main/io/serial_cli.c | 706 +++++++++++++++++++++--------- src/main/target/CC3D/target.h | 12 +- src/main/target/NAZE/target.h | 2 +- src/main/target/PORT103R/target.h | 10 +- 10 files changed, 696 insertions(+), 381 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 1db98cec84..243ac2f45b 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -275,7 +275,7 @@ void resetProfile(profile_t *profile) resetControlRateConfig(&profile->controlRateProfile[rI]); } - profile->activeRateProfile = 0; + profile->activeRateProfile = 0; } #ifdef GPS @@ -442,263 +442,280 @@ uint16_t getCurrentMinthrottle(void) return masterConfig.escAndServoConfig.minthrottle; } +static void intFeatureClearAll(master_t *config); +static void intFeatureSet(uint32_t mask, master_t *config); +static void intFeatureClear(uint32_t mask, master_t *config); + // Default settings -static void resetConf(void) +void createDefaultConfig(master_t *config) { // Clear all configuration - memset(&masterConfig, 0, sizeof(master_t)); - setProfile(0); + memset(config, 0, sizeof(master_t)); - featureClearAll(); - featureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES); + intFeatureClearAll(config); + intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES, config); #ifdef DEFAULT_FEATURES - featureSet(DEFAULT_FEATURES); + intFeatureSet(DEFAULT_FEATURES, config); #endif #ifdef OSD - resetOsdConfig(); + intFeatureSet(FEATURE_OSD, config); + resetOsdConfig(&config->osdProfile); #endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise // the user may see incorrect readings and unexpected issues with pin mappings may occur. - featureSet(FEATURE_VBAT); + intFeatureSet(FEATURE_VBAT, config); #endif - - masterConfig.version = EEPROM_CONF_VERSION; - masterConfig.mixerMode = MIXER_QUADX; + config->version = EEPROM_CONF_VERSION; + config->mixerMode = MIXER_QUADX; // global settings - masterConfig.current_profile_index = 0; // default profile - masterConfig.dcm_kp = 2500; // 1.0 * 10000 - masterConfig.dcm_ki = 0; // 0.003 * 10000 - masterConfig.gyro_lpf = 0; // 256HZ default + config->current_profile_index = 0; // default profile + config->dcm_kp = 2500; // 1.0 * 10000 + config->dcm_ki = 0; // 0.003 * 10000 + config->gyro_lpf = 0; // 256HZ default #ifdef STM32F10X - masterConfig.gyro_sync_denom = 8; - masterConfig.pid_process_denom = 1; + config->gyro_sync_denom = 8; + config->pid_process_denom = 1; #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) - masterConfig.gyro_sync_denom = 1; - masterConfig.pid_process_denom = 4; + config->gyro_sync_denom = 1; + config->pid_process_denom = 4; #else - masterConfig.gyro_sync_denom = 4; - masterConfig.pid_process_denom = 2; + config->gyro_sync_denom = 4; + config->pid_process_denom = 2; #endif - masterConfig.gyro_soft_type = FILTER_PT1; - masterConfig.gyro_soft_lpf_hz = 90; - masterConfig.gyro_soft_notch_hz = 0; - masterConfig.gyro_soft_notch_cutoff = 150; + config->gyro_soft_type = FILTER_PT1; + config->gyro_soft_lpf_hz = 90; + config->gyro_soft_notch_hz = 0; + config->gyro_soft_notch_cutoff = 150; - masterConfig.debug_mode = DEBUG_NONE; + config->debug_mode = DEBUG_NONE; - resetAccelerometerTrims(&masterConfig.accZero); + resetAccelerometerTrims(&config->accZero); - resetSensorAlignment(&masterConfig.sensorAlignmentConfig); + resetSensorAlignment(&config->sensorAlignmentConfig); - masterConfig.boardAlignment.rollDegrees = 0; - masterConfig.boardAlignment.pitchDegrees = 0; - masterConfig.boardAlignment.yawDegrees = 0; - masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect - masterConfig.max_angle_inclination = 700; // 70 degrees - masterConfig.yaw_control_direction = 1; - masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32; + config->boardAlignment.rollDegrees = 0; + config->boardAlignment.pitchDegrees = 0; + config->boardAlignment.yawDegrees = 0; + config->acc_hardware = ACC_DEFAULT; // default/autodetect + config->max_angle_inclination = 700; // 70 degrees + config->yaw_control_direction = 1; + config->gyroConfig.gyroMovementCalibrationThreshold = 32; // xxx_hardware: 0:default/autodetect, 1: disable - masterConfig.mag_hardware = 0; + config->mag_hardware = 0; - masterConfig.baro_hardware = 0; + config->baro_hardware = 0; - resetBatteryConfig(&masterConfig.batteryConfig); + resetBatteryConfig(&config->batteryConfig); #ifdef TELEMETRY - resetTelemetryConfig(&masterConfig.telemetryConfig); + resetTelemetryConfig(&config->telemetryConfig); #endif #ifdef SERIALRX_PROVIDER - masterConfig.rxConfig.serialrx_provider = SERIALRX_PROVIDER; + config->rxConfig.serialrx_provider = SERIALRX_PROVIDER; #else - masterConfig.rxConfig.serialrx_provider = 0; + config->rxConfig.serialrx_provider = 0; #endif - masterConfig.rxConfig.sbus_inversion = 1; - masterConfig.rxConfig.spektrum_sat_bind = 0; - masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; - masterConfig.rxConfig.midrc = 1500; - masterConfig.rxConfig.mincheck = 1100; - masterConfig.rxConfig.maxcheck = 1900; - masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection - masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection + config->rxConfig.sbus_inversion = 1; + config->rxConfig.spektrum_sat_bind = 0; + config->rxConfig.spektrum_sat_bind_autoreset = 1; + config->rxConfig.midrc = 1500; + config->rxConfig.mincheck = 1100; + config->rxConfig.maxcheck = 1900; + config->rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection + config->rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { - rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[i]; + rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &config->rxConfig.failsafe_channel_configurations[i]; channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD; - channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc); + channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.midrc); } - masterConfig.rxConfig.rssi_channel = 0; - masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; - masterConfig.rxConfig.rssi_ppm_invert = 0; - masterConfig.rxConfig.rcInterpolation = RC_SMOOTHING_AUTO; - masterConfig.rxConfig.rcInterpolationInterval = 19; - masterConfig.rxConfig.fpvCamAngleDegrees = 0; - masterConfig.rxConfig.max_aux_channel = MAX_AUX_CHANNELS; - masterConfig.rxConfig.airModeActivateThreshold = 1350; + config->rxConfig.rssi_channel = 0; + config->rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; + config->rxConfig.rssi_ppm_invert = 0; + config->rxConfig.rcInterpolation = RC_SMOOTHING_AUTO; + config->rxConfig.rcInterpolationInterval = 19; + config->rxConfig.fpvCamAngleDegrees = 0; + config->rxConfig.max_aux_channel = MAX_AUX_CHANNELS; + config->rxConfig.airModeActivateThreshold = 1350; - resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); + resetAllRxChannelRangeConfigurations(config->rxConfig.channelRanges); - masterConfig.inputFilteringMode = INPUT_FILTERING_DISABLED; + config->inputFilteringMode = INPUT_FILTERING_DISABLED; - masterConfig.gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support - masterConfig.disarm_kill_switch = 1; - masterConfig.auto_disarm_delay = 5; - masterConfig.small_angle = 25; + config->gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support + config->disarm_kill_switch = 1; + config->auto_disarm_delay = 5; + config->small_angle = 25; - resetMixerConfig(&masterConfig.mixerConfig); + resetMixerConfig(&config->mixerConfig); - masterConfig.airplaneConfig.fixedwing_althold_dir = 1; + config->airplaneConfig.fixedwing_althold_dir = 1; // Motor/ESC/Servo - resetEscAndServoConfig(&masterConfig.escAndServoConfig); - resetFlight3DConfig(&masterConfig.flight3DConfig); + resetEscAndServoConfig(&config->escAndServoConfig); + resetFlight3DConfig(&config->flight3DConfig); #ifdef BRUSHED_MOTORS - masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; - masterConfig.motor_pwm_protocol = PWM_TYPE_BRUSHED; - masterConfig.use_unsyncedPwm = true; + config->motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; + config->motor_pwm_protocol = PWM_TYPE_BRUSHED; + config->use_unsyncedPwm = true; #else - masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; - masterConfig.motor_pwm_protocol = PWM_TYPE_ONESHOT125; + config->motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; + config->motor_pwm_protocol = PWM_TYPE_ONESHOT125; #endif - masterConfig.servo_pwm_rate = 50; + + config->servo_pwm_rate = 50; #ifdef CC3D - masterConfig.use_buzzer_p6 = 0; + config->use_buzzer_p6 = 0; #endif #ifdef GPS // gps/nav stuff - masterConfig.gpsConfig.provider = GPS_NMEA; - masterConfig.gpsConfig.sbasMode = SBAS_AUTO; - masterConfig.gpsConfig.autoConfig = GPS_AUTOCONFIG_ON; - masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_OFF; + config->gpsConfig.provider = GPS_NMEA; + config->gpsConfig.sbasMode = SBAS_AUTO; + config->gpsConfig.autoConfig = GPS_AUTOCONFIG_ON; + config->gpsConfig.autoBaud = GPS_AUTOBAUD_OFF; #endif - resetSerialConfig(&masterConfig.serialConfig); + resetSerialConfig(&config->serialConfig); - masterConfig.emf_avoidance = 0; // TODO - needs removal + config->emf_avoidance = 0; // TODO - needs removal - resetProfile(currentProfile); + resetProfile(&config->profile[0]); - resetRollAndPitchTrims(&masterConfig.accelerometerTrims); + resetRollAndPitchTrims(&config->accelerometerTrims); - masterConfig.mag_declination = 0; - masterConfig.acc_lpf_hz = 10.0f; - masterConfig.accDeadband.xy = 40; - masterConfig.accDeadband.z = 40; - masterConfig.acc_unarmedcal = 1; + config->mag_declination = 0; + config->acc_lpf_hz = 10.0f; + config->accDeadband.xy = 40; + config->accDeadband.z = 40; + config->acc_unarmedcal = 1; #ifdef BARO - resetBarometerConfig(&masterConfig.barometerConfig); + resetBarometerConfig(&config->barometerConfig); #endif // Radio #ifdef RX_CHANNELS_TAER - parseRcChannels("TAER1234", &masterConfig.rxConfig); + parseRcChannels("TAER1234", &config->rxConfig); #else - parseRcChannels("AETR1234", &masterConfig.rxConfig); + parseRcChannels("AETR1234", &config->rxConfig); #endif - resetRcControlsConfig(&masterConfig.rcControlsConfig); + resetRcControlsConfig(&config->rcControlsConfig); - masterConfig.throttle_correction_value = 0; // could 10 with althold or 40 for fpv - masterConfig.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv + config->throttle_correction_value = 0; // could 10 with althold or 40 for fpv + config->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv // Failsafe Variables - masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec - masterConfig.failsafeConfig.failsafe_off_delay = 10; // 1sec - masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off. - masterConfig.failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss - masterConfig.failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition - masterConfig.failsafeConfig.failsafe_procedure = 0; // default full failsafe procedure is 0: auto-landing + config->failsafeConfig.failsafe_delay = 10; // 1sec + config->failsafeConfig.failsafe_off_delay = 10; // 1sec + config->failsafeConfig.failsafe_throttle = 1000; // default throttle off. + config->failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss + config->failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition + config->failsafeConfig.failsafe_procedure = 0; // default full failsafe procedure is 0: auto-landing #ifdef USE_SERVOS // servos for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - masterConfig.servoConf[i].min = DEFAULT_SERVO_MIN; - masterConfig.servoConf[i].max = DEFAULT_SERVO_MAX; - masterConfig.servoConf[i].middle = DEFAULT_SERVO_MIDDLE; - masterConfig.servoConf[i].rate = 100; - masterConfig.servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE; - masterConfig.servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; - masterConfig.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; + config->servoConf[i].min = DEFAULT_SERVO_MIN; + config->servoConf[i].max = DEFAULT_SERVO_MAX; + config->servoConf[i].middle = DEFAULT_SERVO_MIDDLE; + config->servoConf[i].rate = 100; + config->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE; + config->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; + config->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } // gimbal - masterConfig.gimbalConfig.mode = GIMBAL_MODE_NORMAL; + config->gimbalConfig.mode = GIMBAL_MODE_NORMAL; #endif #ifdef GPS - resetGpsProfile(&masterConfig.gpsProfile); + resetGpsProfile(&config->gpsProfile); #endif // custom mixer. clear by defaults. for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - masterConfig.customMotorMixer[i].throttle = 0.0f; + config->customMotorMixer[i].throttle = 0.0f; } #ifdef LED_STRIP - applyDefaultColors(masterConfig.colors); - applyDefaultLedStripConfig(masterConfig.ledConfigs); - applyDefaultModeColors(masterConfig.modeColors); - applyDefaultSpecialColors(&(masterConfig.specialColors)); - masterConfig.ledstrip_visual_beeper = 0; + applyDefaultColors(config->colors); + applyDefaultLedStripConfig(config->ledConfigs); + applyDefaultModeColors(config->modeColors); + applyDefaultSpecialColors(&(config->specialColors)); + config->ledstrip_visual_beeper = 0; #endif #ifdef VTX - masterConfig.vtx_band = 4; //Fatshark/Airwaves - masterConfig.vtx_channel = 1; //CH1 - masterConfig.vtx_mode = 0; //CH+BAND mode - masterConfig.vtx_mhz = 5740; //F0 + config->vtx_band = 4; //Fatshark/Airwaves + config->vtx_channel = 1; //CH1 + config->vtx_mode = 0; //CH+BAND mode + config->vtx_mhz = 5740; //F0 #endif #ifdef TRANSPONDER static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware - memcpy(masterConfig.transponderData, &defaultTransponderData, sizeof(defaultTransponderData)); + memcpy(config->transponderData, &defaultTransponderData, sizeof(defaultTransponderData)); #endif #ifdef BLACKBOX - #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) - featureSet(FEATURE_BLACKBOX); - masterConfig.blackbox_device = BLACKBOX_DEVICE_FLASH; + intFeatureSet(FEATURE_BLACKBOX, config); + config->blackbox_device = BLACKBOX_DEVICE_FLASH; #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT) - featureSet(FEATURE_BLACKBOX); - masterConfig.blackbox_device = BLACKBOX_DEVICE_SDCARD; + intFeatureSet(FEATURE_BLACKBOX, config); + config->blackbox_device = BLACKBOX_DEVICE_SDCARD; #else - masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL; + config->blackbox_device = BLACKBOX_DEVICE_SERIAL; #endif - masterConfig.blackbox_rate_num = 1; - masterConfig.blackbox_rate_denom = 1; - + config->blackbox_rate_num = 1; + config->blackbox_rate_denom = 1; #endif // BLACKBOX #ifdef SERIALRX_UART if (featureConfigured(FEATURE_RX_SERIAL)) { - masterConfig.serialConfig.portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL; + config->serialConfig.portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL; } #endif #if defined(TARGET_CONFIG) - targetConfiguration(); + // Don't look at target specific config settings when getting default + // values for a CLI diff. This is suboptimal, but it doesn't break the + // public API + if (config == &masterConfig) { + targetConfiguration(); + } #endif // copy first profile into remaining profile for (int i = 1; i < MAX_PROFILE_COUNT; i++) { - memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); + memcpy(&config->profile[i], &config->profile[0], sizeof(profile_t)); } +} +static void resetConf(void) +{ + createDefaultConfig(&masterConfig); + + setProfile(0); + +#ifdef LED_STRIP + reevaluateLedConfig(); +#endif } static uint8_t calculateChecksum(const uint8_t *data, uint32_t length) @@ -1069,17 +1086,32 @@ bool feature(uint32_t mask) void featureSet(uint32_t mask) { - masterConfig.enabledFeatures |= mask; + intFeatureSet(mask, &masterConfig); +} + +static void intFeatureSet(uint32_t mask, master_t *config) +{ + config->enabledFeatures |= mask; } void featureClear(uint32_t mask) { - masterConfig.enabledFeatures &= ~(mask); + intFeatureClear(mask, &masterConfig); +} + +static void intFeatureClear(uint32_t mask, master_t *config) +{ + config->enabledFeatures &= ~(mask); } void featureClearAll() { - masterConfig.enabledFeatures = 0; + intFeatureClearAll(&masterConfig); +} + +static void intFeatureClearAll(master_t *config) +{ + config->enabledFeatures = 0; } uint32_t featureMask(void) diff --git a/src/main/config/config.h b/src/main/config/config.h index e1307b0159..97e9166289 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -25,7 +25,6 @@ #define MAX_RATEPROFILES 3 #define MAX_NAME_LENGTH 16 - typedef enum { FEATURE_RX_PPM = 1 << 0, FEATURE_VBAT = 1 << 1, diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 9e37154d1b..0f79ee2f44 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -174,3 +174,5 @@ typedef struct master_t { extern master_t masterConfig; extern profile_t *currentProfile; extern controlRateConfig_t *currentControlRateProfile; + +void createDefaultConfig(master_t *config); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 07c0e65bd0..68b47a7c2f 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -1068,8 +1068,6 @@ void pgResetFn_specialColors(specialColorIndexes_t *instance) void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) { memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); - - reevaluateLedConfig(); } void applyDefaultColors(hsvColor_t *colors) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d956df4832..5d7aaba1ff 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -833,24 +833,22 @@ void osdInit(void) max7456_draw_screen(); } -void resetOsdConfig(void) +void resetOsdConfig(osd_profile *osdProfile) { - featureSet(FEATURE_OSD); - masterConfig.osdProfile.video_system = AUTO; - masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; - masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; - masterConfig.osdProfile.item_pos[OSD_TIMER] = -39; - masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] = -9; - masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] = 26; - masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL] = 1; - masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] = -80; - masterConfig.osdProfile.item_pos[OSD_ARMED] = -107; - masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; - masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON] = -1; - masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS] = -1; - masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW] = -23; - masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] = -18; - masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME] = 1; + osdProfile->video_system = AUTO; + osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; + osdProfile->item_pos[OSD_RSSI_VALUE] = -59; + osdProfile->item_pos[OSD_TIMER] = -39; + osdProfile->item_pos[OSD_THROTTLE_POS] = -9; + osdProfile->item_pos[OSD_CPU_LOAD] = 26; + osdProfile->item_pos[OSD_VTX_CHANNEL] = 1; + osdProfile->item_pos[OSD_VOLTAGE_WARNING] = -80; + osdProfile->item_pos[OSD_ARMED] = -107; + osdProfile->item_pos[OSD_DISARMED] = -109; + osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = -1; + osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = -1; + osdProfile->item_pos[OSD_CURRENT_DRAW] = -23; + osdProfile->item_pos[OSD_MAH_DRAWN] = -18; + osdProfile->item_pos[OSD_CRAFT_NAME] = 1; } - #endif diff --git a/src/main/io/osd.h b/src/main/io/osd.h index b5f4e953db..0685f9897f 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -65,4 +65,4 @@ typedef struct { void updateOsd(void); void osdInit(void); -void resetOsdConfig(void); +void resetOsdConfig(osd_profile *osdProfile); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 958a602c39..14e51461fb 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -115,8 +115,10 @@ static void cliMotorMix(char *cmdline); static void cliDefaults(char *cmdline); void cliDfu(char *cmdLine); static void cliDump(char *cmdLine); -void cliDumpProfile(uint8_t profileIndex); -void cliDumpRateProfile(uint8_t rateProfileIndex) ; +static void cliDiff(char *cmdLine); +static void printConfig(char *cmdLine, bool doDiff); +static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, master_t *defaultProfile); +static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, master_t *defaultProfile) ; static void cliExit(char *cmdline); static void cliFeature(char *cmdline); static void cliMotor(char *cmdline); @@ -190,6 +192,17 @@ static void cliBeeper(char *cmdline); static char cliBuffer[48]; static uint32_t bufferIndex = 0; +typedef enum { + DUMP_MASTER = (1 << 0), + DUMP_PROFILE = (1 << 1), + DUMP_RATES = (1 << 2), + DUMP_ALL = (1 << 3), + DO_DIFF = (1 << 4), + DIFF_COMMENTED = (1 << 5), +} dumpFlags_e; + +static const char* const sectionBreak = "\r\n"; + #ifndef USE_QUAD_MIXER_ONLY // sync this with mixerMode_e static const char * const mixerNames[] = { @@ -270,7 +283,10 @@ const clicmd_t cmdTable[] = { #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), - CLI_COMMAND_DEF("dump", "dump configuration", "[master|profile]", cliDump), + CLI_COMMAND_DEF("diff", "list configuration changes from default", + "[master|profile|rates|all] {commented}", cliDiff), + CLI_COMMAND_DEF("dump", "dump configuration", + "[master|profile|rates|all]", cliDump), CLI_COMMAND_DEF("exit", NULL, NULL, cliExit), CLI_COMMAND_DEF("feature", "configure features", "list\r\n" @@ -924,6 +940,11 @@ static void cliPrint(const char *str); static void cliPrintf(const char *fmt, ...); static void cliWrite(uint8_t ch); +#define printSectionBreak() cliPrintf((char *)sectionBreak) + +#define COMPARE_CONFIG(value) (masterConfig.value == defaultConfig.value) +static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...); + static void cliPrompt(void) { cliPrint("\r\n# "); @@ -969,6 +990,36 @@ static bool isEmpty(const char *string) return *string == '\0'; } +static void printRxFail(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out rxConfig failsafe settings + rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration; + rxFailsafeChannelConfiguration_t *channelFailsafeConfigurationDefault; + bool equalsDefault; + bool requireValue; + char modeCharacter; + for (uint8_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { + channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[channel]; + channelFailsafeConfigurationDefault = &defaultConfig->rxConfig.failsafe_channel_configurations[channel]; + equalsDefault = channelFailsafeConfiguration->mode == channelFailsafeConfigurationDefault->mode + && channelFailsafeConfiguration->step == channelFailsafeConfigurationDefault->step; + requireValue = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET; + modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfiguration->mode]; + if (requireValue) { + cliDumpPrintf(dumpMask, equalsDefault, "rxfail %u %c %d\r\n", + channel, + modeCharacter, + RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step) + ); + } else { + cliDumpPrintf(dumpMask, equalsDefault, "rxfail %u %c\r\n", + channel, + modeCharacter + ); + } + } +} + static void cliRxFail(char *cmdline) { uint8_t channel; @@ -1031,10 +1082,9 @@ static void cliRxFail(char *cmdline) char modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfiguration->mode]; - // triple use of cliPrintf below + // double use of cliPrintf below // 1. acknowledge interpretation on command, // 2. query current setting on single item, - // 3. recursive use for full list. if (requireValue) { cliPrintf("rxfail %u %c %d\r\n", @@ -1054,23 +1104,36 @@ static void cliRxFail(char *cmdline) } } +static void printAux(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out aux channel settings + modeActivationCondition_t *mac; + modeActivationCondition_t *macDefault; + bool equalsDefault; + for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + mac = &masterConfig.modeActivationConditions[i]; + macDefault = &defaultConfig->modeActivationConditions[i]; + equalsDefault = mac->modeId == macDefault->modeId + && mac->auxChannelIndex == macDefault->auxChannelIndex + && mac->range.startStep == macDefault->range.startStep + && mac->range.endStep == macDefault->range.endStep; + cliDumpPrintf(dumpMask, equalsDefault, "aux %u %u %u %u %u\r\n", + i, + mac->modeId, + mac->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep) + ); + } +} + static void cliAux(char *cmdline) { int i, val = 0; char *ptr; if (isEmpty(cmdline)) { - // print out aux channel settings - for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { - modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; - cliPrintf("aux %u %u %u %u %u\r\n", - i, - mac->modeId, - mac->auxChannelIndex, - MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep) - ); - } + printAux(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr++); @@ -1104,28 +1167,44 @@ static void cliAux(char *cmdline) } } +static void printSerial(uint8_t dumpMask, master_t *defaultConfig) +{ + serialConfig_t *serialConfig; + serialConfig_t *serialConfigDefault; + bool equalsDefault; + for (int i = 0; i < SERIAL_PORT_COUNT; i++) { + serialConfig = &masterConfig.serialConfig; + if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) { + continue; + }; + serialConfigDefault = &defaultConfig->serialConfig; + equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier + && serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask + && serialConfig->portConfigs[i].msp_baudrateIndex == serialConfigDefault->portConfigs[i].msp_baudrateIndex + && serialConfig->portConfigs[i].gps_baudrateIndex == serialConfigDefault->portConfigs[i].gps_baudrateIndex + && serialConfig->portConfigs[i].telemetry_baudrateIndex == serialConfigDefault->portConfigs[i].telemetry_baudrateIndex + && serialConfig->portConfigs[i].blackbox_baudrateIndex == serialConfigDefault->portConfigs[i].blackbox_baudrateIndex; + cliDumpPrintf(dumpMask, equalsDefault, "serial %d %d %ld %ld %ld %ld\r\n" , + serialConfig->portConfigs[i].identifier, + serialConfig->portConfigs[i].functionMask, + baudRates[serialConfig->portConfigs[i].msp_baudrateIndex], + baudRates[serialConfig->portConfigs[i].gps_baudrateIndex], + baudRates[serialConfig->portConfigs[i].telemetry_baudrateIndex], + baudRates[serialConfig->portConfigs[i].blackbox_baudrateIndex] + ); + } +} + static void cliSerial(char *cmdline) { int i, val; char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < SERIAL_PORT_COUNT; i++) { - if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) { - continue; - }; - cliPrintf("serial %d %d %ld %ld %ld %ld\r\n" , - masterConfig.serialConfig.portConfigs[i].identifier, - masterConfig.serialConfig.portConfigs[i].functionMask, - baudRates[masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex], - baudRates[masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex], - baudRates[masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex], - baudRates[masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex] - ); - } - return; - } + printSerial(DUMP_MASTER, &masterConfig); + return; + } serialPortConfig_t portConfig; memset(&portConfig, 0 , sizeof(portConfig)); @@ -1271,25 +1350,40 @@ static void cliSerialPassthrough(char *cmdline) } #endif +static void printAdjustmentRange(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out adjustment ranges channel settings + adjustmentRange_t *ar; + adjustmentRange_t *arDefault; + bool equalsDefault; + for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { + ar = &masterConfig.adjustmentRanges[i]; + arDefault = &defaultConfig->adjustmentRanges[i]; + equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex + && ar->range.startStep == arDefault->range.startStep + && ar->range.endStep == arDefault->range.endStep + && ar->adjustmentFunction == arDefault->adjustmentFunction + && ar->auxSwitchChannelIndex == arDefault->auxSwitchChannelIndex + && ar->adjustmentIndex == arDefault->adjustmentIndex; + cliDumpPrintf(dumpMask, equalsDefault, "adjrange %u %u %u %u %u %u %u\r\n", + i, + ar->adjustmentIndex, + ar->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(ar->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(ar->range.endStep), + ar->adjustmentFunction, + ar->auxSwitchChannelIndex + ); + } +} + static void cliAdjustmentRange(char *cmdline) { int i, val = 0; char *ptr; if (isEmpty(cmdline)) { - // print out adjustment ranges channel settings - for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { - adjustmentRange_t *ar = &masterConfig.adjustmentRanges[i]; - cliPrintf("adjrange %u %u %u %u %u %u %u\r\n", - i, - ar->adjustmentIndex, - ar->auxChannelIndex, - MODE_STEP_TO_CHANNEL_VALUE(ar->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(ar->range.endStep), - ar->adjustmentFunction, - ar->auxSwitchChannelIndex - ); - } + printAdjustmentRange(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr++); @@ -1424,16 +1518,27 @@ static void cliMotorMix(char *cmdline) #endif } +static void printRxRange(uint8_t dumpMask, master_t *defaultConfig) +{ + rxChannelRangeConfiguration_t *channelRangeConfiguration; + rxChannelRangeConfiguration_t *channelRangeConfigurationDefault; + bool equalsDefault; + for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { + channelRangeConfiguration = &masterConfig.rxConfig.channelRanges[i]; + channelRangeConfigurationDefault = &defaultConfig->rxConfig.channelRanges[i]; + equalsDefault = channelRangeConfiguration->min == channelRangeConfigurationDefault->min + && channelRangeConfiguration->max == channelRangeConfigurationDefault->max; + cliDumpPrintf(dumpMask, equalsDefault, "rxrange %u %u %u\r\n", i, channelRangeConfiguration->min, channelRangeConfiguration->max); + } +} + static void cliRxRange(char *cmdline) { int i, validArgumentCount = 0; char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { - rxChannelRangeConfiguration_t *channelRangeConfiguration = &masterConfig.rxConfig.channelRanges[i]; - cliPrintf("rxrange %u %u %u\r\n", i, channelRangeConfiguration->min, channelRangeConfiguration->max); - } + printRxRange(DUMP_MASTER, &masterConfig); } else if (strcasecmp(cmdline, "reset") == 0) { resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); } else { @@ -1470,17 +1575,28 @@ static void cliRxRange(char *cmdline) } #ifdef LED_STRIP +static void printLed(uint8_t dumpMask, master_t *defaultConfig) +{ + bool equalsDefault; + ledConfig_t ledConfig; + ledConfig_t ledConfigDefault; + char ledConfigBuffer[20]; + for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) { + ledConfig = masterConfig.ledConfigs[i]; + ledConfigDefault = defaultConfig->ledConfigs[i]; + equalsDefault = ledConfig == ledConfigDefault; + generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); + cliDumpPrintf(dumpMask, equalsDefault, "led %u %s\r\n", i, ledConfigBuffer); + } +} + static void cliLed(char *cmdline) { int i; char *ptr; - char ledConfigBuffer[20]; if (isEmpty(cmdline)) { - for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); - cliPrintf("led %u %s\r\n", i, ledConfigBuffer); - } + printLed(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr); @@ -1495,20 +1611,33 @@ static void cliLed(char *cmdline) } } +static void printColor(uint8_t dumpMask, master_t *defaultConfig) +{ + hsvColor_t *color; + hsvColor_t *colorDefault; + bool equalsDefault; + for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { + color = &masterConfig.colors[i]; + colorDefault = &defaultConfig->colors[i]; + equalsDefault = color->h == colorDefault->h + && color->s == colorDefault->s + && color->v == colorDefault->v; + cliDumpPrintf(dumpMask, equalsDefault, "color %u %d,%u,%u\r\n", + i, + color->h, + color->s, + color->v + ); + } +} + static void cliColor(char *cmdline) { int i; char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - cliPrintf("color %u %d,%u,%u\r\n", - i, - masterConfig.colors[i].h, - masterConfig.colors[i].s, - masterConfig.colors[i].v - ); - } + printColor(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr); @@ -1523,20 +1652,27 @@ static void cliColor(char *cmdline) } } +static void printModeColor(uint8_t dumpMask, master_t *defaultConfig) +{ + for (int i = 0; i < LED_MODE_COUNT; i++) { + for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + int colorIndex = masterConfig.modeColors[i].color[j]; + int colorIndexDefault = defaultConfig->modeColors[i].color[j]; + cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, "mode_color %u %u %u\r\n", i, j, colorIndex); + } + } + + for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + int colorIndex = masterConfig.specialColors.color[j]; + int colorIndexDefault = defaultConfig->specialColors.color[j]; + cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, "mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex); + } +} + static void cliModeColor(char *cmdline) { if (isEmpty(cmdline)) { - for (int i = 0; i < LED_MODE_COUNT; i++) { - for (int j = 0; j < LED_DIRECTION_COUNT; j++) { - int colorIndex = modeColors[i].color[j]; - cliPrintf("mode_color %u %u %u\r\n", i, j, colorIndex); - } - } - - for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { - int colorIndex = specialColors.color[j]; - cliPrintf("mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex); - } + printModeColor(DUMP_MASTER, &masterConfig); } else { enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; int args[ARGS_COUNT]; @@ -1566,6 +1702,52 @@ static void cliModeColor(char *cmdline) #endif #ifdef USE_SERVOS +static void printServo(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out servo settings + int i; + servoParam_t *servoConf; + servoParam_t *servoConfDefault; + bool equalsDefault; + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servoConf = &masterConfig.servoConf[i]; + servoConfDefault = &defaultConfig->servoConf[i]; + equalsDefault = servoConf->min == servoConfDefault->min + && servoConf->max == servoConfDefault->max + && servoConf->middle == servoConfDefault->middle + && servoConf->angleAtMin == servoConfDefault->angleAtMax + && servoConf->rate == servoConfDefault->rate + && servoConf->forwardFromChannel == servoConfDefault->forwardFromChannel; + + cliDumpPrintf(dumpMask, equalsDefault, "servo %u %d %d %d %d %d %d %d\r\n", + i, + servoConf->min, + servoConf->max, + servoConf->middle, + servoConf->angleAtMin, + servoConf->angleAtMax, + servoConf->rate, + servoConf->forwardFromChannel + ); + } + + printSectionBreak(); + + // print servo directions + unsigned int channel; + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servoConf = &masterConfig.servoConf[i]; + servoConfDefault = &defaultConfig->servoConf[i]; + equalsDefault = servoConf->reversedSources == servoConfDefault->reversedSources; + for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { + if (servoConf->reversedSources & (1 << channel)) { + equalsDefault = ~(servoConf->reversedSources ^ servoConfDefault->reversedSources) & (1 << channel); + cliDumpPrintf(dumpMask, equalsDefault, "smix reverse %d %d r\r\n", i , channel); + } + } + } +} + static void cliServo(char *cmdline) { enum { SERVO_ARGUMENT_COUNT = 8 }; @@ -1577,21 +1759,7 @@ static void cliServo(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - // print out servo settings - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo = &masterConfig.servoConf[i]; - - cliPrintf("servo %u %d %d %d %d %d %d %d\r\n", - i, - servo->min, - servo->max, - servo->middle, - servo->angleAtMin, - servo->angleAtMax, - servo->rate, - servo->forwardFromChannel - ); - } + printServo(DUMP_MASTER, &masterConfig); } else { int validArgumentCount = 0; @@ -1948,24 +2116,38 @@ static void cliFlashRead(char *cmdline) #endif #ifdef VTX +static void printVtx(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out vtx channel settings + vtxChannelActivationCondition_t *cac; + vtxChannelActivationCondition_t *cacDefault; + bool equalsDefault; + for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { + cac = &masterConfig.vtxChannelActivationConditions[i]; + cacDefault = &defaultConfig->vtxChannelActivationConditions[i]; + equalsDefault = cac->auxChannelIndex == cacDefault->auxChannelIndex + && cac->band == cacDefault->band + && cac->channel == cacDefault->channel + && cac->range.startStep == cacDefault->range.startStep + && cac->range.endStep == cacDefault->range.endStep; + cliDumpPrintf(dumpMask, equalsDefault, "vtx %u %u %u %u %u %u\r\n", + i, + cac->auxChannelIndex, + cac->band, + cac->channel, + MODE_STEP_TO_CHANNEL_VALUE(cac->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(cac->range.endStep) + ); + } +} + static void cliVtx(char *cmdline) { int i, val = 0; char *ptr; if (isEmpty(cmdline)) { - // print out vtx channel settings - for (i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { - vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i]; - printf("vtx %u %u %u %u %u %u\r\n", - i, - cac->auxChannelIndex, - cac->band, - cac->channel, - MODE_STEP_TO_CHANNEL_VALUE(cac->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(cac->range.endStep) - ); - } + printVtx(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr++); @@ -2008,7 +2190,72 @@ static void cliVtx(char *cmdline) } #endif -static void dumpValues(uint16_t valueSection) +static void printName(uint8_t dumpMask) +{ + cliDumpPrintf(dumpMask, strlen(masterConfig.name) == 0, "name %s\r\n", strlen(masterConfig.name) > 0 ? masterConfig.name : "-"); +} + +static void cliName(char *cmdline) +{ + uint32_t len = strlen(cmdline); + if (len > 0) { + memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); + strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); + } + printName(DUMP_MASTER); +} + +void *getValuePointer(const clivalue_t *value) +{ + void *ptr = value->ptr; + + if ((value->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { + ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); + } + + if ((value->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { + ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); + } + + return ptr; +} + +static bool valueEqualsDefault(const clivalue_t *value, master_t *defaultConfig) +{ + void *ptr = getValuePointer(value); + + void *ptrDefault = ((uint8_t *)ptr) - (uint32_t)&masterConfig + (uint32_t)defaultConfig; + + bool result = false; + switch (value->type & VALUE_TYPE_MASK) { + case VAR_UINT8: + result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault; + break; + + case VAR_INT8: + result = *(int8_t *)ptr == *(int8_t *)ptrDefault; + break; + + case VAR_UINT16: + result = *(uint16_t *)ptr == *(uint16_t *)ptrDefault; + break; + + case VAR_INT16: + result = *(int16_t *)ptr == *(int16_t *)ptrDefault; + break; + + case VAR_UINT32: + result = *(uint32_t *)ptr == *(uint32_t *)ptrDefault; + break; + + case VAR_FLOAT: + result = *(float *)ptr == *(float *)ptrDefault; + break; + } + return result; +} + +static void dumpValues(uint16_t valueSection, uint8_t dumpMask, master_t *defaultConfig) { uint32_t i; const clivalue_t *value; @@ -2019,64 +2266,83 @@ static void dumpValues(uint16_t valueSection) continue; } - cliPrintf("set %s = ", valueTable[i].name); - cliPrintVar(value, 0); - cliPrint("\r\n"); + if (cliDumpPrintf(dumpMask, valueEqualsDefault(value, defaultConfig), "set %s = ", valueTable[i].name)) { + cliPrintVar(value, 0); + cliPrint("\r\n"); + } } } -typedef enum { - DUMP_MASTER = (1 << 0), - DUMP_PROFILE = (1 << 1), - DUMP_RATES = (1 << 2), - DUMP_ALL = (1 << 3), -} dumpFlags_e; - -static const char* const sectionBreak = "\r\n"; - -#define printSectionBreak() cliPrintf((char *)sectionBreak) - static void cliDump(char *cmdline) +{ + printConfig(cmdline, false); +} + +static void cliDiff(char *cmdline) +{ + printConfig(cmdline, true); +} + +char *checkCommand(char *cmdLine, const char *command) +{ + if(!strncasecmp(cmdLine, command, strlen(command)) // command names match + && !isalnum((unsigned)cmdLine[strlen(command)])) { // next characted in bufffer is not alphanumeric (command is correctly terminated) + return cmdLine + strlen(command) + 1; + } else { + return 0; + } +} + +static void printConfig(char *cmdline, bool doDiff) { unsigned int i; char buf[16]; uint32_t mask; - -#ifndef USE_QUAD_MIXER_ONLY - float thr, roll, pitch, yaw; -#endif + uint32_t defaultMask; + bool equalsDefault; uint8_t dumpMask = DUMP_MASTER; - if (strcasecmp(cmdline, "master") == 0) { + char *options; + if ((options = checkCommand(cmdline, "master"))) { dumpMask = DUMP_MASTER; // only - } - if (strcasecmp(cmdline, "profile") == 0) { + } else if ((options = checkCommand(cmdline, "profile"))) { dumpMask = DUMP_PROFILE; // only - } - if (strcasecmp(cmdline, "rates") == 0) { - dumpMask = DUMP_RATES; + } else if ((options = checkCommand(cmdline, "rates"))) { + dumpMask = DUMP_RATES; // only + } else if ((options = checkCommand(cmdline, "all"))) { + dumpMask = DUMP_ALL; // all profiles and rates } - if (strcasecmp(cmdline, "all") == 0) { - dumpMask = DUMP_ALL; // All profiles and rates + master_t defaultConfig; + if (doDiff) { + dumpMask = dumpMask | DO_DIFF; + createDefaultConfig(&defaultConfig); + } + + if (checkCommand(options, "commented")) { + dumpMask = dumpMask | DIFF_COMMENTED; // add unchanged values as comments for diff } if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { - cliPrint("\r\n# version\r\n"); cliVersion(NULL); + cliPrint("\r\n"); + + if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) { + cliPrint("\r\n# reset configuration to default settings\r\ndefaults\r\n"); + } printSectionBreak(); - cliPrint("\r\n# name\r\n"); - cliName(NULL); + cliPrint("# name\r\n"); + printName(dumpMask); cliPrint("\r\n# mixer\r\n"); - #ifndef USE_QUAD_MIXER_ONLY - cliPrintf("mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]); + cliDumpPrintf(dumpMask, COMPARE_CONFIG(mixerMode), "mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]); - cliPrintf("mmix reset\r\n"); + cliDumpPrintf(dumpMask, masterConfig.customMotorMixer[0].throttle == 0.0f, "mmix reset\r\n"); + float thr, roll, pitch, yaw, thrDefault, rollDefault, pitchDefault, yawDefault; for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) { if (masterConfig.customMotorMixer[i].throttle == 0.0f) break; @@ -2084,31 +2350,45 @@ static void cliDump(char *cmdline) roll = masterConfig.customMotorMixer[i].roll; pitch = masterConfig.customMotorMixer[i].pitch; yaw = masterConfig.customMotorMixer[i].yaw; - cliPrintf("mmix %d", i); - if (thr < 0) - cliWrite(' '); - cliPrintf("%s", ftoa(thr, buf)); - if (roll < 0) - cliWrite(' '); - cliPrintf("%s", ftoa(roll, buf)); - if (pitch < 0) - cliWrite(' '); - cliPrintf("%s", ftoa(pitch, buf)); - if (yaw < 0) - cliWrite(' '); - cliPrintf("%s\r\n", ftoa(yaw, buf)); + thrDefault = defaultConfig.customMotorMixer[i].throttle; + rollDefault = defaultConfig.customMotorMixer[i].roll; + pitchDefault = defaultConfig.customMotorMixer[i].pitch; + yawDefault = defaultConfig.customMotorMixer[i].yaw; + equalsDefault = thr == thrDefault && roll == rollDefault && pitch == pitchDefault && yaw == yawDefault; + + if (cliDumpPrintf(dumpMask, equalsDefault, "mmix %d", i)) { + if (thr < 0) + cliWrite(' '); + cliPrintf("%s", ftoa(thr, buf)); + if (roll < 0) + cliWrite(' '); + cliPrintf("%s", ftoa(roll, buf)); + if (pitch < 0) + cliWrite(' '); + cliPrintf("%s", ftoa(pitch, buf)); + if (yaw < 0) + cliWrite(' '); + cliPrintf("%s\r\n", ftoa(yaw, buf)); + } } #ifdef USE_SERVOS // print custom servo mixer if exists - cliPrintf("smix reset\r\n"); + cliDumpPrintf(dumpMask, masterConfig.customServoMixer[i].rate == 0, "smix reset\r\n"); for (i = 0; i < MAX_SERVO_RULES; i++) { - if (masterConfig.customServoMixer[i].rate == 0) break; - cliPrintf("smix %d %d %d %d %d %d %d %d\r\n", + equalsDefault = masterConfig.customServoMixer[i].targetChannel == defaultConfig.customServoMixer[i].targetChannel + && masterConfig.customServoMixer[i].inputSource == defaultConfig.customServoMixer[i].inputSource + && masterConfig.customServoMixer[i].rate == defaultConfig.customServoMixer[i].rate + && masterConfig.customServoMixer[i].speed == defaultConfig.customServoMixer[i].speed + && masterConfig.customServoMixer[i].min == defaultConfig.customServoMixer[i].min + && masterConfig.customServoMixer[i].max == defaultConfig.customServoMixer[i].max + && masterConfig.customServoMixer[i].box == masterConfig.customServoMixer[i].box; + + cliDumpPrintf(dumpMask, equalsDefault,"smix %d %d %d %d %d %d %d %d\r\n", i, masterConfig.customServoMixer[i].targetChannel, masterConfig.customServoMixer[i].inputSource, @@ -2119,102 +2399,96 @@ static void cliDump(char *cmdline) masterConfig.customServoMixer[i].box ); } - #endif #endif cliPrint("\r\n# feature\r\n"); + mask = featureMask(); + defaultMask = defaultConfig.enabledFeatures; for (i = 0; ; i++) { // disable all feature first if (featureNames[i] == NULL) break; - cliPrintf("feature -%s\r\n", featureNames[i]); + cliDumpPrintf(dumpMask, (mask | ~defaultMask) & (1 << i), "feature -%s\r\n", featureNames[i]); } for (i = 0; ; i++) { // reenable what we want. if (featureNames[i] == NULL) break; if (mask & (1 << i)) - cliPrintf("feature %s\r\n", featureNames[i]); + cliDumpPrintf(dumpMask, defaultMask & (1 << i), "feature %s\r\n", featureNames[i]); } #ifdef BEEPER cliPrint("\r\n# beeper\r\n"); uint8_t beeperCount = beeperTableEntryCount(); mask = getBeeperOffMask(); + defaultMask = defaultConfig.beeper_off_flags; for (int i = 0; i < (beeperCount-2); i++) { if (mask & (1 << i)) - cliPrintf("beeper -%s\r\n", beeperNameForTableIndex(i)); + cliDumpPrintf(dumpMask, (~(mask ^ defaultMask) & (1 << i)), "beeper -%s\r\n", beeperNameForTableIndex(i)); else - cliPrintf("beeper %s\r\n", beeperNameForTableIndex(i)); + cliDumpPrintf(dumpMask, (~(mask ^ defaultMask) & (1 << i)), "beeper %s\r\n", beeperNameForTableIndex(i)); } #endif cliPrint("\r\n# map\r\n"); - for (i = 0; i < 8; i++) + equalsDefault = true; + for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; + equalsDefault = equalsDefault && (masterConfig.rxConfig.rcmap[i] == defaultConfig.rxConfig.rcmap[i]); + } buf[i] = '\0'; - cliPrintf("map %s\r\n", buf); + cliDumpPrintf(dumpMask, equalsDefault, "map %s\r\n", buf); cliPrint("\r\n# serial\r\n"); - cliSerial(""); + printSerial(dumpMask, &defaultConfig); #ifdef LED_STRIP cliPrint("\r\n# led\r\n"); - cliLed(""); + printLed(dumpMask, &defaultConfig); cliPrint("\r\n# color\r\n"); - cliColor(""); + printColor(dumpMask, &defaultConfig); cliPrint("\r\n# mode_color\r\n"); - cliModeColor(""); + printModeColor(dumpMask, &defaultConfig); #endif cliPrint("\r\n# aux\r\n"); - cliAux(""); + printAux(dumpMask, &defaultConfig); cliPrint("\r\n# adjrange\r\n"); - cliAdjustmentRange(""); + printAdjustmentRange(dumpMask, &defaultConfig); cliPrintf("\r\n# rxrange\r\n"); - cliRxRange(""); + printRxRange(dumpMask, &defaultConfig); #ifdef USE_SERVOS cliPrint("\r\n# servo\r\n"); - cliServo(""); - - // print servo directions - unsigned int channel; - - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { - if (servoDirection(i, channel) < 0) { - cliPrintf("smix reverse %d %d r\r\n", i , channel); - } - } - } + printServo(dumpMask, &defaultConfig); #endif #ifdef VTX cliPrint("\r\n# vtx\r\n"); - cliVtx(""); + printVtx(dumpMask, &defaultConfig); #endif - cliPrint("\r\n# master\r\n"); - dumpValues(MASTER_VALUE); - cliPrint("\r\n# rxfail\r\n"); - cliRxFail(""); + printRxFail(dumpMask, &defaultConfig); + + cliPrint("\r\n# master\r\n"); + dumpValues(MASTER_VALUE, dumpMask, &defaultConfig); if (dumpMask & DUMP_ALL) { uint8_t activeProfile = masterConfig.current_profile_index; uint8_t profileCount; for (profileCount=0; profileCountactiveRateProfile; uint8_t rateCount; for (rateCount=0; rateCountactiveRateProfile); + cliDumpProfile(masterConfig.current_profile_index, dumpMask, &defaultConfig); + cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig); } } if (dumpMask & DUMP_PROFILE) { - cliDumpProfile(masterConfig.current_profile_index); + cliDumpProfile(masterConfig.current_profile_index, dumpMask, &defaultConfig); } if (dumpMask & DUMP_RATES) { - cliDumpRateProfile(currentProfile->activeRateProfile); + cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig); } } -void cliDumpProfile(uint8_t profileIndex) +static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, master_t *defaultConfig) { if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values return; + changeProfile(profileIndex); cliPrint("\r\n# profile\r\n"); cliProfile(""); + printSectionBreak(); - dumpValues(PROFILE_VALUE); + dumpValues(PROFILE_VALUE, dumpMask, defaultConfig); + + cliRateProfile(""); } -void cliDumpRateProfile(uint8_t rateProfileIndex) +static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, master_t *defaultConfig) { if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values return; @@ -2261,7 +2538,8 @@ void cliDumpRateProfile(uint8_t rateProfileIndex) cliPrint("\r\n# rateprofile\r\n"); cliRateProfile(""); printSectionBreak(); - dumpValues(PROFILE_RATE_VALUE); + + dumpValues(PROFILE_RATE_VALUE, dumpMask, defaultConfig); } void cliEnter(serialPort_t *serialPort) @@ -2575,18 +2853,6 @@ static void cliMotor(char *cmdline) cliPrintf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]); } -static void cliName(char *cmdline) -{ - uint32_t len = strlen(cmdline); - if (len > 0) { - memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); - strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); - } - cliPrintf("name %s\r\n", strlen(masterConfig.name) > 0 ? masterConfig.name : "-"); - - return; -} - static void cliPlaySound(char *cmdline) { #if FLASH_SIZE <= 64 @@ -2710,6 +2976,26 @@ static void cliPutp(void *p, char ch) bufWriterAppend(p, ch); } +static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...) +{ + bool printValue = !((dumpMask & DO_DIFF) && equalsDefault); + bool printComment = (dumpMask & DIFF_COMMENTED); + if (printValue || printComment) { + if (printComment && !printValue) { + cliWrite('#'); + } + + va_list va; + va_start(va, format); + tfp_format(cliWriter, cliPutp, format, va); + va_end(va); + + return true; + } + + return false; +} + static void cliPrintf(const char *fmt, ...) { va_list va; @@ -2732,14 +3018,7 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full) int32_t value = 0; char buf[8]; - void *ptr = var->ptr; - if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); - } - - if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); - } + void *ptr = getValuePointer(var); switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: @@ -3143,13 +3422,14 @@ void cliProcess(void) cliBuffer[bufferIndex] = 0; // null terminate const clicmd_t *cmd; + char *options; for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) { - if(!strncasecmp(cliBuffer, cmd->name, strlen(cmd->name)) // command names match - && !isalnum((unsigned)cliBuffer[strlen(cmd->name)])) // next characted in bufffer is not alphanumeric (command is correctly terminated) + if ((options = checkCommand(cliBuffer, cmd->name))) { break; + } } if(cmd < cmdTable + CMD_COUNT) - cmd->func(cliBuffer + strlen(cmd->name) + 1); + cmd->func(options); else cliPrint("Unknown command, try 'help'"); bufferIndex = 0; diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 2c61276d16..342e4c0692 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -91,7 +91,7 @@ #define VBAT_ADC_PIN PA0 #define RSSI_ADC_PIN PB0 -#define LED_STRIP +//#define LED_STRIP #define WS2811_PIN PB4 #define WS2811_TIMER TIM3 #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 @@ -103,20 +103,20 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define SONAR -#define SONAR_ECHO_PIN PB0 -#define SONAR_TRIGGER_PIN PB5 +//#define SONAR +//#define SONAR_ECHO_PIN PB0 +//#define SONAR_TRIGGER_PIN PB5 #undef GPS #ifdef CC3D_OPBL -//#undef LED_STRIP +#undef LED_STRIP #define SKIP_CLI_COMMAND_HELP #define SKIP_PID_FLOAT #undef BARO #undef MAG #undef SONAR -#undef LED_STRIP +#undef USE_SOFTSERIAL1 #endif #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 7e3705b47a..cb19b4a8a0 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -110,7 +110,7 @@ #define SONAR_TRIGGER_PIN_PWM PB8 #define SONAR_ECHO_PIN_PWM PB9 -#define DISPLAY +//#define DISPLAY #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 9f0239d572..545674aa14 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -81,13 +81,13 @@ #define USE_FLASHTOOLS #define USE_FLASH_M25P16 -#define SONAR +//#define SONAR #define SONAR_TRIGGER_PIN PB0 #define SONAR_ECHO_PIN PB1 #define SONAR_TRIGGER_PIN_PWM PB8 #define SONAR_ECHO_PIN_PWM PB9 -#define DISPLAY +//#define DISPLAY #define USE_UART1 #define USE_UART2 @@ -115,6 +115,12 @@ #define RSSI_ADC_PIN PA1 #define EXTERNAL1_ADC_PIN PA5 +//#define LED_STRIP +//#define LED_STRIP_TIMER TIM3 + +#define SKIP_CLI_COMMAND_HELP +#define SKIP_PID_LUXFLOAT + #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f103RCT6 in 64pin package From 7f96eafc2d4291d0a7648b61cf66537c8433af16 Mon Sep 17 00:00:00 2001 From: Maxim Khomenko Date: Mon, 8 Aug 2016 23:37:34 +0300 Subject: [PATCH 272/456] Revert "Some reworking of INDICATOR's and RING's functionality." This reverts commit 2f65e0d7f72e43b93e007c605531800d52215716. --- src/main/io/ledstrip.c | 112 +++++++++++++++++++++++++++++------------ 1 file changed, 81 insertions(+), 31 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 1fd4bae7a7..701d29a0a5 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -558,6 +558,42 @@ void applyDirectionalModeColor(const uint8_t ledIndex, const ledConfig_t *ledCon } +typedef enum { + QUADRANT_NORTH_EAST = 1, + QUADRANT_SOUTH_EAST, + QUADRANT_SOUTH_WEST, + QUADRANT_NORTH_WEST +} quadrant_e; + +void applyQuadrantColor(const uint8_t ledIndex, const ledConfig_t *ledConfig, const quadrant_e quadrant, const hsvColor_t *color) +{ + switch (quadrant) { + case QUADRANT_NORTH_EAST: + if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) >= lowestXValueForEast) { + setLedHsv(ledIndex, color); + } + return; + + case QUADRANT_SOUTH_EAST: + if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) >= lowestXValueForEast) { + setLedHsv(ledIndex, color); + } + return; + + case QUADRANT_SOUTH_WEST: + if (GET_LED_Y(ledConfig) >= lowestYValueForSouth && GET_LED_X(ledConfig) <= highestXValueForWest) { + setLedHsv(ledIndex, color); + } + return; + + case QUADRANT_NORTH_WEST: + if (GET_LED_Y(ledConfig) <= highestYValueForNorth && GET_LED_X(ledConfig) <= highestXValueForWest) { + setLedHsv(ledIndex, color); + } + return; + } +} + void applyLedModeLayer(void) { const ledConfig_t *ledConfig; @@ -677,24 +713,18 @@ void applyLedWarningLayer(uint8_t updateNow) void applyLedIndicatorLayer(uint8_t indicatorFlashState) { const ledConfig_t *ledConfig; - static const hsvColor_t *turnColor; - static const hsvColor_t *accColor; - static const hsvColor_t *decColor; + static const hsvColor_t *flashColor; if (!rxIsReceivingSignal()) { return; } if (indicatorFlashState == 0) { - turnColor = &hsv_orange; - accColor = &hsv_blue; - decColor = &hsv_red; + flashColor = &hsv_orange; } else { - turnColor = &hsv_black; - accColor = &hsv_black; - decColor = &hsv_black; + flashColor = &hsv_black; } - + uint8_t ledIndex; for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { @@ -704,31 +734,25 @@ void applyLedIndicatorLayer(uint8_t indicatorFlashState) if (!(ledConfig->flags & LED_FUNCTION_INDICATOR)) { continue; } - + if (rcCommand[ROLL] > INDICATOR_DEADBAND) { - if ((ledConfig->flags & LED_DIRECTION_EAST)) { - setLedHsv(ledIndex, turnColor); - continue; - } + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); } if (rcCommand[ROLL] < -INDICATOR_DEADBAND) { - if ((ledConfig->flags & LED_DIRECTION_WEST)) { - setLedHsv(ledIndex, turnColor); - continue; - } + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); } if (rcCommand[PITCH] > INDICATOR_DEADBAND) { - if ((ledConfig->flags & LED_DIRECTION_NORTH)) { - setLedHsv(ledIndex, accColor); - } + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_EAST, flashColor); + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_NORTH_WEST, flashColor); } if (rcCommand[PITCH] < -INDICATOR_DEADBAND) { - if ((ledConfig->flags & LED_DIRECTION_SOUTH)) { - setLedHsv(ledIndex, decColor); - } + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_EAST, flashColor); + applyQuadrantColor(ledIndex, ledConfig, QUADRANT_SOUTH_WEST, flashColor); } } } @@ -766,8 +790,11 @@ void applyLedThrustRingLayer(void) uint8_t ledIndex; // initialised to special value instead of using more memory for a flag. + static uint8_t rotationSeqLedCount = RING_PATTERN_NOT_CALCULATED; static uint8_t rotationPhase = ROTATION_SEQUENCE_LED_COUNT; + bool nextLedOn = false; + uint8_t ledRingIndex = 0; for (ledIndex = 0; ledIndex < ledCount; ledIndex++) { ledConfig = &ledConfigs[ledIndex]; @@ -776,15 +803,16 @@ void applyLedThrustRingLayer(void) continue; } - uint8_t ledRingIndex = GET_LED_X(ledConfig) + GET_LED_Y(ledConfig); bool applyColor = false; - if (ARMING_FLAG(ARMED)) { - if ((ledRingIndex + rotationPhase) % ROTATION_SEQUENCE_LED_COUNT < ROTATION_SEQUENCE_LED_WIDTH) { + if ((ledRingIndex + rotationPhase) % rotationSeqLedCount < ROTATION_SEQUENCE_LED_WIDTH) { applyColor = true; } } else { - applyColor = (ledRingIndex % 2) == 0; + if (nextLedOn == false) { + applyColor = true; + } + nextLedOn = !nextLedOn; } if (applyColor) { @@ -794,11 +822,33 @@ void applyLedThrustRingLayer(void) } setLedHsv(ledIndex, &ringColor); + + ledRingIndex++; } - + + uint8_t ledRingLedCount = ledRingIndex; + if (rotationSeqLedCount == RING_PATTERN_NOT_CALCULATED) { + // update ring pattern according to total number of ring leds found + + rotationSeqLedCount = ledRingLedCount; + + // try to split in segments/rings of exactly ROTATION_SEQUENCE_LED_COUNT leds + if ((ledRingLedCount % ROTATION_SEQUENCE_LED_COUNT) == 0) { + rotationSeqLedCount = ROTATION_SEQUENCE_LED_COUNT; + } else { + // else split up in equal segments/rings of at most ROTATION_SEQUENCE_LED_COUNT leds + while ((rotationSeqLedCount > ROTATION_SEQUENCE_LED_COUNT) && ((rotationSeqLedCount % 2) == 0)) { + rotationSeqLedCount >>= 1; + } + } + + // trigger start over + rotationPhase = 1; + } + rotationPhase--; if (rotationPhase == 0) { - rotationPhase = ROTATION_SEQUENCE_LED_COUNT; + rotationPhase = rotationSeqLedCount; } } From e9f99462195336ca7594ef3e13354de5e54bc6c9 Mon Sep 17 00:00:00 2001 From: Maxim Khomenko Date: Tue, 9 Aug 2016 00:15:44 +0300 Subject: [PATCH 273/456] Target for I-Shaped F3 FC from BG: http://www.banggood.com/I-shaped-F3-10DF-Flight-Controller-5V-2A-BEC-Bent-Straight-Pins-For-RC-Multirotors-p-1047005.html --- src/main/target/ISHAPEDF3/target.c | 121 +++++++++++++++++++++++++++ src/main/target/ISHAPEDF3/target.h | 122 ++++++++++++++++++++++++++++ src/main/target/ISHAPEDF3/target.mk | 11 +++ 3 files changed, 254 insertions(+) create mode 100644 src/main/target/ISHAPEDF3/target.c create mode 100644 src/main/target/ISHAPEDF3/target.h create mode 100644 src/main/target/ISHAPEDF3/target.mk diff --git a/src/main/target/ISHAPEDF3/target.c b/src/main/target/ISHAPEDF3/target.c new file mode 100644 index 0000000000..ec9176a27f --- /dev/null +++ b/src/main/target/ISHAPEDF3/target.c @@ -0,0 +1,121 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM15 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM16 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #10 + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), + PWM15 | (MAP_TO_SERVO_OUTPUT << 8), + PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // server #6 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N + // Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype. + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) + { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH6 - PB5 - *TIM3_CH2 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N + + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM3 - PA11 + { TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10 },// PWM4 - PA12 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM6 - PB9 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 + + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP +}; + diff --git a/src/main/target/ISHAPEDF3/target.h b/src/main/target/ISHAPEDF3/target.h new file mode 100644 index 0000000000..5efad550fd --- /dev/null +++ b/src/main/target/ISHAPEDF3/target.h @@ -0,0 +1,122 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "ISF3" + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + +#define LED0 PB3 + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 + +#define USE_EXTI +#define MPU_INT_EXTI PC13 +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + +#define GYRO +#define USE_GYRO_MPU6500 +//#define USE_GYRO_SPI_MPU6500 + +#define ACC +#define USE_ACC_MPU6500 +//#define USE_ACC_SPI_MPU6500 + +#define BARO +#define USE_BARO_BMP280 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define SONAR +#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_SOFTSERIAL1 +#define USE_SOFTSERIAL2 +#define SERIAL_PORT_COUNT 5 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA14 // PA14 / SWCLK +#define UART2_RX_PIN PA15 + +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) + +#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 +#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 +#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA + +#define USE_SPI +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 + +#define BOARD_HAS_VOLTAGE_DIVIDER +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 + +#define LED_STRIP +#define WS2811_PIN PA8 +#define WS2811_TIMER TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_BLACKBOX + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// IO - stm32f303cc in 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) + +#define USABLE_TIMER_CHANNEL_COUNT 17 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) + + diff --git a/src/main/target/ISHAPEDF3/target.mk b/src/main/target/ISHAPEDF3/target.mk new file mode 100644 index 0000000000..94676e07dc --- /dev/null +++ b/src/main/target/ISHAPEDF3/target.mk @@ -0,0 +1,11 @@ +F3_TARGETS += $(TARGET) +FEATURES = ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/barometer_bmp280.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/sonar_hcsr04.c From c3dd940107accbbf53ad2f93572da6b45df85028 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 10:32:51 +0200 Subject: [PATCH 274/456] README.md merged from development. Needs to be here on master too. --- README.md | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index 0253334eff..9b88d1fbe6 100644 --- a/README.md +++ b/README.md @@ -61,11 +61,9 @@ If what you need is not covered then refer to the baseflight documentation. If y ## IRC Support and Developers Channel -There's a dedicated IRC channel here: +There's a dedicated IRCgitter chat channel here: -irc://irc.freenode.net/#cleanflight - -If you are using windows and don't have an IRC client installed then take a look at HydraIRC - here: http://hydrairc.com/ +https://gitter.im/betaflight/betaflight Etiquette: Don't ask to ask and please wait around long enough for a reply - sometimes people are out flying, asleep or at work and can't answer immediately. @@ -79,13 +77,13 @@ Please subscribe and '+1' the videos if you find them useful. ## Configuration Tool -To configure Cleanflight you should use the Cleanflight-configurator GUI tool (Windows/OSX/Linux) that can be found here: +To configure Betaflight you should use the Betaflight-configurator GUI tool (Windows/OSX/Linux) that can be found here: -https://chrome.google.com/webstore/detail/cleanflight-configurator/enacoimjcgeinfnnnpajinjgmkahmfgb +https://chrome.google.com/webstore/detail/betaflight-configurator/kdaghagfopacdngbohiknlhcocjccjao The source for it is here: -https://github.com/cleanflight/cleanflight-configurator +https://github.com/betaflight/betaflight-configurator ## Contributing @@ -97,10 +95,10 @@ Contributions are welcome and encouraged. You can contribute in many ways: * New features. * Telling us your ideas and suggestions. -The best place to start is the IRC channel on freenode (see above), drop in, say hi. Next place is the github issue tracker: +The best place to start is the IRC channel on gitter (see above), drop in, say hi. Next place is the github issue tracker: -https://github.com/cleanflight/cleanflight/issues -https://github.com/cleanflight/cleanflight-configurator/issues +https://github.com/betaflight/betaflight/issues +https://github.com/betaflight/betaflight-configurator/issues Before creating new issues please check to see if there is an existing one, search first otherwise you waste peoples time when they could be coding instead! @@ -110,11 +108,11 @@ Please refer to the development section in the `docs/development` folder. TravisCI is used to run automatic builds -https://travis-ci.org/cleanflight/cleanflight +https://travis-ci.org/betaflight/betaflight -[![Build Status](https://travis-ci.org/cleanflight/cleanflight.svg?branch=master)](https://travis-ci.org/cleanflight/cleanflight) +[![Build Status](https://travis-ci.org/betaflight/betaflight.svg?branch=master)](https://travis-ci.org/betaflight/betaflight) -## Cleanflight Releases -https://github.com/cleanflight/cleanflight/releases +## Betaflight Releases +https://github.com/betaflight/betaflight/releases From f9354e53b3b41c71e0982ef65b01b26474d7af41 Mon Sep 17 00:00:00 2001 From: mikeller Date: Wed, 10 Aug 2016 00:00:40 +1200 Subject: [PATCH 275/456] Graft of 'cli_diff_command' into 'master'. --- src/main/config/config.c | 306 +++++++------ src/main/config/config.h | 1 - src/main/config/config_master.h | 2 + src/main/io/ledstrip.c | 3 - src/main/io/osd.c | 34 +- src/main/io/osd.h | 2 +- src/main/io/serial_cli.c | 706 +++++++++++++++++++++--------- src/main/target/CC3D/target.h | 12 +- src/main/target/NAZE/target.h | 2 +- src/main/target/PORT103R/target.h | 10 +- 10 files changed, 696 insertions(+), 382 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index dcb0ce85c3..b8b5cf11e0 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -273,7 +273,7 @@ void resetProfile(profile_t *profile) resetControlRateConfig(&profile->controlRateProfile[rI]); } - profile->activeRateProfile = 0; + profile->activeRateProfile = 0; } #ifdef GPS @@ -440,263 +440,280 @@ uint16_t getCurrentMinthrottle(void) return masterConfig.escAndServoConfig.minthrottle; } +static void intFeatureClearAll(master_t *config); +static void intFeatureSet(uint32_t mask, master_t *config); +static void intFeatureClear(uint32_t mask, master_t *config); + // Default settings -static void resetConf(void) +void createDefaultConfig(master_t *config) { // Clear all configuration - memset(&masterConfig, 0, sizeof(master_t)); - setProfile(0); + memset(config, 0, sizeof(master_t)); - featureClearAll(); - featureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES); + intFeatureClearAll(config); + intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES, config); #ifdef DEFAULT_FEATURES - featureSet(DEFAULT_FEATURES); + intFeatureSet(DEFAULT_FEATURES, config); #endif #ifdef OSD - resetOsdConfig(); + intFeatureSet(FEATURE_OSD, config); + resetOsdConfig(&config->osdProfile); #endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise // the user may see incorrect readings and unexpected issues with pin mappings may occur. - featureSet(FEATURE_VBAT); + intFeatureSet(FEATURE_VBAT, config); #endif - - masterConfig.version = EEPROM_CONF_VERSION; - masterConfig.mixerMode = MIXER_QUADX; + config->version = EEPROM_CONF_VERSION; + config->mixerMode = MIXER_QUADX; // global settings - masterConfig.current_profile_index = 0; // default profile - masterConfig.dcm_kp = 2500; // 1.0 * 10000 - masterConfig.dcm_ki = 0; // 0.003 * 10000 - masterConfig.gyro_lpf = 0; // 256HZ default + config->current_profile_index = 0; // default profile + config->dcm_kp = 2500; // 1.0 * 10000 + config->dcm_ki = 0; // 0.003 * 10000 + config->gyro_lpf = 0; // 256HZ default #ifdef STM32F10X - masterConfig.gyro_sync_denom = 8; - masterConfig.pid_process_denom = 1; + config->gyro_sync_denom = 8; + config->pid_process_denom = 1; #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) - masterConfig.gyro_sync_denom = 1; - masterConfig.pid_process_denom = 4; + config->gyro_sync_denom = 1; + config->pid_process_denom = 4; #else - masterConfig.gyro_sync_denom = 4; - masterConfig.pid_process_denom = 2; + config->gyro_sync_denom = 4; + config->pid_process_denom = 2; #endif - masterConfig.gyro_soft_type = FILTER_PT1; - masterConfig.gyro_soft_lpf_hz = 90; - masterConfig.gyro_soft_notch_hz = 0; - masterConfig.gyro_soft_notch_cutoff = 150; + config->gyro_soft_type = FILTER_PT1; + config->gyro_soft_lpf_hz = 90; + config->gyro_soft_notch_hz = 0; + config->gyro_soft_notch_cutoff = 150; - masterConfig.debug_mode = DEBUG_NONE; + config->debug_mode = DEBUG_NONE; - resetAccelerometerTrims(&masterConfig.accZero); + resetAccelerometerTrims(&config->accZero); - resetSensorAlignment(&masterConfig.sensorAlignmentConfig); + resetSensorAlignment(&config->sensorAlignmentConfig); - masterConfig.boardAlignment.rollDegrees = 0; - masterConfig.boardAlignment.pitchDegrees = 0; - masterConfig.boardAlignment.yawDegrees = 0; - masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect - masterConfig.max_angle_inclination = 700; // 70 degrees - masterConfig.yaw_control_direction = 1; - masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32; + config->boardAlignment.rollDegrees = 0; + config->boardAlignment.pitchDegrees = 0; + config->boardAlignment.yawDegrees = 0; + config->acc_hardware = ACC_DEFAULT; // default/autodetect + config->max_angle_inclination = 700; // 70 degrees + config->yaw_control_direction = 1; + config->gyroConfig.gyroMovementCalibrationThreshold = 32; // xxx_hardware: 0:default/autodetect, 1: disable - masterConfig.mag_hardware = 0; + config->mag_hardware = 0; - masterConfig.baro_hardware = 0; + config->baro_hardware = 0; - resetBatteryConfig(&masterConfig.batteryConfig); + resetBatteryConfig(&config->batteryConfig); #ifdef TELEMETRY - resetTelemetryConfig(&masterConfig.telemetryConfig); + resetTelemetryConfig(&config->telemetryConfig); #endif #ifdef SERIALRX_PROVIDER - masterConfig.rxConfig.serialrx_provider = SERIALRX_PROVIDER; + config->rxConfig.serialrx_provider = SERIALRX_PROVIDER; #else - masterConfig.rxConfig.serialrx_provider = 0; + config->rxConfig.serialrx_provider = 0; #endif - masterConfig.rxConfig.sbus_inversion = 1; - masterConfig.rxConfig.spektrum_sat_bind = 0; - masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; - masterConfig.rxConfig.midrc = 1500; - masterConfig.rxConfig.mincheck = 1100; - masterConfig.rxConfig.maxcheck = 1900; - masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection - masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection + config->rxConfig.sbus_inversion = 1; + config->rxConfig.spektrum_sat_bind = 0; + config->rxConfig.spektrum_sat_bind_autoreset = 1; + config->rxConfig.midrc = 1500; + config->rxConfig.mincheck = 1100; + config->rxConfig.maxcheck = 1900; + config->rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection + config->rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { - rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[i]; + rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &config->rxConfig.failsafe_channel_configurations[i]; channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD; - channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc); + channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.midrc); } - masterConfig.rxConfig.rssi_channel = 0; - masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; - masterConfig.rxConfig.rssi_ppm_invert = 0; - masterConfig.rxConfig.rcInterpolation = RC_SMOOTHING_AUTO; - masterConfig.rxConfig.rcInterpolationInterval = 19; - masterConfig.rxConfig.fpvCamAngleDegrees = 0; - masterConfig.rxConfig.max_aux_channel = MAX_AUX_CHANNELS; - masterConfig.rxConfig.airModeActivateThreshold = 1350; + config->rxConfig.rssi_channel = 0; + config->rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; + config->rxConfig.rssi_ppm_invert = 0; + config->rxConfig.rcInterpolation = RC_SMOOTHING_AUTO; + config->rxConfig.rcInterpolationInterval = 19; + config->rxConfig.fpvCamAngleDegrees = 0; + config->rxConfig.max_aux_channel = MAX_AUX_CHANNELS; + config->rxConfig.airModeActivateThreshold = 1350; - resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); + resetAllRxChannelRangeConfigurations(config->rxConfig.channelRanges); - masterConfig.inputFilteringMode = INPUT_FILTERING_DISABLED; + config->inputFilteringMode = INPUT_FILTERING_DISABLED; - masterConfig.gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support - masterConfig.disarm_kill_switch = 1; - masterConfig.auto_disarm_delay = 5; - masterConfig.small_angle = 25; + config->gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support + config->disarm_kill_switch = 1; + config->auto_disarm_delay = 5; + config->small_angle = 25; - resetMixerConfig(&masterConfig.mixerConfig); + resetMixerConfig(&config->mixerConfig); - masterConfig.airplaneConfig.fixedwing_althold_dir = 1; + config->airplaneConfig.fixedwing_althold_dir = 1; // Motor/ESC/Servo - resetEscAndServoConfig(&masterConfig.escAndServoConfig); - resetFlight3DConfig(&masterConfig.flight3DConfig); + resetEscAndServoConfig(&config->escAndServoConfig); + resetFlight3DConfig(&config->flight3DConfig); #ifdef BRUSHED_MOTORS - masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; - masterConfig.motor_pwm_protocol = PWM_TYPE_BRUSHED; - masterConfig.use_unsyncedPwm = true; + config->motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; + config->motor_pwm_protocol = PWM_TYPE_BRUSHED; + config->use_unsyncedPwm = true; #else - masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; - masterConfig.motor_pwm_protocol = PWM_TYPE_ONESHOT125; + config->motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; + config->motor_pwm_protocol = PWM_TYPE_ONESHOT125; #endif - masterConfig.servo_pwm_rate = 50; + + config->servo_pwm_rate = 50; #ifdef CC3D - masterConfig.use_buzzer_p6 = 0; + config->use_buzzer_p6 = 0; #endif #ifdef GPS // gps/nav stuff - masterConfig.gpsConfig.provider = GPS_NMEA; - masterConfig.gpsConfig.sbasMode = SBAS_AUTO; - masterConfig.gpsConfig.autoConfig = GPS_AUTOCONFIG_ON; - masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_OFF; + config->gpsConfig.provider = GPS_NMEA; + config->gpsConfig.sbasMode = SBAS_AUTO; + config->gpsConfig.autoConfig = GPS_AUTOCONFIG_ON; + config->gpsConfig.autoBaud = GPS_AUTOBAUD_OFF; #endif - resetSerialConfig(&masterConfig.serialConfig); + resetSerialConfig(&config->serialConfig); - masterConfig.emf_avoidance = 0; // TODO - needs removal + config->emf_avoidance = 0; // TODO - needs removal - resetProfile(currentProfile); + resetProfile(&config->profile[0]); - resetRollAndPitchTrims(&masterConfig.accelerometerTrims); + resetRollAndPitchTrims(&config->accelerometerTrims); - masterConfig.mag_declination = 0; - masterConfig.acc_lpf_hz = 10.0f; - masterConfig.accDeadband.xy = 40; - masterConfig.accDeadband.z = 40; - masterConfig.acc_unarmedcal = 1; + config->mag_declination = 0; + config->acc_lpf_hz = 10.0f; + config->accDeadband.xy = 40; + config->accDeadband.z = 40; + config->acc_unarmedcal = 1; #ifdef BARO - resetBarometerConfig(&masterConfig.barometerConfig); + resetBarometerConfig(&config->barometerConfig); #endif // Radio #ifdef RX_CHANNELS_TAER - parseRcChannels("TAER1234", &masterConfig.rxConfig); + parseRcChannels("TAER1234", &config->rxConfig); #else - parseRcChannels("AETR1234", &masterConfig.rxConfig); + parseRcChannels("AETR1234", &config->rxConfig); #endif - resetRcControlsConfig(&masterConfig.rcControlsConfig); + resetRcControlsConfig(&config->rcControlsConfig); - masterConfig.throttle_correction_value = 0; // could 10 with althold or 40 for fpv - masterConfig.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv + config->throttle_correction_value = 0; // could 10 with althold or 40 for fpv + config->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv // Failsafe Variables - masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec - masterConfig.failsafeConfig.failsafe_off_delay = 10; // 1sec - masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off. - masterConfig.failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss - masterConfig.failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition - masterConfig.failsafeConfig.failsafe_procedure = 0; // default full failsafe procedure is 0: auto-landing + config->failsafeConfig.failsafe_delay = 10; // 1sec + config->failsafeConfig.failsafe_off_delay = 10; // 1sec + config->failsafeConfig.failsafe_throttle = 1000; // default throttle off. + config->failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss + config->failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition + config->failsafeConfig.failsafe_procedure = 0; // default full failsafe procedure is 0: auto-landing #ifdef USE_SERVOS // servos for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - masterConfig.servoConf[i].min = DEFAULT_SERVO_MIN; - masterConfig.servoConf[i].max = DEFAULT_SERVO_MAX; - masterConfig.servoConf[i].middle = DEFAULT_SERVO_MIDDLE; - masterConfig.servoConf[i].rate = 100; - masterConfig.servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE; - masterConfig.servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; - masterConfig.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; + config->servoConf[i].min = DEFAULT_SERVO_MIN; + config->servoConf[i].max = DEFAULT_SERVO_MAX; + config->servoConf[i].middle = DEFAULT_SERVO_MIDDLE; + config->servoConf[i].rate = 100; + config->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE; + config->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; + config->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } // gimbal - masterConfig.gimbalConfig.mode = GIMBAL_MODE_NORMAL; + config->gimbalConfig.mode = GIMBAL_MODE_NORMAL; #endif #ifdef GPS - resetGpsProfile(&masterConfig.gpsProfile); + resetGpsProfile(&config->gpsProfile); #endif // custom mixer. clear by defaults. for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - masterConfig.customMotorMixer[i].throttle = 0.0f; + config->customMotorMixer[i].throttle = 0.0f; } #ifdef LED_STRIP - applyDefaultColors(masterConfig.colors); - applyDefaultLedStripConfig(masterConfig.ledConfigs); - applyDefaultModeColors(masterConfig.modeColors); - applyDefaultSpecialColors(&(masterConfig.specialColors)); - masterConfig.ledstrip_visual_beeper = 0; + applyDefaultColors(config->colors); + applyDefaultLedStripConfig(config->ledConfigs); + applyDefaultModeColors(config->modeColors); + applyDefaultSpecialColors(&(config->specialColors)); + config->ledstrip_visual_beeper = 0; #endif #ifdef VTX - masterConfig.vtx_band = 4; //Fatshark/Airwaves - masterConfig.vtx_channel = 1; //CH1 - masterConfig.vtx_mode = 0; //CH+BAND mode - masterConfig.vtx_mhz = 5740; //F0 + config->vtx_band = 4; //Fatshark/Airwaves + config->vtx_channel = 1; //CH1 + config->vtx_mode = 0; //CH+BAND mode + config->vtx_mhz = 5740; //F0 #endif #ifdef TRANSPONDER static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware - memcpy(masterConfig.transponderData, &defaultTransponderData, sizeof(defaultTransponderData)); + memcpy(config->transponderData, &defaultTransponderData, sizeof(defaultTransponderData)); #endif #ifdef BLACKBOX - #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) - featureSet(FEATURE_BLACKBOX); - masterConfig.blackbox_device = BLACKBOX_DEVICE_FLASH; + intFeatureSet(FEATURE_BLACKBOX, config); + config->blackbox_device = BLACKBOX_DEVICE_FLASH; #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT) - featureSet(FEATURE_BLACKBOX); - masterConfig.blackbox_device = BLACKBOX_DEVICE_SDCARD; + intFeatureSet(FEATURE_BLACKBOX, config); + config->blackbox_device = BLACKBOX_DEVICE_SDCARD; #else - masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL; + config->blackbox_device = BLACKBOX_DEVICE_SERIAL; #endif - masterConfig.blackbox_rate_num = 1; - masterConfig.blackbox_rate_denom = 1; - + config->blackbox_rate_num = 1; + config->blackbox_rate_denom = 1; #endif // BLACKBOX #ifdef SERIALRX_UART if (featureConfigured(FEATURE_RX_SERIAL)) { - masterConfig.serialConfig.portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL; + config->serialConfig.portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL; } #endif #if defined(TARGET_CONFIG) - targetConfiguration(); + // Don't look at target specific config settings when getting default + // values for a CLI diff. This is suboptimal, but it doesn't break the + // public API + if (config == &masterConfig) { + targetConfiguration(); + } #endif // copy first profile into remaining profile for (int i = 1; i < MAX_PROFILE_COUNT; i++) { - memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); + memcpy(&config->profile[i], &config->profile[0], sizeof(profile_t)); } +} +static void resetConf(void) +{ + createDefaultConfig(&masterConfig); + + setProfile(0); + +#ifdef LED_STRIP + reevaluateLedConfig(); +#endif } static uint8_t calculateChecksum(const uint8_t *data, uint32_t length) @@ -1067,17 +1084,32 @@ bool feature(uint32_t mask) void featureSet(uint32_t mask) { - masterConfig.enabledFeatures |= mask; + intFeatureSet(mask, &masterConfig); +} + +static void intFeatureSet(uint32_t mask, master_t *config) +{ + config->enabledFeatures |= mask; } void featureClear(uint32_t mask) { - masterConfig.enabledFeatures &= ~(mask); + intFeatureClear(mask, &masterConfig); +} + +static void intFeatureClear(uint32_t mask, master_t *config) +{ + config->enabledFeatures &= ~(mask); } void featureClearAll() { - masterConfig.enabledFeatures = 0; + intFeatureClearAll(&masterConfig); +} + +static void intFeatureClearAll(master_t *config) +{ + config->enabledFeatures = 0; } uint32_t featureMask(void) diff --git a/src/main/config/config.h b/src/main/config/config.h index cc239da923..6570440c35 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -26,7 +26,6 @@ #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 #define MAX_NAME_LENGTH 16 - typedef enum { FEATURE_RX_PPM = 1 << 0, FEATURE_VBAT = 1 << 1, diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 9e37154d1b..0f79ee2f44 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -174,3 +174,5 @@ typedef struct master_t { extern master_t masterConfig; extern profile_t *currentProfile; extern controlRateConfig_t *currentControlRateProfile; + +void createDefaultConfig(master_t *config); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 573d562374..b707a2a017 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -1223,9 +1223,6 @@ void pgResetFn_specialColors(specialColorIndexes_t *instance) void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) { memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t)); - memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); - - reevaluateLedConfig(); } void applyDefaultColors(hsvColor_t *colors) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index abd862dd13..e7e02eac9b 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -831,24 +831,22 @@ void osdInit(void) max7456_draw_screen(); } -void resetOsdConfig(void) +void resetOsdConfig(osd_profile *osdProfile) { - featureSet(FEATURE_OSD); - masterConfig.osdProfile.video_system = AUTO; - masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; - masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] = -59; - masterConfig.osdProfile.item_pos[OSD_TIMER] = -39; - masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS] = -9; - masterConfig.osdProfile.item_pos[OSD_CPU_LOAD] = 26; - masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL] = 1; - masterConfig.osdProfile.item_pos[OSD_VOLTAGE_WARNING] = -80; - masterConfig.osdProfile.item_pos[OSD_ARMED] = -107; - masterConfig.osdProfile.item_pos[OSD_DISARMED] = -109; - masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON] = -1; - masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS] = -1; - masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW] = -23; - masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] = -18; - masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME] = 1; + osdProfile->video_system = AUTO; + osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = -29; + osdProfile->item_pos[OSD_RSSI_VALUE] = -59; + osdProfile->item_pos[OSD_TIMER] = -39; + osdProfile->item_pos[OSD_THROTTLE_POS] = -9; + osdProfile->item_pos[OSD_CPU_LOAD] = 26; + osdProfile->item_pos[OSD_VTX_CHANNEL] = 1; + osdProfile->item_pos[OSD_VOLTAGE_WARNING] = -80; + osdProfile->item_pos[OSD_ARMED] = -107; + osdProfile->item_pos[OSD_DISARMED] = -109; + osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = -1; + osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = -1; + osdProfile->item_pos[OSD_CURRENT_DRAW] = -23; + osdProfile->item_pos[OSD_MAH_DRAWN] = -18; + osdProfile->item_pos[OSD_CRAFT_NAME] = 1; } - #endif diff --git a/src/main/io/osd.h b/src/main/io/osd.h index b5f4e953db..0685f9897f 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -65,4 +65,4 @@ typedef struct { void updateOsd(void); void osdInit(void); -void resetOsdConfig(void); +void resetOsdConfig(osd_profile *osdProfile); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 463ffb72da..179b187791 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -114,8 +114,10 @@ static void cliMotorMix(char *cmdline); static void cliDefaults(char *cmdline); void cliDfu(char *cmdLine); static void cliDump(char *cmdLine); -void cliDumpProfile(uint8_t profileIndex); -void cliDumpRateProfile(uint8_t rateProfileIndex) ; +static void cliDiff(char *cmdLine); +static void printConfig(char *cmdLine, bool doDiff); +static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, master_t *defaultProfile); +static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, master_t *defaultProfile) ; static void cliExit(char *cmdline); static void cliFeature(char *cmdline); static void cliMotor(char *cmdline); @@ -189,6 +191,17 @@ static void cliBeeper(char *cmdline); static char cliBuffer[48]; static uint32_t bufferIndex = 0; +typedef enum { + DUMP_MASTER = (1 << 0), + DUMP_PROFILE = (1 << 1), + DUMP_RATES = (1 << 2), + DUMP_ALL = (1 << 3), + DO_DIFF = (1 << 4), + DIFF_COMMENTED = (1 << 5), +} dumpFlags_e; + +static const char* const sectionBreak = "\r\n"; + #ifndef USE_QUAD_MIXER_ONLY // sync this with mixerMode_e static const char * const mixerNames[] = { @@ -269,7 +282,10 @@ const clicmd_t cmdTable[] = { #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), - CLI_COMMAND_DEF("dump", "dump configuration", "[master|profile]", cliDump), + CLI_COMMAND_DEF("diff", "list configuration changes from default", + "[master|profile|rates|all] {commented}", cliDiff), + CLI_COMMAND_DEF("dump", "dump configuration", + "[master|profile|rates|all]", cliDump), CLI_COMMAND_DEF("exit", NULL, NULL, cliExit), CLI_COMMAND_DEF("feature", "configure features", "list\r\n" @@ -923,6 +939,11 @@ static void cliPrint(const char *str); static void cliPrintf(const char *fmt, ...); static void cliWrite(uint8_t ch); +#define printSectionBreak() cliPrintf((char *)sectionBreak) + +#define COMPARE_CONFIG(value) (masterConfig.value == defaultConfig.value) +static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...); + static void cliPrompt(void) { cliPrint("\r\n# "); @@ -968,6 +989,36 @@ static bool isEmpty(const char *string) return *string == '\0'; } +static void printRxFail(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out rxConfig failsafe settings + rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration; + rxFailsafeChannelConfiguration_t *channelFailsafeConfigurationDefault; + bool equalsDefault; + bool requireValue; + char modeCharacter; + for (uint8_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { + channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[channel]; + channelFailsafeConfigurationDefault = &defaultConfig->rxConfig.failsafe_channel_configurations[channel]; + equalsDefault = channelFailsafeConfiguration->mode == channelFailsafeConfigurationDefault->mode + && channelFailsafeConfiguration->step == channelFailsafeConfigurationDefault->step; + requireValue = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET; + modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfiguration->mode]; + if (requireValue) { + cliDumpPrintf(dumpMask, equalsDefault, "rxfail %u %c %d\r\n", + channel, + modeCharacter, + RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step) + ); + } else { + cliDumpPrintf(dumpMask, equalsDefault, "rxfail %u %c\r\n", + channel, + modeCharacter + ); + } + } +} + static void cliRxFail(char *cmdline) { uint8_t channel; @@ -1030,10 +1081,9 @@ static void cliRxFail(char *cmdline) char modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfiguration->mode]; - // triple use of cliPrintf below + // double use of cliPrintf below // 1. acknowledge interpretation on command, // 2. query current setting on single item, - // 3. recursive use for full list. if (requireValue) { cliPrintf("rxfail %u %c %d\r\n", @@ -1053,23 +1103,36 @@ static void cliRxFail(char *cmdline) } } +static void printAux(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out aux channel settings + modeActivationCondition_t *mac; + modeActivationCondition_t *macDefault; + bool equalsDefault; + for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + mac = &masterConfig.modeActivationConditions[i]; + macDefault = &defaultConfig->modeActivationConditions[i]; + equalsDefault = mac->modeId == macDefault->modeId + && mac->auxChannelIndex == macDefault->auxChannelIndex + && mac->range.startStep == macDefault->range.startStep + && mac->range.endStep == macDefault->range.endStep; + cliDumpPrintf(dumpMask, equalsDefault, "aux %u %u %u %u %u\r\n", + i, + mac->modeId, + mac->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep) + ); + } +} + static void cliAux(char *cmdline) { int i, val = 0; char *ptr; if (isEmpty(cmdline)) { - // print out aux channel settings - for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { - modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; - cliPrintf("aux %u %u %u %u %u\r\n", - i, - mac->modeId, - mac->auxChannelIndex, - MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep) - ); - } + printAux(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr++); @@ -1103,28 +1166,44 @@ static void cliAux(char *cmdline) } } +static void printSerial(uint8_t dumpMask, master_t *defaultConfig) +{ + serialConfig_t *serialConfig; + serialConfig_t *serialConfigDefault; + bool equalsDefault; + for (int i = 0; i < SERIAL_PORT_COUNT; i++) { + serialConfig = &masterConfig.serialConfig; + if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) { + continue; + }; + serialConfigDefault = &defaultConfig->serialConfig; + equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier + && serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask + && serialConfig->portConfigs[i].msp_baudrateIndex == serialConfigDefault->portConfigs[i].msp_baudrateIndex + && serialConfig->portConfigs[i].gps_baudrateIndex == serialConfigDefault->portConfigs[i].gps_baudrateIndex + && serialConfig->portConfigs[i].telemetry_baudrateIndex == serialConfigDefault->portConfigs[i].telemetry_baudrateIndex + && serialConfig->portConfigs[i].blackbox_baudrateIndex == serialConfigDefault->portConfigs[i].blackbox_baudrateIndex; + cliDumpPrintf(dumpMask, equalsDefault, "serial %d %d %ld %ld %ld %ld\r\n" , + serialConfig->portConfigs[i].identifier, + serialConfig->portConfigs[i].functionMask, + baudRates[serialConfig->portConfigs[i].msp_baudrateIndex], + baudRates[serialConfig->portConfigs[i].gps_baudrateIndex], + baudRates[serialConfig->portConfigs[i].telemetry_baudrateIndex], + baudRates[serialConfig->portConfigs[i].blackbox_baudrateIndex] + ); + } +} + static void cliSerial(char *cmdline) { int i, val; char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < SERIAL_PORT_COUNT; i++) { - if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) { - continue; - }; - cliPrintf("serial %d %d %ld %ld %ld %ld\r\n" , - masterConfig.serialConfig.portConfigs[i].identifier, - masterConfig.serialConfig.portConfigs[i].functionMask, - baudRates[masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex], - baudRates[masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex], - baudRates[masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex], - baudRates[masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex] - ); - } - return; - } + printSerial(DUMP_MASTER, &masterConfig); + return; + } serialPortConfig_t portConfig; memset(&portConfig, 0 , sizeof(portConfig)); @@ -1270,25 +1349,40 @@ static void cliSerialPassthrough(char *cmdline) } #endif +static void printAdjustmentRange(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out adjustment ranges channel settings + adjustmentRange_t *ar; + adjustmentRange_t *arDefault; + bool equalsDefault; + for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { + ar = &masterConfig.adjustmentRanges[i]; + arDefault = &defaultConfig->adjustmentRanges[i]; + equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex + && ar->range.startStep == arDefault->range.startStep + && ar->range.endStep == arDefault->range.endStep + && ar->adjustmentFunction == arDefault->adjustmentFunction + && ar->auxSwitchChannelIndex == arDefault->auxSwitchChannelIndex + && ar->adjustmentIndex == arDefault->adjustmentIndex; + cliDumpPrintf(dumpMask, equalsDefault, "adjrange %u %u %u %u %u %u %u\r\n", + i, + ar->adjustmentIndex, + ar->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(ar->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(ar->range.endStep), + ar->adjustmentFunction, + ar->auxSwitchChannelIndex + ); + } +} + static void cliAdjustmentRange(char *cmdline) { int i, val = 0; char *ptr; if (isEmpty(cmdline)) { - // print out adjustment ranges channel settings - for (i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { - adjustmentRange_t *ar = &masterConfig.adjustmentRanges[i]; - cliPrintf("adjrange %u %u %u %u %u %u %u\r\n", - i, - ar->adjustmentIndex, - ar->auxChannelIndex, - MODE_STEP_TO_CHANNEL_VALUE(ar->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(ar->range.endStep), - ar->adjustmentFunction, - ar->auxSwitchChannelIndex - ); - } + printAdjustmentRange(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr++); @@ -1423,16 +1517,27 @@ static void cliMotorMix(char *cmdline) #endif } +static void printRxRange(uint8_t dumpMask, master_t *defaultConfig) +{ + rxChannelRangeConfiguration_t *channelRangeConfiguration; + rxChannelRangeConfiguration_t *channelRangeConfigurationDefault; + bool equalsDefault; + for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { + channelRangeConfiguration = &masterConfig.rxConfig.channelRanges[i]; + channelRangeConfigurationDefault = &defaultConfig->rxConfig.channelRanges[i]; + equalsDefault = channelRangeConfiguration->min == channelRangeConfigurationDefault->min + && channelRangeConfiguration->max == channelRangeConfigurationDefault->max; + cliDumpPrintf(dumpMask, equalsDefault, "rxrange %u %u %u\r\n", i, channelRangeConfiguration->min, channelRangeConfiguration->max); + } +} + static void cliRxRange(char *cmdline) { int i, validArgumentCount = 0; char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { - rxChannelRangeConfiguration_t *channelRangeConfiguration = &masterConfig.rxConfig.channelRanges[i]; - cliPrintf("rxrange %u %u %u\r\n", i, channelRangeConfiguration->min, channelRangeConfiguration->max); - } + printRxRange(DUMP_MASTER, &masterConfig); } else if (strcasecmp(cmdline, "reset") == 0) { resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); } else { @@ -1469,17 +1574,28 @@ static void cliRxRange(char *cmdline) } #ifdef LED_STRIP +static void printLed(uint8_t dumpMask, master_t *defaultConfig) +{ + bool equalsDefault; + ledConfig_t ledConfig; + ledConfig_t ledConfigDefault; + char ledConfigBuffer[20]; + for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) { + ledConfig = masterConfig.ledConfigs[i]; + ledConfigDefault = defaultConfig->ledConfigs[i]; + equalsDefault = ledConfig == ledConfigDefault; + generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); + cliDumpPrintf(dumpMask, equalsDefault, "led %u %s\r\n", i, ledConfigBuffer); + } +} + static void cliLed(char *cmdline) { int i; char *ptr; - char ledConfigBuffer[20]; if (isEmpty(cmdline)) { - for (i = 0; i < LED_MAX_STRIP_LENGTH; i++) { - generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); - cliPrintf("led %u %s\r\n", i, ledConfigBuffer); - } + printLed(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr); @@ -1494,20 +1610,33 @@ static void cliLed(char *cmdline) } } +static void printColor(uint8_t dumpMask, master_t *defaultConfig) +{ + hsvColor_t *color; + hsvColor_t *colorDefault; + bool equalsDefault; + for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { + color = &masterConfig.colors[i]; + colorDefault = &defaultConfig->colors[i]; + equalsDefault = color->h == colorDefault->h + && color->s == colorDefault->s + && color->v == colorDefault->v; + cliDumpPrintf(dumpMask, equalsDefault, "color %u %d,%u,%u\r\n", + i, + color->h, + color->s, + color->v + ); + } +} + static void cliColor(char *cmdline) { int i; char *ptr; if (isEmpty(cmdline)) { - for (i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { - cliPrintf("color %u %d,%u,%u\r\n", - i, - masterConfig.colors[i].h, - masterConfig.colors[i].s, - masterConfig.colors[i].v - ); - } + printColor(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr); @@ -1522,20 +1651,27 @@ static void cliColor(char *cmdline) } } +static void printModeColor(uint8_t dumpMask, master_t *defaultConfig) +{ + for (int i = 0; i < LED_MODE_COUNT; i++) { + for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + int colorIndex = masterConfig.modeColors[i].color[j]; + int colorIndexDefault = defaultConfig->modeColors[i].color[j]; + cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, "mode_color %u %u %u\r\n", i, j, colorIndex); + } + } + + for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + int colorIndex = masterConfig.specialColors.color[j]; + int colorIndexDefault = defaultConfig->specialColors.color[j]; + cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, "mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex); + } +} + static void cliModeColor(char *cmdline) { if (isEmpty(cmdline)) { - for (int i = 0; i < LED_MODE_COUNT; i++) { - for (int j = 0; j < LED_DIRECTION_COUNT; j++) { - int colorIndex = modeColors[i].color[j]; - cliPrintf("mode_color %u %u %u\r\n", i, j, colorIndex); - } - } - - for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { - int colorIndex = specialColors.color[j]; - cliPrintf("mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex); - } + printModeColor(DUMP_MASTER, &masterConfig); } else { enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; int args[ARGS_COUNT]; @@ -1565,6 +1701,52 @@ static void cliModeColor(char *cmdline) #endif #ifdef USE_SERVOS +static void printServo(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out servo settings + int i; + servoParam_t *servoConf; + servoParam_t *servoConfDefault; + bool equalsDefault; + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servoConf = &masterConfig.servoConf[i]; + servoConfDefault = &defaultConfig->servoConf[i]; + equalsDefault = servoConf->min == servoConfDefault->min + && servoConf->max == servoConfDefault->max + && servoConf->middle == servoConfDefault->middle + && servoConf->angleAtMin == servoConfDefault->angleAtMax + && servoConf->rate == servoConfDefault->rate + && servoConf->forwardFromChannel == servoConfDefault->forwardFromChannel; + + cliDumpPrintf(dumpMask, equalsDefault, "servo %u %d %d %d %d %d %d %d\r\n", + i, + servoConf->min, + servoConf->max, + servoConf->middle, + servoConf->angleAtMin, + servoConf->angleAtMax, + servoConf->rate, + servoConf->forwardFromChannel + ); + } + + printSectionBreak(); + + // print servo directions + unsigned int channel; + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servoConf = &masterConfig.servoConf[i]; + servoConfDefault = &defaultConfig->servoConf[i]; + equalsDefault = servoConf->reversedSources == servoConfDefault->reversedSources; + for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { + if (servoConf->reversedSources & (1 << channel)) { + equalsDefault = ~(servoConf->reversedSources ^ servoConfDefault->reversedSources) & (1 << channel); + cliDumpPrintf(dumpMask, equalsDefault, "smix reverse %d %d r\r\n", i , channel); + } + } + } +} + static void cliServo(char *cmdline) { enum { SERVO_ARGUMENT_COUNT = 8 }; @@ -1576,21 +1758,7 @@ static void cliServo(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - // print out servo settings - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo = &masterConfig.servoConf[i]; - - cliPrintf("servo %u %d %d %d %d %d %d %d\r\n", - i, - servo->min, - servo->max, - servo->middle, - servo->angleAtMin, - servo->angleAtMax, - servo->rate, - servo->forwardFromChannel - ); - } + printServo(DUMP_MASTER, &masterConfig); } else { int validArgumentCount = 0; @@ -1947,24 +2115,38 @@ static void cliFlashRead(char *cmdline) #endif #ifdef VTX +static void printVtx(uint8_t dumpMask, master_t *defaultConfig) +{ + // print out vtx channel settings + vtxChannelActivationCondition_t *cac; + vtxChannelActivationCondition_t *cacDefault; + bool equalsDefault; + for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { + cac = &masterConfig.vtxChannelActivationConditions[i]; + cacDefault = &defaultConfig->vtxChannelActivationConditions[i]; + equalsDefault = cac->auxChannelIndex == cacDefault->auxChannelIndex + && cac->band == cacDefault->band + && cac->channel == cacDefault->channel + && cac->range.startStep == cacDefault->range.startStep + && cac->range.endStep == cacDefault->range.endStep; + cliDumpPrintf(dumpMask, equalsDefault, "vtx %u %u %u %u %u %u\r\n", + i, + cac->auxChannelIndex, + cac->band, + cac->channel, + MODE_STEP_TO_CHANNEL_VALUE(cac->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(cac->range.endStep) + ); + } +} + static void cliVtx(char *cmdline) { int i, val = 0; char *ptr; if (isEmpty(cmdline)) { - // print out vtx channel settings - for (i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { - vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i]; - printf("vtx %u %u %u %u %u %u\r\n", - i, - cac->auxChannelIndex, - cac->band, - cac->channel, - MODE_STEP_TO_CHANNEL_VALUE(cac->range.startStep), - MODE_STEP_TO_CHANNEL_VALUE(cac->range.endStep) - ); - } + printVtx(DUMP_MASTER, &masterConfig); } else { ptr = cmdline; i = atoi(ptr++); @@ -2007,7 +2189,72 @@ static void cliVtx(char *cmdline) } #endif -static void dumpValues(uint16_t valueSection) +static void printName(uint8_t dumpMask) +{ + cliDumpPrintf(dumpMask, strlen(masterConfig.name) == 0, "name %s\r\n", strlen(masterConfig.name) > 0 ? masterConfig.name : "-"); +} + +static void cliName(char *cmdline) +{ + uint32_t len = strlen(cmdline); + if (len > 0) { + memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); + strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); + } + printName(DUMP_MASTER); +} + +void *getValuePointer(const clivalue_t *value) +{ + void *ptr = value->ptr; + + if ((value->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { + ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); + } + + if ((value->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { + ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); + } + + return ptr; +} + +static bool valueEqualsDefault(const clivalue_t *value, master_t *defaultConfig) +{ + void *ptr = getValuePointer(value); + + void *ptrDefault = ((uint8_t *)ptr) - (uint32_t)&masterConfig + (uint32_t)defaultConfig; + + bool result = false; + switch (value->type & VALUE_TYPE_MASK) { + case VAR_UINT8: + result = *(uint8_t *)ptr == *(uint8_t *)ptrDefault; + break; + + case VAR_INT8: + result = *(int8_t *)ptr == *(int8_t *)ptrDefault; + break; + + case VAR_UINT16: + result = *(uint16_t *)ptr == *(uint16_t *)ptrDefault; + break; + + case VAR_INT16: + result = *(int16_t *)ptr == *(int16_t *)ptrDefault; + break; + + case VAR_UINT32: + result = *(uint32_t *)ptr == *(uint32_t *)ptrDefault; + break; + + case VAR_FLOAT: + result = *(float *)ptr == *(float *)ptrDefault; + break; + } + return result; +} + +static void dumpValues(uint16_t valueSection, uint8_t dumpMask, master_t *defaultConfig) { uint32_t i; const clivalue_t *value; @@ -2018,64 +2265,83 @@ static void dumpValues(uint16_t valueSection) continue; } - cliPrintf("set %s = ", valueTable[i].name); - cliPrintVar(value, 0); - cliPrint("\r\n"); + if (cliDumpPrintf(dumpMask, valueEqualsDefault(value, defaultConfig), "set %s = ", valueTable[i].name)) { + cliPrintVar(value, 0); + cliPrint("\r\n"); + } } } -typedef enum { - DUMP_MASTER = (1 << 0), - DUMP_PROFILE = (1 << 1), - DUMP_RATES = (1 << 2), - DUMP_ALL = (1 << 3), -} dumpFlags_e; - -static const char* const sectionBreak = "\r\n"; - -#define printSectionBreak() cliPrintf((char *)sectionBreak) - static void cliDump(char *cmdline) +{ + printConfig(cmdline, false); +} + +static void cliDiff(char *cmdline) +{ + printConfig(cmdline, true); +} + +char *checkCommand(char *cmdLine, const char *command) +{ + if(!strncasecmp(cmdLine, command, strlen(command)) // command names match + && !isalnum((unsigned)cmdLine[strlen(command)])) { // next characted in bufffer is not alphanumeric (command is correctly terminated) + return cmdLine + strlen(command) + 1; + } else { + return 0; + } +} + +static void printConfig(char *cmdline, bool doDiff) { unsigned int i; char buf[16]; uint32_t mask; - -#ifndef USE_QUAD_MIXER_ONLY - float thr, roll, pitch, yaw; -#endif + uint32_t defaultMask; + bool equalsDefault; uint8_t dumpMask = DUMP_MASTER; - if (strcasecmp(cmdline, "master") == 0) { + char *options; + if ((options = checkCommand(cmdline, "master"))) { dumpMask = DUMP_MASTER; // only - } - if (strcasecmp(cmdline, "profile") == 0) { + } else if ((options = checkCommand(cmdline, "profile"))) { dumpMask = DUMP_PROFILE; // only - } - if (strcasecmp(cmdline, "rates") == 0) { - dumpMask = DUMP_RATES; + } else if ((options = checkCommand(cmdline, "rates"))) { + dumpMask = DUMP_RATES; // only + } else if ((options = checkCommand(cmdline, "all"))) { + dumpMask = DUMP_ALL; // all profiles and rates } - if (strcasecmp(cmdline, "all") == 0) { - dumpMask = DUMP_ALL; // All profiles and rates + master_t defaultConfig; + if (doDiff) { + dumpMask = dumpMask | DO_DIFF; + createDefaultConfig(&defaultConfig); + } + + if (checkCommand(options, "commented")) { + dumpMask = dumpMask | DIFF_COMMENTED; // add unchanged values as comments for diff } if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { - cliPrint("\r\n# version\r\n"); cliVersion(NULL); + cliPrint("\r\n"); + + if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) { + cliPrint("\r\n# reset configuration to default settings\r\ndefaults\r\n"); + } printSectionBreak(); - cliPrint("\r\n# name\r\n"); - cliName(NULL); + cliPrint("# name\r\n"); + printName(dumpMask); cliPrint("\r\n# mixer\r\n"); - #ifndef USE_QUAD_MIXER_ONLY - cliPrintf("mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]); + cliDumpPrintf(dumpMask, COMPARE_CONFIG(mixerMode), "mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]); - cliPrintf("mmix reset\r\n"); + cliDumpPrintf(dumpMask, masterConfig.customMotorMixer[0].throttle == 0.0f, "mmix reset\r\n"); + float thr, roll, pitch, yaw, thrDefault, rollDefault, pitchDefault, yawDefault; for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) { if (masterConfig.customMotorMixer[i].throttle == 0.0f) break; @@ -2083,31 +2349,45 @@ static void cliDump(char *cmdline) roll = masterConfig.customMotorMixer[i].roll; pitch = masterConfig.customMotorMixer[i].pitch; yaw = masterConfig.customMotorMixer[i].yaw; - cliPrintf("mmix %d", i); - if (thr < 0) - cliWrite(' '); - cliPrintf("%s", ftoa(thr, buf)); - if (roll < 0) - cliWrite(' '); - cliPrintf("%s", ftoa(roll, buf)); - if (pitch < 0) - cliWrite(' '); - cliPrintf("%s", ftoa(pitch, buf)); - if (yaw < 0) - cliWrite(' '); - cliPrintf("%s\r\n", ftoa(yaw, buf)); + thrDefault = defaultConfig.customMotorMixer[i].throttle; + rollDefault = defaultConfig.customMotorMixer[i].roll; + pitchDefault = defaultConfig.customMotorMixer[i].pitch; + yawDefault = defaultConfig.customMotorMixer[i].yaw; + equalsDefault = thr == thrDefault && roll == rollDefault && pitch == pitchDefault && yaw == yawDefault; + + if (cliDumpPrintf(dumpMask, equalsDefault, "mmix %d", i)) { + if (thr < 0) + cliWrite(' '); + cliPrintf("%s", ftoa(thr, buf)); + if (roll < 0) + cliWrite(' '); + cliPrintf("%s", ftoa(roll, buf)); + if (pitch < 0) + cliWrite(' '); + cliPrintf("%s", ftoa(pitch, buf)); + if (yaw < 0) + cliWrite(' '); + cliPrintf("%s\r\n", ftoa(yaw, buf)); + } } #ifdef USE_SERVOS // print custom servo mixer if exists - cliPrintf("smix reset\r\n"); + cliDumpPrintf(dumpMask, masterConfig.customServoMixer[i].rate == 0, "smix reset\r\n"); for (i = 0; i < MAX_SERVO_RULES; i++) { - if (masterConfig.customServoMixer[i].rate == 0) break; - cliPrintf("smix %d %d %d %d %d %d %d %d\r\n", + equalsDefault = masterConfig.customServoMixer[i].targetChannel == defaultConfig.customServoMixer[i].targetChannel + && masterConfig.customServoMixer[i].inputSource == defaultConfig.customServoMixer[i].inputSource + && masterConfig.customServoMixer[i].rate == defaultConfig.customServoMixer[i].rate + && masterConfig.customServoMixer[i].speed == defaultConfig.customServoMixer[i].speed + && masterConfig.customServoMixer[i].min == defaultConfig.customServoMixer[i].min + && masterConfig.customServoMixer[i].max == defaultConfig.customServoMixer[i].max + && masterConfig.customServoMixer[i].box == masterConfig.customServoMixer[i].box; + + cliDumpPrintf(dumpMask, equalsDefault,"smix %d %d %d %d %d %d %d %d\r\n", i, masterConfig.customServoMixer[i].targetChannel, masterConfig.customServoMixer[i].inputSource, @@ -2118,102 +2398,96 @@ static void cliDump(char *cmdline) masterConfig.customServoMixer[i].box ); } - #endif #endif cliPrint("\r\n# feature\r\n"); + mask = featureMask(); + defaultMask = defaultConfig.enabledFeatures; for (i = 0; ; i++) { // disable all feature first if (featureNames[i] == NULL) break; - cliPrintf("feature -%s\r\n", featureNames[i]); + cliDumpPrintf(dumpMask, (mask | ~defaultMask) & (1 << i), "feature -%s\r\n", featureNames[i]); } for (i = 0; ; i++) { // reenable what we want. if (featureNames[i] == NULL) break; if (mask & (1 << i)) - cliPrintf("feature %s\r\n", featureNames[i]); + cliDumpPrintf(dumpMask, defaultMask & (1 << i), "feature %s\r\n", featureNames[i]); } #ifdef BEEPER cliPrint("\r\n# beeper\r\n"); uint8_t beeperCount = beeperTableEntryCount(); mask = getBeeperOffMask(); + defaultMask = defaultConfig.beeper_off_flags; for (int i = 0; i < (beeperCount-2); i++) { if (mask & (1 << i)) - cliPrintf("beeper -%s\r\n", beeperNameForTableIndex(i)); + cliDumpPrintf(dumpMask, (~(mask ^ defaultMask) & (1 << i)), "beeper -%s\r\n", beeperNameForTableIndex(i)); else - cliPrintf("beeper %s\r\n", beeperNameForTableIndex(i)); + cliDumpPrintf(dumpMask, (~(mask ^ defaultMask) & (1 << i)), "beeper %s\r\n", beeperNameForTableIndex(i)); } #endif cliPrint("\r\n# map\r\n"); - for (i = 0; i < 8; i++) + equalsDefault = true; + for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; + equalsDefault = equalsDefault && (masterConfig.rxConfig.rcmap[i] == defaultConfig.rxConfig.rcmap[i]); + } buf[i] = '\0'; - cliPrintf("map %s\r\n", buf); + cliDumpPrintf(dumpMask, equalsDefault, "map %s\r\n", buf); cliPrint("\r\n# serial\r\n"); - cliSerial(""); + printSerial(dumpMask, &defaultConfig); #ifdef LED_STRIP cliPrint("\r\n# led\r\n"); - cliLed(""); + printLed(dumpMask, &defaultConfig); cliPrint("\r\n# color\r\n"); - cliColor(""); + printColor(dumpMask, &defaultConfig); cliPrint("\r\n# mode_color\r\n"); - cliModeColor(""); + printModeColor(dumpMask, &defaultConfig); #endif cliPrint("\r\n# aux\r\n"); - cliAux(""); + printAux(dumpMask, &defaultConfig); cliPrint("\r\n# adjrange\r\n"); - cliAdjustmentRange(""); + printAdjustmentRange(dumpMask, &defaultConfig); cliPrintf("\r\n# rxrange\r\n"); - cliRxRange(""); + printRxRange(dumpMask, &defaultConfig); #ifdef USE_SERVOS cliPrint("\r\n# servo\r\n"); - cliServo(""); - - // print servo directions - unsigned int channel; - - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { - if (servoDirection(i, channel) < 0) { - cliPrintf("smix reverse %d %d r\r\n", i , channel); - } - } - } + printServo(dumpMask, &defaultConfig); #endif #ifdef VTX cliPrint("\r\n# vtx\r\n"); - cliVtx(""); + printVtx(dumpMask, &defaultConfig); #endif - cliPrint("\r\n# master\r\n"); - dumpValues(MASTER_VALUE); - cliPrint("\r\n# rxfail\r\n"); - cliRxFail(""); + printRxFail(dumpMask, &defaultConfig); + + cliPrint("\r\n# master\r\n"); + dumpValues(MASTER_VALUE, dumpMask, &defaultConfig); if (dumpMask & DUMP_ALL) { uint8_t activeProfile = masterConfig.current_profile_index; uint8_t profileCount; for (profileCount=0; profileCountactiveRateProfile; uint8_t rateCount; for (rateCount=0; rateCountactiveRateProfile); + cliDumpProfile(masterConfig.current_profile_index, dumpMask, &defaultConfig); + cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig); } } if (dumpMask & DUMP_PROFILE) { - cliDumpProfile(masterConfig.current_profile_index); + cliDumpProfile(masterConfig.current_profile_index, dumpMask, &defaultConfig); } if (dumpMask & DUMP_RATES) { - cliDumpRateProfile(currentProfile->activeRateProfile); + cliDumpRateProfile(currentProfile->activeRateProfile, dumpMask, &defaultConfig); } } -void cliDumpProfile(uint8_t profileIndex) +static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, master_t *defaultConfig) { if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values return; + changeProfile(profileIndex); cliPrint("\r\n# profile\r\n"); cliProfile(""); + printSectionBreak(); - dumpValues(PROFILE_VALUE); + dumpValues(PROFILE_VALUE, dumpMask, defaultConfig); + + cliRateProfile(""); } -void cliDumpRateProfile(uint8_t rateProfileIndex) +static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, master_t *defaultConfig) { if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values return; @@ -2260,7 +2537,8 @@ void cliDumpRateProfile(uint8_t rateProfileIndex) cliPrint("\r\n# rateprofile\r\n"); cliRateProfile(""); printSectionBreak(); - dumpValues(PROFILE_RATE_VALUE); + + dumpValues(PROFILE_RATE_VALUE, dumpMask, defaultConfig); } void cliEnter(serialPort_t *serialPort) @@ -2574,18 +2852,6 @@ static void cliMotor(char *cmdline) cliPrintf("motor %d: %d\r\n", motor_index, motor_disarmed[motor_index]); } -static void cliName(char *cmdline) -{ - uint32_t len = strlen(cmdline); - if (len > 0) { - memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); - strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); - } - cliPrintf("name %s\r\n", strlen(masterConfig.name) > 0 ? masterConfig.name : "-"); - - return; -} - static void cliPlaySound(char *cmdline) { #if FLASH_SIZE <= 64 @@ -2709,6 +2975,26 @@ static void cliPutp(void *p, char ch) bufWriterAppend(p, ch); } +static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...) +{ + bool printValue = !((dumpMask & DO_DIFF) && equalsDefault); + bool printComment = (dumpMask & DIFF_COMMENTED); + if (printValue || printComment) { + if (printComment && !printValue) { + cliWrite('#'); + } + + va_list va; + va_start(va, format); + tfp_format(cliWriter, cliPutp, format, va); + va_end(va); + + return true; + } + + return false; +} + static void cliPrintf(const char *fmt, ...) { va_list va; @@ -2731,14 +3017,7 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full) int32_t value = 0; char buf[8]; - void *ptr = var->ptr; - if ((var->type & VALUE_SECTION_MASK) == PROFILE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index); - } - - if ((var->type & VALUE_SECTION_MASK) == PROFILE_RATE_VALUE) { - ptr = ((uint8_t *)ptr) + (sizeof(profile_t) * masterConfig.current_profile_index) + (sizeof(controlRateConfig_t) * getCurrentControlRateProfile()); - } + void *ptr = getValuePointer(var); switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: @@ -3142,13 +3421,14 @@ void cliProcess(void) cliBuffer[bufferIndex] = 0; // null terminate const clicmd_t *cmd; + char *options; for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) { - if(!strncasecmp(cliBuffer, cmd->name, strlen(cmd->name)) // command names match - && !isalnum((unsigned)cliBuffer[strlen(cmd->name)])) // next characted in bufffer is not alphanumeric (command is correctly terminated) + if ((options = checkCommand(cliBuffer, cmd->name))) { break; + } } if(cmd < cmdTable + CMD_COUNT) - cmd->func(cliBuffer + strlen(cmd->name) + 1); + cmd->func(options); else cliPrint("Unknown command, try 'help'"); bufferIndex = 0; diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 2c61276d16..342e4c0692 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -91,7 +91,7 @@ #define VBAT_ADC_PIN PA0 #define RSSI_ADC_PIN PB0 -#define LED_STRIP +//#define LED_STRIP #define WS2811_PIN PB4 #define WS2811_TIMER TIM3 #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 @@ -103,20 +103,20 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define SONAR -#define SONAR_ECHO_PIN PB0 -#define SONAR_TRIGGER_PIN PB5 +//#define SONAR +//#define SONAR_ECHO_PIN PB0 +//#define SONAR_TRIGGER_PIN PB5 #undef GPS #ifdef CC3D_OPBL -//#undef LED_STRIP +#undef LED_STRIP #define SKIP_CLI_COMMAND_HELP #define SKIP_PID_FLOAT #undef BARO #undef MAG #undef SONAR -#undef LED_STRIP +#undef USE_SOFTSERIAL1 #endif #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 0e8ba62be2..212ea2c83f 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -110,7 +110,7 @@ #define SONAR_TRIGGER_PIN_PWM PB8 #define SONAR_ECHO_PIN_PWM PB9 -#define DISPLAY +//#define DISPLAY #define USE_UART1 #define USE_UART2 diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 66e83fa3d5..06a49b25a5 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -81,13 +81,13 @@ #define USE_FLASHTOOLS #define USE_FLASH_M25P16 -#define SONAR +//#define SONAR #define SONAR_TRIGGER_PIN PB0 #define SONAR_ECHO_PIN PB1 #define SONAR_TRIGGER_PIN_PWM PB8 #define SONAR_ECHO_PIN_PWM PB9 -#define DISPLAY +//#define DISPLAY #define USE_UART1 #define USE_UART2 @@ -115,6 +115,12 @@ #define RSSI_ADC_PIN PA1 #define EXTERNAL1_ADC_PIN PA5 +//#define LED_STRIP +//#define LED_STRIP_TIMER TIM3 + +#define SKIP_CLI_COMMAND_HELP +#define SKIP_PID_LUXFLOAT + #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f103RCT6 in 64pin package From 1300952472579016c4f28ce97414af22ed9df69f Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 14:52:22 +0200 Subject: [PATCH 276/456] Added MPU9250 ACC detection. --- src/main/io/serial_cli.c | 2 +- src/main/sensors/acceleration.h | 19 ++++++++++--------- src/main/sensors/initialisation.c | 14 ++++++++++++++ 3 files changed, 25 insertions(+), 10 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 463ffb72da..56a3f037a7 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -228,7 +228,7 @@ static const char * const sensorTypeNames[] = { static const char * const sensorHardwareNames[4][11] = { { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "FAKE", NULL }, - { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", NULL }, + { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "FAKE", NULL }, { "", "None", "BMP085", "MS5611", "BMP280", NULL }, { "", "None", "HMC5883", "AK8975", "AK8963", NULL } }; diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index dc98ae78ef..62bf405820 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -20,15 +20,16 @@ // Type of accelerometer used/detected typedef enum { ACC_DEFAULT = 0, - ACC_NONE = 1, - ACC_ADXL345 = 2, - ACC_MPU6050 = 3, - ACC_MMA8452 = 4, - ACC_BMA280 = 5, - ACC_LSM303DLHC = 6, - ACC_MPU6000 = 7, - ACC_MPU6500 = 8, - ACC_FAKE = 9, + ACC_NONE, + ACC_ADXL345, + ACC_MPU6050, + ACC_MMA8452, + ACC_BMA280, + ACC_LSM303DLHC, + ACC_MPU6000, + ACC_MPU6500, + ACC_MPU9250, + ACC_FAKE, ACC_MAX = ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index bb46059ac1..2529a8c330 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -384,6 +384,20 @@ retry: } #endif ; // fallthrough + case ACC_MPU9250: +#ifdef USE_ACC_SPI_MPU9250 + + if (mpu9250SpiAccDetect(&acc)) + { + accHardware = ACC_MPU9250; +#ifdef ACC_MPU9250_ALIGN + accAlign = ACC_MPU9250_ALIGN; +#endif + + break; + } +#endif + ; // fallthrough case ACC_FAKE: #ifdef USE_FAKE_ACC if (fakeAccDetect(&acc)) { From cb2b63112d31604f2fe25d8f0c809c3cb231a3a6 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 15:18:21 +0200 Subject: [PATCH 277/456] Cleanup some target junk --- src/main/io/ledstrip.c | 79 --------------------------- src/main/target/COLIBRI_RACE/target.h | 1 - src/main/target/MOTOLAB/target.h | 1 + 3 files changed, 1 insertion(+), 80 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index b707a2a017..03c4c5d1ef 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -238,85 +238,6 @@ static const specialColorIndexes_t defaultSpecialColors[] = { static int scaledThrottle; - - - -/* -#ifdef USE_LED_RING_DEFAULT_CONFIG -const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 2, 2), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 2, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 2, 0), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 0), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 0, 0), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 0, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 0, 2), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 2), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 1, 1), 3, LED_FUNCTION_THRUST_RING}, -}; -#elif defined(USE_COLIBTI_RACE_LED_DEFAULT_CONFIG) -const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY( 0, 0), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 1), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 0, 8), 6, LED_DIRECTION_WEST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 15), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 7, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 8, 14), 6, LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 8), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 1), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, - { CALCULATE_LED_XY( 15, 0), 6, LED_DIRECTION_EAST | LED_FUNCTION_WARNING | LED_FUNCTION_COLOR }, -}; -#else -const ledConfig_t defaultLedStripConfig[] = { - { CALCULATE_LED_XY(15, 15), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { CALCULATE_LED_XY(15, 8), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY(15, 7), 0, LED_DIRECTION_EAST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - - { CALCULATE_LED_XY(15, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_EAST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { CALCULATE_LED_XY( 8, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - { CALCULATE_LED_XY( 7, 0), 0, LED_DIRECTION_NORTH | LED_FUNCTION_FLIGHT_MODE }, - - { CALCULATE_LED_XY( 0, 0), 0, LED_DIRECTION_NORTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { CALCULATE_LED_XY( 0, 7), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 0, 8), 0, LED_DIRECTION_WEST | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - - { CALCULATE_LED_XY( 0, 15), 0, LED_DIRECTION_SOUTH | LED_DIRECTION_WEST | LED_FUNCTION_INDICATOR | LED_FUNCTION_ARM_STATE }, - - { CALCULATE_LED_XY( 7, 15), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 8, 15), 0, LED_DIRECTION_SOUTH | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - - { CALCULATE_LED_XY( 7, 7), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 8, 7), 0, LED_DIRECTION_UP | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 7, 8), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - { CALCULATE_LED_XY( 8, 8), 0, LED_DIRECTION_DOWN | LED_FUNCTION_FLIGHT_MODE | LED_FUNCTION_WARNING }, - - { CALCULATE_LED_XY( 8, 9), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 9, 10), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY(10, 11), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY(10, 12), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 9, 13), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 8, 14), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 7, 14), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 6, 13), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 5, 12), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 5, 11), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 6, 10), 3, LED_FUNCTION_THRUST_RING}, - { CALCULATE_LED_XY( 7, 9), 3, LED_FUNCTION_THRUST_RING}, - -}; -#endif -*/ - - - - static void updateLedRingCounts(void); STATIC_UNIT_TESTED void determineLedStripDimensions(void) diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index d4db45a6de..a68d7a71d9 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -113,7 +113,6 @@ #define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP -#define USE_COLIBTI_RACE_LED_DEFAULT_CONFIG #define WS2811_PIN PA6 // TIM16_CH1 #define WS2811_TIMER TIM16 diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index fe465da9c9..beb5431c7a 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -21,6 +21,7 @@ #define USE_CLI #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT +#define TARGET_CONFIG #define LED0 PB5 // Blue LEDs - PB5 //#define LED1 PB9 // Green LEDs - PB9 From da9673b9825751facf4b82074c6945428ef3277b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 15:23:14 +0200 Subject: [PATCH 278/456] Increase sensor Array size --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f23aa37df4..68bd96e0e0 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -239,7 +239,7 @@ static const char * const sensorTypeNames[] = { #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) -static const char * const sensorHardwareNames[4][11] = { +static const char * const sensorHardwareNames[4][12] = { { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "FAKE", NULL }, { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "FAKE", NULL }, { "", "None", "BMP085", "MS5611", "BMP280", NULL }, From f933e316c6872bf1a474f55578f9d361b2bdd4c3 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 15:23:14 +0200 Subject: [PATCH 279/456] Increase sensor Array size --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 14e51461fb..24f4562306 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -240,7 +240,7 @@ static const char * const sensorTypeNames[] = { #define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) -static const char * const sensorHardwareNames[4][11] = { +static const char * const sensorHardwareNames[4][12] = { { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "FAKE", NULL }, { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", NULL }, { "", "None", "BMP085", "MS5611", "BMP280", NULL }, From 4ff280209511e242205da2c38ee10b8b2a649735 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 16:18:06 +0200 Subject: [PATCH 280/456] Fix CPU bug due toe reinitialising of filters --- src/main/config/config.c | 2 +- src/main/flight/pid.c | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b8b5cf11e0..6ffaf93d2f 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dtermSetpointWeight = 120; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; - pidProfile->toleranceBand = 15; + pidProfile->toleranceBand = 0; pidProfile->toleranceBandReduction = 40; pidProfile->zeroCrossAllowanceCount = 2; pidProfile->itermThrottleGain = 0; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0f91875e2a..09364a8b3a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -116,11 +116,13 @@ void initFilters(const pidProfile_t *pidProfile) { if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, notchQ, FILTER_NOTCH); + dtermNotchInitialised = true; } if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) { for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, gyro.targetLooptime); + dtermBiquadLpfInitialised = true; } } } From 107f09d6e1d54c11d86d3c36474bb73f655a651f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 16:18:06 +0200 Subject: [PATCH 281/456] Fix CPU bug due toe reinitialising of filters --- src/main/config/config.c | 2 +- src/main/flight/pid.c | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 243ac2f45b..0b9dff11e8 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -249,7 +249,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->dtermSetpointWeight = 120; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; - pidProfile->toleranceBand = 15; + pidProfile->toleranceBand = 0; pidProfile->toleranceBandReduction = 40; pidProfile->zeroCrossAllowanceCount = 2; pidProfile->itermThrottleGain = 0; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 05969e4f1e..b9d82cac35 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -114,11 +114,13 @@ void initFilters(const pidProfile_t *pidProfile) if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, notchQ, FILTER_NOTCH); + dtermNotchInitialised = true; } if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) { for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, gyro.targetLooptime); + dtermBiquadLpfInitialised = true; } } } From 629f96969c48f3d48be5c6959317b66bc569f147 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 16:38:38 +0200 Subject: [PATCH 282/456] Added MPU9250 to acc lookup table. Duplication. Missing in Allowed Values printout. Duplication... --- src/main/io/serial_cli.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 68bd96e0e0..3ebafa4ec6 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -450,6 +450,7 @@ static const char * const lookupTableAccHardware[] = { "LSM303DLHC", "MPU6000", "MPU6500", + "MPU9250", "FAKE" }; From 86ea38f6073dd1a4764e2c292acb06c677a50c00 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 9 Aug 2016 17:58:33 +0100 Subject: [PATCH 283/456] Changed mpu6500GyroInit to only call mpuIntExtiInit once --- src/main/drivers/accgyro_mpu6500.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index ed872230ea..9a762dc7c4 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -86,8 +86,6 @@ void mpu6500GyroInit(uint8_t lpf) } #endif - mpuIntExtiInit(); - mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); delay(100); mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07); From 685ae3aee376879e536d009de3b37c4def054cce Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 19:47:52 +0200 Subject: [PATCH 284/456] Limit number of targets to build on Travis. --- .travis.yml | 69 ++++++++++++++++++++++++++++------------------------- 1 file changed, 36 insertions(+), 33 deletions(-) diff --git a/.travis.yml b/.travis.yml index ca73f8c14e..787ac12dea 100644 --- a/.travis.yml +++ b/.travis.yml @@ -2,51 +2,54 @@ env: # - RUNTESTS=True # - PUBLISHMETA=True # - PUBLISHDOCS=True - - TARGET=AFROMINI - - TARGET=AIORACERF3 - - TARGET=AIR32 - - TARGET=ALIENFLIGHTF1 +# - TARGET=AFROMINI +# - TARGET=AIORACERF3 +# - TARGET=AIR32 +# - TARGET=ALIENFLIGHTF1 - TARGET=ALIENFLIGHTF3 - TARGET=ALIENFLIGHTF4 - TARGET=BLUEJAYF4 - TARGET=CC3D - TARGET=CC3D_OPBL - - TARGET=CHEBUZZF3 - - TARGET=CJMCU - - TARGET=COLIBRI - - TARGET=COLIBRI_RACE - - TARGET=DOGE - - TARGET=EUSTM32F103RC - - TARGET=F4BY - - TARGET=FURYF3 +# - TARGET=CHEBUZZF3 +# - TARGET=CJMCU +# - TARGET=COLIBRI +# - TARGET=COLIBRI_RACE +# - TARGET=DOGE +# - TARGET=EUSTM32F103RC +# - TARGET=F4BY +# - TARGET=FURYF3 - TARGET=FURYF4 - - TARGET=IRCFUSIONF3 - - TARGET=KISSFC - - TARGET=LUX_RACE - - TARGET=MICROSCISKY - - TARGET=MOTOLAB +# - TARGET=IRCFUSIONF3 +# - TARGET=KISSFC +# - TARGET=LUX_RACE +# - TARGET=MICROSCISKY +# - TARGET=MOTOLAB - TARGET=NAZE - - TARGET=OLIMEXINO - - TARGET=OMNIBUS - - TARGET=OMNIBUSF4 - - TARGET=PIKOBLX - - TARGET=PORT103R +# - TARGET=OLIMEXINO +# - TARGET=OMNIBUS +# - TARGET=OMNIBUSF4 +# - TARGET=PIKOBLX +# - TARGET=PORT103R - TARGET=REVO - - TARGET=REVONANO - - TARGET=REVO_OPBL - - TARGET=RMDO - - TARGET=SINGULARITY - - TARGET=SIRINFPV +# - TARGET=REVONANO +# - TARGET=REVO_OPBL +# - TARGET=RMDO +# - TARGET=SINGULARITY +# - TARGET=SIRINFPV - TARGET=SPARKY - - TARGET=SPARKY2 - - TARGET=SPARKY_OPBL +# - TARGET=SPARKY2 +# - TARGET=SPARKY_OPBL - TARGET=SPRACINGF3 - TARGET=SPRACINGF3EVO - - TARGET=SPRACINGF3MINI +# - TARGET=SPRACINGF3MINI - TARGET=STM32F3DISCOVERY - - TARGET=VRRACE - - TARGET=X_RACERSPI - - TARGET=ZCOREF3 +# - TARGET=VRRACE +# - TARGET=X_RACERSPI +# - TARGET=ZCOREF3 +CC3D NAZE +ALIENFLIGHTF3 SPARKY SPRACINGF3 SPRACINGF3EVO STM32F3DISCOVERY +ALIENFLIGHTF4 BLUEJAYF4 FURYF4 REVO # use new docker environment sudo: false From 69aecd9e926fdb372366970a3984fb8a3949eebe Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 19:50:22 +0200 Subject: [PATCH 285/456] Limit number of targets to build on Travis. --- .travis.yml | 3 --- 1 file changed, 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 787ac12dea..16b89723f4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -47,9 +47,6 @@ env: # - TARGET=VRRACE # - TARGET=X_RACERSPI # - TARGET=ZCOREF3 -CC3D NAZE -ALIENFLIGHTF3 SPARKY SPRACINGF3 SPRACINGF3EVO STM32F3DISCOVERY -ALIENFLIGHTF4 BLUEJAYF4 FURYF4 REVO # use new docker environment sudo: false From a396ba0c119c6008b985730c742b18c7f9d7c36b Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Tue, 9 Aug 2016 19:53:18 +0200 Subject: [PATCH 286/456] cleanup cli commands outputs and over - deleted printSectionBreak and his #define (not more used) - cleaned cli commads output (dump, sd_info, version) --- src/main/io/serial_cli.c | 25 +++++-------------------- 1 file changed, 5 insertions(+), 20 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3ebafa4ec6..1850472799 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -927,7 +927,6 @@ const clivalue_t valueTable[] = { #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) - typedef union { int32_t int_value; float float_value; @@ -940,8 +939,6 @@ static void cliPrint(const char *str); static void cliPrintf(const char *fmt, ...); static void cliWrite(uint8_t ch); -#define printSectionBreak() cliPrintf((char *)sectionBreak) - #define COMPARE_CONFIG(value) (masterConfig.value == defaultConfig.value) static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...); @@ -1731,8 +1728,6 @@ static void printServo(uint8_t dumpMask, master_t *defaultConfig) ); } - printSectionBreak(); - // print servo directions unsigned int channel; for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { @@ -2022,10 +2017,9 @@ static void cliSdInfo(char *cmdline) { ; // Nothing more detailed to print break; } - - cliPrint("\r\n"); break; } + cliPrint("\r\n"); } #endif @@ -2326,14 +2320,12 @@ static void printConfig(char *cmdline, bool doDiff) if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { cliPrint("\r\n# version\r\n"); cliVersion(NULL); - cliPrint("\r\n"); if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) { cliPrint("\r\n# reset configuration to default settings\r\ndefaults\r\n"); } - printSectionBreak(); - cliPrint("# name\r\n"); + cliPrint("\r\n# name\r\n"); printName(dumpMask); cliPrint("\r\n# mixer\r\n"); @@ -2460,7 +2452,7 @@ static void printConfig(char *cmdline, bool doDiff) cliPrint("\r\n# adjrange\r\n"); printAdjustmentRange(dumpMask, &defaultConfig); - cliPrintf("\r\n# rxrange\r\n"); + cliPrint("\r\n# rxrange\r\n"); printRxRange(dumpMask, &defaultConfig); #ifdef USE_SERVOS @@ -2519,26 +2511,19 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, master_t *def { if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values return; - changeProfile(profileIndex); cliPrint("\r\n# profile\r\n"); cliProfile(""); - - printSectionBreak(); dumpValues(PROFILE_VALUE, dumpMask, defaultConfig); - - cliRateProfile(""); } static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, master_t *defaultConfig) { if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values - return; + return; changeControlRateProfile(rateProfileIndex); cliPrint("\r\n# rateprofile\r\n"); cliRateProfile(""); - printSectionBreak(); - dumpValues(PROFILE_RATE_VALUE, dumpMask, defaultConfig); } @@ -3338,7 +3323,7 @@ static void cliVersion(char *cmdline) { UNUSED(cmdline); - cliPrintf("# BetaFlight/%s %s %s / %s (%s)", + cliPrintf("# BetaFlight/%s %s %s / %s (%s)\r\n", targetName, FC_VERSION_STRING, buildDate, From 15dfc33d964700e3d88a5de53c49605907137c21 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 9 Aug 2016 20:07:26 +0200 Subject: [PATCH 287/456] Limit number of targets to build on Travis. --- .travis.yml | 66 ++++++++++++++++++++++++++--------------------------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/.travis.yml b/.travis.yml index ca73f8c14e..16b89723f4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -2,51 +2,51 @@ env: # - RUNTESTS=True # - PUBLISHMETA=True # - PUBLISHDOCS=True - - TARGET=AFROMINI - - TARGET=AIORACERF3 - - TARGET=AIR32 - - TARGET=ALIENFLIGHTF1 +# - TARGET=AFROMINI +# - TARGET=AIORACERF3 +# - TARGET=AIR32 +# - TARGET=ALIENFLIGHTF1 - TARGET=ALIENFLIGHTF3 - TARGET=ALIENFLIGHTF4 - TARGET=BLUEJAYF4 - TARGET=CC3D - TARGET=CC3D_OPBL - - TARGET=CHEBUZZF3 - - TARGET=CJMCU - - TARGET=COLIBRI - - TARGET=COLIBRI_RACE - - TARGET=DOGE - - TARGET=EUSTM32F103RC - - TARGET=F4BY - - TARGET=FURYF3 +# - TARGET=CHEBUZZF3 +# - TARGET=CJMCU +# - TARGET=COLIBRI +# - TARGET=COLIBRI_RACE +# - TARGET=DOGE +# - TARGET=EUSTM32F103RC +# - TARGET=F4BY +# - TARGET=FURYF3 - TARGET=FURYF4 - - TARGET=IRCFUSIONF3 - - TARGET=KISSFC - - TARGET=LUX_RACE - - TARGET=MICROSCISKY - - TARGET=MOTOLAB +# - TARGET=IRCFUSIONF3 +# - TARGET=KISSFC +# - TARGET=LUX_RACE +# - TARGET=MICROSCISKY +# - TARGET=MOTOLAB - TARGET=NAZE - - TARGET=OLIMEXINO - - TARGET=OMNIBUS - - TARGET=OMNIBUSF4 - - TARGET=PIKOBLX - - TARGET=PORT103R +# - TARGET=OLIMEXINO +# - TARGET=OMNIBUS +# - TARGET=OMNIBUSF4 +# - TARGET=PIKOBLX +# - TARGET=PORT103R - TARGET=REVO - - TARGET=REVONANO - - TARGET=REVO_OPBL - - TARGET=RMDO - - TARGET=SINGULARITY - - TARGET=SIRINFPV +# - TARGET=REVONANO +# - TARGET=REVO_OPBL +# - TARGET=RMDO +# - TARGET=SINGULARITY +# - TARGET=SIRINFPV - TARGET=SPARKY - - TARGET=SPARKY2 - - TARGET=SPARKY_OPBL +# - TARGET=SPARKY2 +# - TARGET=SPARKY_OPBL - TARGET=SPRACINGF3 - TARGET=SPRACINGF3EVO - - TARGET=SPRACINGF3MINI +# - TARGET=SPRACINGF3MINI - TARGET=STM32F3DISCOVERY - - TARGET=VRRACE - - TARGET=X_RACERSPI - - TARGET=ZCOREF3 +# - TARGET=VRRACE +# - TARGET=X_RACERSPI +# - TARGET=ZCOREF3 # use new docker environment sudo: false From 220145d788aacf612a656cb0a77445f852459503 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 22:57:54 +0200 Subject: [PATCH 288/456] Set back the max limit for rc rates --- src/main/io/serial_cli.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 68bd96e0e0..b45ece523c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -789,8 +789,8 @@ const clivalue_t valueTable[] = { { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, #endif - { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 300 } }, - { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 300 } }, + { "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 255 } }, + { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 255 } }, { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, From 98eb820cdbd06b629335aa5f18f02531a7745938 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 23:34:12 +0200 Subject: [PATCH 289/456] defaults --- src/main/config/config.c | 13 ++++++------- src/main/flight/pid.c | 2 +- src/main/flight/pid.h | 2 +- src/main/io/serial_cli.c | 2 +- 4 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 6ffaf93d2f..fd74196200 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -206,7 +206,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[ROLL] = 20; pidProfile->P8[PITCH] = 60; pidProfile->I8[PITCH] = 60; - pidProfile->D8[PITCH] = 25; + pidProfile->D8[PITCH] = 22; pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; @@ -232,15 +232,15 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 80; - pidProfile->rollPitchItermIgnoreRate = 200; - pidProfile->yawItermIgnoreRate = 50; + pidProfile->rollPitchItermIgnoreRate = 130; + pidProfile->yawItermIgnoreRate = 32; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 0; pidProfile->dterm_notch_cutoff = 150; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; - pidProfile->zeroThrottleStabilisation = PID_STABILISATION_OFF; + pidProfile->pidAtMinThrottle = PID_STABILISATION_OFF; // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; @@ -310,10 +310,9 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) { #ifdef BRUSHED_MOTORS escAndServoConfig->minthrottle = 1000; - escAndServoConfig->maxthrottle = 2000; #else - escAndServoConfig->minthrottle = 1150; - escAndServoConfig->maxthrottle = 1850; + escAndServoConfig->maxthrottle = 2000; + escAndServoConfig->minthrottle = 1070; #endif escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 09364a8b3a..2e136af636 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -274,7 +274,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // -----calculate I component. // Reduce strong Iterm accumulation during higher stick inputs float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - float setpointRateScaler = constrainf(1.0f - (1.5f * (ABS(setpointRate[axis]) / accumulationThreshold)), 0.0f, 1.0f); + float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f); // Handle All windup Scenarios // limit maximum integrator value to prevent WindUp diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c12404ec5a..12fa8324e8 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -91,7 +91,7 @@ typedef struct pidProfile_s { uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage - uint8_t zeroThrottleStabilisation; // Disable/Enable zero throttle stabilisation. Normally even without airmode P and D would be active. + uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. // Betaflight PID controller parameters uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5947e954c1..7eedf297ae 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -842,7 +842,7 @@ const clivalue_t valueTable[] = { { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_hz, .config.minmax = { 0, 500 } }, { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 500 } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, - { "zero_throttle_stabilisation",VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.zeroThrottleStabilisation, .config.lookup = { TABLE_OFF_ON } }, + { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, From ea67304a1a7f683a3cba644ed68b5a20ea68e23f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 23:38:07 +0200 Subject: [PATCH 290/456] Fix mw.c --- src/main/mw.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/mw.c b/src/main/mw.c index b2c569e98b..3652108743 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -536,7 +536,7 @@ void processRx(void) This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */ if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) { pidResetErrorGyroState(); - if (currentProfile->pidProfile.zeroThrottleStabilisation) + if (currentProfile->pidProfile.pidAtMinThrottle) pidStabilisationState(PID_STABILISATION_ON); else pidStabilisationState(PID_STABILISATION_OFF); From 5a493ff6a0a31f4e38e8ad2bf734d61dcfd1f2a5 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Sun, 31 Jul 2016 15:42:03 +1000 Subject: [PATCH 291/456] AIRHEROF3 from MultiWiiCopter basic support RC4 fix, LEDSTRIP enable OUT2 Fix Features Alignment fix Add BMP280 baro on SPI bus --- Makefile | 4 + src/main/target/AIRHEROF3/target.c | 106 +++++++++++++++++++++++++ src/main/target/AIRHEROF3/target.h | 115 ++++++++++++++++++++++++++++ src/main/target/AIRHEROF3/target.mk | 12 +++ src/main/target/system_stm32f30x.c | 8 +- 5 files changed, 244 insertions(+), 1 deletion(-) create mode 100755 src/main/target/AIRHEROF3/target.c create mode 100755 src/main/target/AIRHEROF3/target.h create mode 100755 src/main/target/AIRHEROF3/target.mk diff --git a/Makefile b/Makefile index 777c04026d..31485f26a5 100644 --- a/Makefile +++ b/Makefile @@ -329,6 +329,10 @@ ifneq ($(FLASH_SIZE),) DEVICE_FLAGS := $(DEVICE_FLAGS) -DFLASH_SIZE=$(FLASH_SIZE) endif +ifneq ($(HSE_VALUE),) +DEVICE_FLAGS := $(DEVICE_FLAGS) -DHSE_VALUE=$(HSE_VALUE) +endif + TARGET_DIR = $(ROOT)/src/main/target/$(BASE_TARGET) TARGET_DIR_SRC = $(notdir $(wildcard $(TARGET_DIR)/*.c)) diff --git a/src/main/target/AIRHEROF3/target.c b/src/main/target/AIRHEROF3/target.c new file mode 100755 index 0000000000..08ff2bcbce --- /dev/null +++ b/src/main/target/AIRHEROF3/target.c @@ -0,0 +1,106 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM4 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM4 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), + PWM3 | (MAP_TO_PWM_INPUT << 8), + PWM4 | (MAP_TO_PWM_INPUT << 8), + PWM5 | (MAP_TO_PWM_INPUT << 8), + PWM6 | (MAP_TO_PWM_INPUT << 8), + PWM7 | (MAP_TO_PWM_INPUT << 8), + PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM1 - RC1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM2 - RC2 + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM3 - RC3 + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM4 - RC4 + { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - RC5 + { TIM3, IO_TAG(PA7), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM6 - RC6 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM7 - RC7 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM8 - RC8 + + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // PWM9 - OUT1 + { TIM1, IO_TAG(PA11), TIM_Channel_4, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_11}, // PWM10 - OUT2 + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM11 - OUT3 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM12 - OUT4 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM13 - OUT5 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2} // PWM14 - OUT6 +}; diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h new file mode 100755 index 0000000000..e4fa195c05 --- /dev/null +++ b/src/main/target/AIRHEROF3/target.h @@ -0,0 +1,115 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "AIR3" + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT +#define CONFIG_PREFER_ACC_ON + +#define LED0 PB3 +#define LED1 PB4 + +#define BEEPER PA12 +#define BEEPER_INVERTED + +#define USE_EXTI +#define MPU_INT_EXTI PC13 +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU INT +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define USE_SPI +#define USE_SPI_DEVICE_2 + +#define MPU6500_CS_PIN PB12 +#define MPU6500_SPI_INSTANCE SPI2 + +#define GYRO +#define USE_GYRO_SPI_MPU6500 +#define GYRO_MPU6500_ALIGN CW0_DEG + +#define ACC +#define USE_ACC_SPI_MPU6500 +#define ACC_MPU6500_ALIGN CW0_DEG + +#define BARO +#define USE_BARO_SPI_BMP280 + +#define BMP280_SPI_INSTANCE SPI2 +#define BMP280_CS_PIN PB5 + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_SOFTSERIAL1 +#define USE_SOFTSERIAL2 +#define SERIAL_PORT_COUNT 5 + +#define SOFTSERIAL_1_TIMER TIM3 +#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5 +#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6 +#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7 +#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) + +#define BOARD_HAS_VOLTAGE_DIVIDER +#define USE_ADC +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA4 +#define RSSI_ADC_PIN PA1 + +#define LED_STRIP +#define WS2811_TIMER TIM3 +#define WS2811_PIN PA6 +#define WS2811_DMA_CHANNEL DMA1_Channel6 +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER + +#define GPS + +#define DEFAULT_FEATURES FEATURE_VBAT +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 +#define TARGET_CONFIG +#define RX_CHANNELS_TAER + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTF (BIT(4)) + +#define USABLE_TIMER_CHANNEL_COUNT 14 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/AIRHEROF3/target.mk b/src/main/target/AIRHEROF3/target.mk new file mode 100755 index 0000000000..372f7bf799 --- /dev/null +++ b/src/main/target/AIRHEROF3/target.mk @@ -0,0 +1,12 @@ +F3_TARGETS += $(TARGET) +HSE_VALUE = 12000000 + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6500.c \ + drivers/barometer_spi_bmp280.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/sonar_hcsr04.c \ + drivers/serial_softserial.c diff --git a/src/main/target/system_stm32f30x.c b/src/main/target/system_stm32f30x.c index 5d62e47aa3..f6bb082a97 100644 --- a/src/main/target/system_stm32f30x.c +++ b/src/main/target/system_stm32f30x.c @@ -331,7 +331,13 @@ void SetSysClock(void) /* PLL configuration */ RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL)); - RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); + + if (HSE_VALUE == 12000000) { + RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL6); + } + else { + RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9); + } /* Enable PLL */ RCC->CR |= RCC_CR_PLLON; From 0a33960258bb4e01f988b19caa4e6cac3fa344dc Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Wed, 10 Aug 2016 16:19:44 +1000 Subject: [PATCH 292/456] Remove target config --- src/main/target/AIRHEROF3/target.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h index e4fa195c05..a578be44ec 100755 --- a/src/main/target/AIRHEROF3/target.h +++ b/src/main/target/AIRHEROF3/target.h @@ -96,7 +96,6 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART2 -#define TARGET_CONFIG #define RX_CHANNELS_TAER #define SPEKTRUM_BIND From 1a23a4f023ac97d7910428e10b3cd19b024ce4cc Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Wed, 10 Aug 2016 11:10:44 +0200 Subject: [PATCH 293/456] Sparky 2.0 UART fix --- src/main/target/SPARKY2/target.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 0ed29e6616..febebfa1fb 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -77,18 +77,18 @@ #define USE_VCP #define VBUS_SENSING_PIN PA8 -#define USE_USART1 -#define USART1_RX_PIN PA10 -#define USART1_TX_PIN PA9 -#define USART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 -#define USE_USART3 -#define USART3_RX_PIN PB11 -#define USART3_TX_PIN PB10 +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 -#define USE_USART6 -#define USART6_RX_PIN PC7 -#define USART6_TX_PIN PC6 //inverter +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 //inverter #define SERIAL_PORT_COUNT 4 From 66f1abe1be4ae571a9e071bccbe974d2e6e68709 Mon Sep 17 00:00:00 2001 From: blckmn Date: Wed, 10 Aug 2016 20:54:26 +1000 Subject: [PATCH 294/456] Added missing delay for F4 to CLI --- src/main/io/serial_cli.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7eedf297ae..215fc67b1a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2975,7 +2975,11 @@ static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *form tfp_format(cliWriter, cliPutp, format, va); va_end(va); - return true; +#ifdef USE_SLOW_SERIAL_CLI + delay(1); +#endif + + return true; } return false; From dfee308c3555e0a513101039e924ed450e2039b6 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Wed, 10 Aug 2016 13:25:37 +0200 Subject: [PATCH 295/456] Redo PR 945. Lost in rebasing/merging to dev. --- Makefile | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index c65876bcac..81b986cc71 100644 --- a/Makefile +++ b/Makefile @@ -179,7 +179,6 @@ LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f303_$(FLASH_SIZE)k.ld ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -fsingle-precision-constant -Wdouble-promotion DEVICE_FLAGS = -DSTM32F303xC -DSTM32F303 -TARGET_FLAGS = -D$(TARGET) # End F3 targets # # Start F4 targets @@ -273,7 +272,6 @@ $(error Unknown MCU for F4 target) endif DEVICE_FLAGS += -DHSE_VALUE=$(HSE_VALUE) -TARGET_FLAGS = -D$(TARGET) # End F4 targets # # Start F1 targets @@ -313,7 +311,6 @@ endif LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f103_$(FLASH_SIZE)k.ld ARCH_FLAGS = -mthumb -mcpu=cortex-m3 -TARGET_FLAGS := -D$(TARGET) -pedantic $(TARGET_FLAGS) ifeq ($(DEVICE_FLAGS),) DEVICE_FLAGS = -DSTM32F10X_MD @@ -583,8 +580,10 @@ CFLAGS += $(ARCH_FLAGS) \ -Wall -Wextra -Wunsafe-loop-optimizations -Wdouble-promotion \ -ffunction-sections \ -fdata-sections \ + -pedantic \ $(DEVICE_FLAGS) \ -DUSE_STDPERIPH_DRIVER \ + -D$(TARGET) \ $(TARGET_FLAGS) \ -D'__FORKNAME__="$(FORKNAME)"' \ -D'__TARGET__="$(TARGET)"' \ From 22a55ce313c2b19daaba54c5a8f89f0541cfdea7 Mon Sep 17 00:00:00 2001 From: KiteAnton Date: Wed, 10 Aug 2016 15:38:25 +0200 Subject: [PATCH 296/456] Fix issue with newline for MAC users --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 215fc67b1a..3a14434be0 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -3209,7 +3209,7 @@ static void cliGet(char *cmdline) val = &valueTable[i]; cliPrintf("%s = ", valueTable[i].name); cliPrintVar(val, 0); - cliPrint("\n"); + cliPrint("\r\n"); cliPrintVarRange(val); cliPrint("\r\n"); From c944f1e351da090878daebd4469ed1cb2129d253 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 10 Aug 2016 15:06:30 +0100 Subject: [PATCH 297/456] Added MSP placeholder for NRF24 data to help ensure future compatibility with iNav --- src/main/io/serial_msp.c | 11 +++++++++-- src/main/rx/rx.h | 3 +++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index a2f472f45d..732070a7d5 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1033,7 +1033,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_RX_CONFIG: - headSerialReply(16); + headSerialReply(22); serialize8(masterConfig.rxConfig.serialrx_provider); serialize16(masterConfig.rxConfig.maxcheck); serialize16(masterConfig.rxConfig.midrc); @@ -1044,6 +1044,9 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(masterConfig.rxConfig.rcInterpolation); serialize8(masterConfig.rxConfig.rcInterpolationInterval); serialize16(masterConfig.rxConfig.airModeActivateThreshold); + serialize8(masterConfig.rxConfig.nrf24rx_protocol); + serialize32(masterConfig.rxConfig.nrf24rx_id); + serialize8(masterConfig.rxConfig.nrf24rx_channel_count); break; case MSP_FAILSAFE_CONFIG: @@ -1692,8 +1695,12 @@ static bool processInCommand(void) masterConfig.rxConfig.rcInterpolationInterval = read8(); masterConfig.rxConfig.airModeActivateThreshold = read16(); } + if (currentPort->dataSize > 16) { + masterConfig.rxConfig.nrf24rx_protocol = read8(); + masterConfig.rxConfig.nrf24rx_id = read32(); + masterConfig.rxConfig.nrf24rx_channel_count = read8(); + } break; - case MSP_SET_FAILSAFE_CONFIG: masterConfig.failsafeConfig.failsafe_delay = read8(); masterConfig.failsafeConfig.failsafe_off_delay = read8(); diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index effd1cc526..e3087ce3ae 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -113,6 +113,9 @@ typedef struct rxConfig_s { uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. uint8_t sbus_inversion; // default sbus (Futaba, FrSKY) is inverted. Support for uninverted OpenLRS (and modified FrSKY) receivers. + uint8_t nrf24rx_protocol; // type of nrf24 protocol (0 = v202 250kbps). Must be enabled by FEATURE_RX_NRF24 first. + uint32_t nrf24rx_id; + uint8_t nrf24rx_channel_count; uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot uint8_t rssi_channel; From ee1f59b266a1242430779b8dd6edd823a15d52e5 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 11 Aug 2016 07:16:49 +1200 Subject: [PATCH 298/456] Re-added delay for CLI prints for F4. --- src/main/io/serial_cli.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 60879a3852..74d3b4ab28 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2976,6 +2976,10 @@ static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *form tfp_format(cliWriter, cliPutp, format, va); va_end(va); +#ifdef USE_SLOW_SERIAL_CLI + delay(1); +#endif + return true; } From 511117d41c61f120560bf83de950cac0207c1fbd Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 11 Aug 2016 08:10:30 +1200 Subject: [PATCH 299/456] Fixed 'diff commented' CLI command. --- src/main/io/serial_cli.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3a14434be0..e1abbbeca6 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2305,16 +2305,18 @@ static void printConfig(char *cmdline, bool doDiff) dumpMask = DUMP_RATES; // only } else if ((options = checkCommand(cmdline, "all"))) { dumpMask = DUMP_ALL; // all profiles and rates + } else { + options = cmdline; } master_t defaultConfig; if (doDiff) { dumpMask = dumpMask | DO_DIFF; - createDefaultConfig(&defaultConfig); - } + createDefaultConfig(&defaultConfig); - if (checkCommand(options, "commented")) { - dumpMask = dumpMask | DIFF_COMMENTED; // add unchanged values as comments for diff + if (checkCommand(cmdline, "commented")) { + dumpMask = dumpMask | DIFF_COMMENTED; // add unchanged values as comments for diff + } } if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { From 00692145efa3d513ac19a8093cfa32e23def3f15 Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Thu, 11 Aug 2016 08:25:42 +0300 Subject: [PATCH 300/456] Fix F4 ledstip --- src/main/drivers/light_ws2811strip.c | 4 ++++ src/main/drivers/light_ws2811strip.h | 4 ++++ src/main/drivers/light_ws2811strip_stm32f4xx.c | 2 +- 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 6c8f2bb18a..b41dc21c9e 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -39,7 +39,11 @@ #include "drivers/dma.h" #include "drivers/light_ws2811strip.h" +#if defined(STM32F4) +uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +#else uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +#endif volatile uint8_t ws2811LedDataTransferInProgress = 0; static hsvColor_t ledColorBuffer[WS2811_LED_STRIP_LENGTH]; diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index 853ac7acbc..c24712502c 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -51,5 +51,9 @@ void setStripColors(const hsvColor_t *colors); bool isWS2811LedStripReady(void); +#if defined(STM32F4) +extern uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +#else extern uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; +#endif extern volatile uint8_t ws2811LedDataTransferInProgress; diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index 63bcd9cee6..d1ea43d7ae 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -50,7 +50,7 @@ static void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { ws2811LedDataTransferInProgress = 0; DMA_Cmd(descriptor->stream, DISABLE); - TIM_DMACmd(TIM5, TIM_DMA_CC1, DISABLE); + TIM_DMACmd(WS2811_TIMER, timDMASource, DISABLE); DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); } } From 5f23efe70c237a0a07c2513b29080f3f4ac84c18 Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Thu, 11 Aug 2016 06:34:04 +0100 Subject: [PATCH 301/456] Update Log Header for Betaflight PID Parameters Add new betaflight PID controller parameters to log header and debug_mode (so we know what is being logged in the debug fields) --- src/main/blackbox/blackbox.c | 29 +++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 406ce05d39..09946a4ab0 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1210,18 +1210,34 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDVEL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDVEL], masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDVEL]); - BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); - BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f)); - BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); - BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.vbatPidCompensation); - BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); - BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); + BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_filter_type); BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_notch_cutoff * 100.0f)); + BLACKBOX_PRINT_HEADER_LINE("deltaMethod:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.deltaMethod); + BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate); + BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate); + BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit); + BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); + BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.vbatPidCompensation); + BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidAtMinThrottle); + + // Betaflight PID controller parameters + BLACKBOX_PRINT_HEADER_LINE("toleranceBand:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.toleranceBand); + BLACKBOX_PRINT_HEADER_LINE("toleranceBandReduction:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.toleranceBandReduction); + BLACKBOX_PRINT_HEADER_LINE("zeroCrossAllowanceCount:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.zeroCrossAllowanceCount); + BLACKBOX_PRINT_HEADER_LINE("itermThrottleGain:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.itermThrottleGain); + BLACKBOX_PRINT_HEADER_LINE("ptermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSetpointWeight); + BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dtermSetpointWeight); + BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawRateAccelLimit); + BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rateAccelLimit); + // End of Betaflight controller parameters + BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband); BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyro_lpf); + BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyro_soft_type); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d", (int)(masterConfig.gyro_soft_notch_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d", (int)(masterConfig.gyro_soft_notch_cutoff * 100.0f)); @@ -1237,6 +1253,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", masterConfig.use_unsyncedPwm); BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", masterConfig.motor_pwm_protocol); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", masterConfig.motor_pwm_rate); + BLACKBOX_PRINT_HEADER_LINE("debug_mode:%d", masterConfig.debug_mode); BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures); default: From 6618f1992cbe96b8e4b6cc8dc2d467fc7ca92714 Mon Sep 17 00:00:00 2001 From: nathan Date: Wed, 10 Aug 2016 15:00:26 -0700 Subject: [PATCH 302/456] option to set `V=1 make ...` for verbose make output --- Makefile | 112 ++++++++++++++++++++++++++++++------------------------- 1 file changed, 62 insertions(+), 50 deletions(-) diff --git a/Makefile b/Makefile index 81b986cc71..1a81c6cbbe 100644 --- a/Makefile +++ b/Makefile @@ -37,6 +37,18 @@ SERIAL_DEVICE ?= $(firstword $(wildcard /dev/ttyUSB*) no-port-found) # Flash size (KB). Some low-end chips actually have more flash than advertised, use this to override. FLASH_SIZE ?= +# Decide on a verbosity level based on the V= parameter +export AT := @ + +ifndef V +export V0 := +export V1 := $(AT) +else ifeq ($(V), 0) +export V0 := $(AT) +export V1 := $(AT) +else ifeq ($(V), 1) +endif + ############################################################################### # Things that need to be maintained as the source changes # @@ -639,42 +651,42 @@ CLEAN_ARTIFACTS += $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP) # It would be nice to compute these lists, but that seems to be just beyond make. $(TARGET_HEX): $(TARGET_ELF) - $(OBJCOPY) -O ihex --set-start 0x8000000 $< $@ + $(V0) $(OBJCOPY) -O ihex --set-start 0x8000000 $< $@ $(TARGET_BIN): $(TARGET_ELF) - $(OBJCOPY) -O binary $< $@ + $(V0) $(OBJCOPY) -O binary $< $@ $(TARGET_ELF): $(TARGET_OBJS) - @echo LD $(notdir $@) - @$(CC) -o $@ $^ $(LDFLAGS) - $(SIZE) $(TARGET_ELF) + $(V1) echo LD $(notdir $@) + $(V1) $(CC) -o $@ $^ $(LDFLAGS) + $(V0) $(SIZE) $(TARGET_ELF) # Compile $(OBJECT_DIR)/$(TARGET)/%.o: %.c - @mkdir -p $(dir $@) - @echo %% $(notdir $<) - @$(CC) -c -o $@ $(CFLAGS) $< + $(V1) mkdir -p $(dir $@) + $(V1) echo %% $(notdir $<) + $(V1) $(CC) -c -o $@ $(CFLAGS) $< # Assemble $(OBJECT_DIR)/$(TARGET)/%.o: %.s - @mkdir -p $(dir $@) - @echo %% $(notdir $<) - @$(CC) -c -o $@ $(ASFLAGS) $< + $(V1) mkdir -p $(dir $@) + $(V1) echo %% $(notdir $<) + $(V1) $(CC) -c -o $@ $(ASFLAGS) $< $(OBJECT_DIR)/$(TARGET)/%.o: %.S - @mkdir -p $(dir $@) - @echo %% $(notdir $<) - @$(CC) -c -o $@ $(ASFLAGS) $< + $(V1) mkdir -p $(dir $@) + $(V1) echo %% $(notdir $<) + $(V1) $(CC) -c -o $@ $(ASFLAGS) $< ## all : Build all valid targets all: $(VALID_TARGETS) $(VALID_TARGETS): - echo "" && \ - echo "Building $@" && \ - $(MAKE) binary hex TARGET=$@ && \ - echo "Building $@ succeeded." + $(V0) echo "" && \ + $(V0) echo "Building $@" && \ + $(V0) $(MAKE) binary hex TARGET=$@ && \ + $(V0) echo "Building $@ succeeded." @@ -683,22 +695,22 @@ TARGETS_CLEAN = $(addsuffix _clean,$(VALID_TARGETS) ) ## clean : clean up temporary / machine-generated files clean: - echo "Cleaning $(TARGET)" - rm -f $(CLEAN_ARTIFACTS) - rm -rf $(OBJECT_DIR)/$(TARGET) - echo "Cleaning $(TARGET) succeeded." + $(V0) echo "Cleaning $(TARGET)" + $(V0) rm -f $(CLEAN_ARTIFACTS) + $(V0) rm -rf $(OBJECT_DIR)/$(TARGET) + $(V0) echo "Cleaning $(TARGET) succeeded." ## clean_test : clean up temporary / machine-generated files (tests) clean_test: - cd src/test && $(MAKE) clean || true + $(V0) cd src/test && $(MAKE) clean || true ## clean_ : clean up one specific target $(CLEAN_TARGETS) : - $(MAKE) -j TARGET=$(subst clean_,,$@) clean + $(V0) $(MAKE) -j TARGET=$(subst clean_,,$@) clean ## _clean : clean up one specific target (alias for above) $(TARGETS_CLEAN) : - $(MAKE) -j TARGET=$(subst _clean,,$@) clean + $(V0) $(MAKE) -j TARGET=$(subst _clean,,$@) clean ## clean_all : clean all valid targets clean_all:$(CLEAN_TARGETS) @@ -708,62 +720,62 @@ all_clean:$(TARGETS_CLEAN) flash_$(TARGET): $(TARGET_HEX) - stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon - echo -n 'R' >$(SERIAL_DEVICE) - stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE) + $(V0) stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon + $(V0) echo -n 'R' >$(SERIAL_DEVICE) + $(V0) stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE) ## flash : flash firmware (.hex) onto flight controller flash: flash_$(TARGET) st-flash_$(TARGET): $(TARGET_BIN) - st-flash --reset write $< 0x08000000 + $(V0) st-flash --reset write $< 0x08000000 ## st-flash : flash firmware (.bin) onto flight controller st-flash: st-flash_$(TARGET) binary: - $(MAKE) -j $(TARGET_BIN) + $(V0) $(MAKE) -j $(TARGET_BIN) hex: - $(MAKE) -j $(TARGET_HEX) + $(V0) $(MAKE) -j $(TARGET_HEX) unbrick_$(TARGET): $(TARGET_HEX) - stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon - stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE) + $(V0) stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon + $(V0) stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE) ## unbrick : unbrick flight controller unbrick: unbrick_$(TARGET) ## cppcheck : run static analysis on C source code cppcheck: $(CSOURCES) - $(CPPCHECK) + $(V0) $(CPPCHECK) cppcheck-result.xml: $(CSOURCES) - $(CPPCHECK) --xml-version=2 2> cppcheck-result.xml + $(V0) $(CPPCHECK) --xml-version=2 2> cppcheck-result.xml ## help : print this help message and exit help: Makefile - @echo "" - @echo "Makefile for the $(FORKNAME) firmware" - @echo "" - @echo "Usage:" - @echo " make [TARGET=] [OPTIONS=\"\"]" - @echo "Or:" - @echo " make [OPTIONS=\"\"]" - @echo "" - @echo "Valid TARGET values are: $(VALID_TARGETS)" - @echo "" - @sed -n 's/^## //p' $< + $(V0) @echo "" + $(V0) @echo "Makefile for the $(FORKNAME) firmware" + $(V0) @echo "" + $(V0) @echo "Usage:" + $(V0) @echo " make [TARGET=] [OPTIONS=\"\"]" + $(V0) @echo "Or:" + $(V0) @echo " make [OPTIONS=\"\"]" + $(V0) @echo "" + $(V0) @echo "Valid TARGET values are: $(VALID_TARGETS)" + $(V0) @echo "" + $(V0) @sed -n 's/^## //p' $< ## targets : print a list of all valid target platforms (for consumption by scripts) targets: - @echo "Valid targets: $(VALID_TARGETS)" - @echo "Target: $(TARGET)" - @echo "Base target: $(BASE_TARGET)" + $(V0) @echo "Valid targets: $(VALID_TARGETS)" + $(V0) @echo "Target: $(TARGET)" + $(V0) @echo "Base target: $(BASE_TARGET)" ## test : run the cleanflight test suite test: - cd src/test && $(MAKE) test || true + $(V0) cd src/test && $(MAKE) test || true # rebuild everything when makefile changes $(TARGET_OBJS) : Makefile From 490f11c9ae072702290fa60d92c4dac2d63985d3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 11 Aug 2016 09:54:56 +0100 Subject: [PATCH 303/456] Split eeprom code into separate module --- Makefile | 1 + src/main/config/config.c | 198 +----------------------- src/main/config/config.h | 7 +- src/main/config/config_eeprom.c | 266 ++++++++++++++++++++++++++++++++ src/main/config/config_eeprom.h | 26 ++++ src/main/io/osd.c | 1 + src/main/io/serial_cli.c | 1 + src/main/io/serial_msp.c | 1 + src/main/main.c | 1 + 9 files changed, 303 insertions(+), 199 deletions(-) create mode 100644 src/main/config/config_eeprom.c create mode 100644 src/main/config/config_eeprom.h diff --git a/Makefile b/Makefile index 81b986cc71..280d5efb53 100644 --- a/Makefile +++ b/Makefile @@ -366,6 +366,7 @@ COMMON_SRC = \ common/printf.c \ common/typeconversion.c \ config/config.c \ + config/config_eeprom.c \ fc/runtime_config.c \ drivers/adc.c \ drivers/buf_writer.c \ diff --git a/src/main/config/config.c b/src/main/config/config.c index eba97a76ff..2cd127d967 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -76,6 +76,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" @@ -94,79 +95,6 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); void targetConfiguration(void); -#if !defined(FLASH_SIZE) -#error "Flash size not defined for target. (specify in KB)" -#endif - - -#ifndef FLASH_PAGE_SIZE - #ifdef STM32F303xC - #define FLASH_PAGE_SIZE ((uint16_t)0x800) - #endif - - #ifdef STM32F10X_MD - #define FLASH_PAGE_SIZE ((uint16_t)0x400) - #endif - - #ifdef STM32F10X_HD - #define FLASH_PAGE_SIZE ((uint16_t)0x800) - #endif - - #if defined(STM32F40_41xxx) - #define FLASH_PAGE_SIZE ((uint32_t)0x20000) - #endif - - #if defined (STM32F411xE) - #define FLASH_PAGE_SIZE ((uint32_t)0x20000) - #endif - -#endif - -#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT) - #ifdef STM32F10X_MD - #define FLASH_PAGE_COUNT 128 - #endif - - #ifdef STM32F10X_HD - #define FLASH_PAGE_COUNT 128 - #endif -#endif - -#if defined(FLASH_SIZE) -#if defined(STM32F40_41xxx) -#define FLASH_PAGE_COUNT 4 // just to make calculations work -#elif defined (STM32F411xE) -#define FLASH_PAGE_COUNT 4 // just to make calculations work -#else -#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE) -#endif -#endif - -#if !defined(FLASH_PAGE_SIZE) -#error "Flash page size not defined for target." -#endif - -#if !defined(FLASH_PAGE_COUNT) -#error "Flash page count not defined for target." -#endif - -#if FLASH_SIZE <= 128 -#define FLASH_TO_RESERVE_FOR_CONFIG 0x800 -#else -#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 -#ifndef CONFIG_START_FLASH_ADDRESS -#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) -#endif -#endif - master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; static uint32_t activeFeaturesLatch = 0; @@ -174,7 +102,6 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 143; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -412,7 +339,7 @@ uint8_t getCurrentProfile(void) return masterConfig.current_profile_index; } -static void setProfile(uint8_t profileIndex) +void setProfile(uint8_t profileIndex) { currentProfile = &masterConfig.profile[profileIndex]; currentControlRateProfileIndex = currentProfile->activeRateProfile; @@ -717,38 +644,6 @@ static void resetConf(void) #endif } -static uint8_t calculateChecksum(const uint8_t *data, uint32_t length) -{ - uint8_t checksum = 0; - const uint8_t *byteOffset; - - for (byteOffset = data; byteOffset < (data + length); byteOffset++) - checksum ^= *byteOffset; - return checksum; -} - -static bool isEEPROMContentValid(void) -{ - const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS; - uint8_t checksum = 0; - - // check version number - if (EEPROM_CONF_VERSION != temp->version) - return false; - - // check size and magic numbers - if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF) - return false; - - // verify integrity of temporary copy - checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t)); - if (checksum != 0) - return false; - - // looks good, let's roll! - return true; -} - void activateControlRateConfig(void) { generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig); @@ -934,32 +829,6 @@ void validateAndFixConfig(void) } } -void initEEPROM(void) -{ -} - -void readEEPROM(void) -{ - // Sanity check - if (!isEEPROMContentValid()) - failureMode(FAILURE_INVALID_EEPROM_CONTENTS); - - suspendRxSignal(); - - // Read flash - memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t)); - - if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check - masterConfig.current_profile_index = 0; - - setProfile(masterConfig.current_profile_index); - - validateAndFixConfig(); - activateConfig(); - - resumeRxSignal(); -} - void readEEPROMAndNotify(void) { // re-read written data @@ -967,69 +836,6 @@ void readEEPROMAndNotify(void) beeperConfirmationBeeps(1); } -void writeEEPROM(void) -{ - // Generate compile time error if the config does not fit in the reserved area of flash. - BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG); - - FLASH_Status status = 0; - uint32_t wordOffset; - int8_t attemptsRemaining = 3; - - suspendRxSignal(); - - // prepare checksum/version constants - masterConfig.version = EEPROM_CONF_VERSION; - masterConfig.size = sizeof(master_t); - masterConfig.magic_be = 0xBE; - masterConfig.magic_ef = 0xEF; - masterConfig.chk = 0; // erase checksum before recalculating - masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t)); - - // write it - FLASH_Unlock(); - while (attemptsRemaining--) { -#if defined(STM32F4) - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); -#elif defined(STM32F303) - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); -#elif defined(STM32F10X) - FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); -#endif - for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) { - if (wordOffset % FLASH_PAGE_SIZE == 0) { -#if defined(STM32F40_41xxx) - status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000 -#elif defined (STM32F411xE) - status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000 -#else - status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset); -#endif - if (status != FLASH_COMPLETE) { - break; - } - } - - status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset, - *(uint32_t *) ((char *) &masterConfig + wordOffset)); - if (status != FLASH_COMPLETE) { - break; - } - } - if (status == FLASH_COMPLETE) { - break; - } - } - FLASH_Lock(); - - // Flash write failed - just die now - if (status != FLASH_COMPLETE || !isEEPROMContentValid()) { - failureMode(FAILURE_FLASH_WRITE_FAILED); - } - - resumeRxSignal(); -} - void ensureEEPROMContainsValidData(void) { if (isEEPROMContentValid()) { diff --git a/src/main/config/config.h b/src/main/config/config.h index 97e9166289..3092e6a6a3 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -73,16 +73,17 @@ void setPreferredBeeperOffMask(uint32_t mask); void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex); -void initEEPROM(void); void resetEEPROM(void); -void readEEPROM(void); void readEEPROMAndNotify(void); -void writeEEPROM(); void ensureEEPROMContainsValidData(void); + void saveConfigAndNotify(void); +void validateAndFixConfig(void); +void activateConfig(void); uint8_t getCurrentProfile(void); void changeProfile(uint8_t profileIndex); +void setProfile(uint8_t profileIndex); uint8_t getCurrentControlRateProfile(void); void changeControlRateProfile(uint8_t profileIndex); diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c new file mode 100644 index 0000000000..e52d3050b3 --- /dev/null +++ b/src/main/config/config_eeprom.c @@ -0,0 +1,266 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" + +#include "common/color.h" +#include "common/axis.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/config.h" +#include "config/config_eeprom.h" +#include "config/config_profile.h" +#include "config/config_master.h" + +#if !defined(FLASH_SIZE) +#error "Flash size not defined for target. (specify in KB)" +#endif + + +#ifndef FLASH_PAGE_SIZE + #ifdef STM32F303xC + #define FLASH_PAGE_SIZE ((uint16_t)0x800) + #endif + + #ifdef STM32F10X_MD + #define FLASH_PAGE_SIZE ((uint16_t)0x400) + #endif + + #ifdef STM32F10X_HD + #define FLASH_PAGE_SIZE ((uint16_t)0x800) + #endif + + #if defined(STM32F40_41xxx) + #define FLASH_PAGE_SIZE ((uint32_t)0x20000) + #endif + + #if defined (STM32F411xE) + #define FLASH_PAGE_SIZE ((uint32_t)0x20000) + #endif + +#endif + +#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT) + #ifdef STM32F10X_MD + #define FLASH_PAGE_COUNT 128 + #endif + + #ifdef STM32F10X_HD + #define FLASH_PAGE_COUNT 128 + #endif +#endif + +#if defined(FLASH_SIZE) +#if defined(STM32F40_41xxx) +#define FLASH_PAGE_COUNT 4 // just to make calculations work +#elif defined (STM32F411xE) +#define FLASH_PAGE_COUNT 4 // just to make calculations work +#else +#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE) +#endif +#endif + +#if !defined(FLASH_PAGE_SIZE) +#error "Flash page size not defined for target." +#endif + +#if !defined(FLASH_PAGE_COUNT) +#error "Flash page count not defined for target." +#endif + +#if FLASH_SIZE <= 128 +#define FLASH_TO_RESERVE_FOR_CONFIG 0x800 +#else +#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 +#ifndef CONFIG_START_FLASH_ADDRESS +#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG)) +#endif +#endif + + +void initEEPROM(void) +{ +} + +static uint8_t calculateChecksum(const uint8_t *data, uint32_t length) +{ + uint8_t checksum = 0; + const uint8_t *byteOffset; + + for (byteOffset = data; byteOffset < (data + length); byteOffset++) + checksum ^= *byteOffset; + return checksum; +} + +bool isEEPROMContentValid(void) +{ + const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS; + uint8_t checksum = 0; + + // check version number + if (EEPROM_CONF_VERSION != temp->version) + return false; + + // check size and magic numbers + if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF) + return false; + + // verify integrity of temporary copy + checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t)); + if (checksum != 0) + return false; + + // looks good, let's roll! + return true; +} + +void writeEEPROM(void) +{ + // Generate compile time error if the config does not fit in the reserved area of flash. + BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG); + + FLASH_Status status = 0; + uint32_t wordOffset; + int8_t attemptsRemaining = 3; + + suspendRxSignal(); + + // prepare checksum/version constants + masterConfig.version = EEPROM_CONF_VERSION; + masterConfig.size = sizeof(master_t); + masterConfig.magic_be = 0xBE; + masterConfig.magic_ef = 0xEF; + masterConfig.chk = 0; // erase checksum before recalculating + masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t)); + + // write it + FLASH_Unlock(); + while (attemptsRemaining--) { +#if defined(STM32F4) + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); +#elif defined(STM32F303) + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); +#elif defined(STM32F10X) + FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); +#endif + for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) { + if (wordOffset % FLASH_PAGE_SIZE == 0) { +#if defined(STM32F40_41xxx) + status = FLASH_EraseSector(FLASH_Sector_8, VoltageRange_3); //0x08080000 to 0x080A0000 +#elif defined (STM32F411xE) + status = FLASH_EraseSector(FLASH_Sector_7, VoltageRange_3); //0x08060000 to 0x08080000 +#else + status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset); +#endif + if (status != FLASH_COMPLETE) { + break; + } + } + + status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset, + *(uint32_t *) ((char *) &masterConfig + wordOffset)); + if (status != FLASH_COMPLETE) { + break; + } + } + if (status == FLASH_COMPLETE) { + break; + } + } + FLASH_Lock(); + + // Flash write failed - just die now + if (status != FLASH_COMPLETE || !isEEPROMContentValid()) { + failureMode(FAILURE_FLASH_WRITE_FAILED); + } + + resumeRxSignal(); +} + +void readEEPROM(void) +{ + // Sanity check + if (!isEEPROMContentValid()) + failureMode(FAILURE_INVALID_EEPROM_CONTENTS); + + suspendRxSignal(); + + // Read flash + memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t)); + + if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check + masterConfig.current_profile_index = 0; + + setProfile(masterConfig.current_profile_index); + + validateAndFixConfig(); + activateConfig(); + + resumeRxSignal(); +} + diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h new file mode 100644 index 0000000000..b48961f91f --- /dev/null +++ b/src/main/config/config_eeprom.h @@ -0,0 +1,26 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define EEPROM_CONF_VERSION 143 + +void initEEPROM(void); +void writeEEPROM(); +void readEEPROM(void); +bool isEEPROMContentValid(void); + diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 5d7aaba1ff..7a8e8db621 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -100,6 +100,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 74d3b4ab28..e58841a922 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -86,6 +86,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index c625ee847d..88d87bc592 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -91,6 +91,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" diff --git a/src/main/main.c b/src/main/main.c index c642eef763..9c4336e7cd 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -105,6 +105,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" From cd18004bf4a3cfcf2f437d91b276a51f451ded26 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 11 Aug 2016 16:15:16 +0100 Subject: [PATCH 304/456] Fixed error in gyro detection --- src/main/sensors/initialisation.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 3b25e0b564..a122c8aacc 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -167,8 +167,8 @@ bool detectGyro(void) case GYRO_MPU6050: #ifdef USE_GYRO_MPU6050 if (mpu6050GyroDetect(&gyro)) { -#ifdef GYRO_MPU6050_ALIGN gyroHardware = GYRO_MPU6050; +#ifdef GYRO_MPU6050_ALIGN gyroAlign = GYRO_MPU6050_ALIGN; #endif break; @@ -178,8 +178,8 @@ bool detectGyro(void) case GYRO_L3G4200D: #ifdef USE_GYRO_L3G4200D if (l3g4200dDetect(&gyro)) { -#ifdef GYRO_L3G4200D_ALIGN gyroHardware = GYRO_L3G4200D; +#ifdef GYRO_L3G4200D_ALIGN gyroAlign = GYRO_L3G4200D_ALIGN; #endif break; @@ -190,8 +190,8 @@ bool detectGyro(void) case GYRO_MPU3050: #ifdef USE_GYRO_MPU3050 if (mpu3050Detect(&gyro)) { -#ifdef GYRO_MPU3050_ALIGN gyroHardware = GYRO_MPU3050; +#ifdef GYRO_MPU3050_ALIGN gyroAlign = GYRO_MPU3050_ALIGN; #endif break; @@ -202,8 +202,8 @@ bool detectGyro(void) case GYRO_L3GD20: #ifdef USE_GYRO_L3GD20 if (l3gd20Detect(&gyro)) { -#ifdef GYRO_L3GD20_ALIGN gyroHardware = GYRO_L3GD20; +#ifdef GYRO_L3GD20_ALIGN gyroAlign = GYRO_L3GD20_ALIGN; #endif break; @@ -214,8 +214,8 @@ bool detectGyro(void) case GYRO_MPU6000: #ifdef USE_GYRO_SPI_MPU6000 if (mpu6000SpiGyroDetect(&gyro)) { -#ifdef GYRO_MPU6000_ALIGN gyroHardware = GYRO_MPU6000; +#ifdef GYRO_MPU6000_ALIGN gyroAlign = GYRO_MPU6000_ALIGN; #endif break; From 147a16a66d01db08962ba0838e3a7b986f5adf44 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 11 Aug 2016 17:44:54 +0100 Subject: [PATCH 305/456] Whitespace and const correctness tidy --- src/main/drivers/accgyro_mpu6050.h | 2 +- src/main/drivers/accgyro_spi_mpu6500.h | 2 +- src/main/drivers/barometer.h | 2 +- src/main/drivers/compass.h | 2 +- src/main/drivers/io.c | 4 ++-- src/main/sensors/acceleration.c | 2 +- src/main/sensors/boardalignment.c | 6 +++--- src/main/sensors/boardalignment.h | 4 ++-- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/drivers/accgyro_mpu6050.h b/src/main/drivers/accgyro_mpu6050.h index 3e947f8fff..d8649a3c6f 100644 --- a/src/main/drivers/accgyro_mpu6050.h +++ b/src/main/drivers/accgyro_mpu6050.h @@ -18,4 +18,4 @@ #pragma once bool mpu6050AccDetect(acc_t *acc); -bool mpu6050GyroDetect(gyro_t *gyro); \ No newline at end of file +bool mpu6050GyroDetect(gyro_t *gyro); diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h index 8b2fe95827..a8d5cf514a 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.h +++ b/src/main/drivers/accgyro_spi_mpu6500.h @@ -23,4 +23,4 @@ bool mpu6500SpiAccDetect(acc_t *acc); bool mpu6500SpiGyroDetect(gyro_t *gyro); bool mpu6500WriteRegister(uint8_t reg, uint8_t data); -bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); \ No newline at end of file +bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data); diff --git a/src/main/drivers/barometer.h b/src/main/drivers/barometer.h index 1d9d8f8914..dec11ad3c2 100644 --- a/src/main/drivers/barometer.h +++ b/src/main/drivers/barometer.h @@ -32,4 +32,4 @@ typedef struct baro_s { #ifndef BARO_I2C_INSTANCE #define BARO_I2C_INSTANCE I2C_DEVICE -#endif \ No newline at end of file +#endif diff --git a/src/main/drivers/compass.h b/src/main/drivers/compass.h index 68d44442f4..b20ae332bc 100644 --- a/src/main/drivers/compass.h +++ b/src/main/drivers/compass.h @@ -24,4 +24,4 @@ typedef struct mag_s { #ifndef MAG_I2C_INSTANCE #define MAG_I2C_INSTANCE I2C_DEVICE -#endif \ No newline at end of file +#endif diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index 0ba9d9169b..6a5761979f 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -207,8 +207,8 @@ void IOInit(IO_t io, resourceOwner_t owner, resourceType_t resource, uint8_t ind { ioRec_t *ioRec = IO_Rec(io); ioRec->owner = owner; - ioRec->resource = resource; - ioRec->index = index; + ioRec->resource = resource; + ioRec->index = index; } void IORelease(IO_t io) diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index c8600d4307..3016e97325 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -167,7 +167,7 @@ static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndP } } -static void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims) +static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims) { accSmooth[X] -= accelerationTrims->raw[X]; accSmooth[Y] -= accelerationTrims->raw[Y]; diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c index d7a6784c80..42ede9155d 100644 --- a/src/main/sensors/boardalignment.c +++ b/src/main/sensors/boardalignment.c @@ -30,12 +30,12 @@ static bool standardBoardAlignment = true; // board orientation correction static float boardRotation[3][3]; // matrix -static bool isBoardAlignmentStandard(boardAlignment_t *boardAlignment) +static bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment) { return !boardAlignment->rollDegrees && !boardAlignment->pitchDegrees && !boardAlignment->yawDegrees; } -void initBoardAlignment(boardAlignment_t *boardAlignment) +void initBoardAlignment(const boardAlignment_t *boardAlignment) { if (isBoardAlignmentStandard(boardAlignment)) { return; @@ -62,7 +62,7 @@ static void alignBoard(int32_t *vec) vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z); } -void alignSensors(int32_t *src, int32_t *dest, uint8_t rotation) +void alignSensors(const int32_t *src, int32_t *dest, uint8_t rotation) { static uint32_t swap[3]; memcpy(swap, src, sizeof(swap)); diff --git a/src/main/sensors/boardalignment.h b/src/main/sensors/boardalignment.h index eea1f23c87..e1e704be03 100644 --- a/src/main/sensors/boardalignment.h +++ b/src/main/sensors/boardalignment.h @@ -23,5 +23,5 @@ typedef struct boardAlignment_s { int32_t yawDegrees; } boardAlignment_t; -void alignSensors(int32_t *src, int32_t *dest, uint8_t rotation); -void initBoardAlignment(boardAlignment_t *boardAlignment); +void alignSensors(const int32_t *src, int32_t *dest, uint8_t rotation); +void initBoardAlignment(const boardAlignment_t *boardAlignment); From 0d057ddf0dece041ea7637d0643976591ee6fcc7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 11 Aug 2016 19:19:24 +0200 Subject: [PATCH 306/456] Fix crash bug on diff // dump command --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e1abbbeca6..4164f4e14a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2309,7 +2309,7 @@ static void printConfig(char *cmdline, bool doDiff) options = cmdline; } - master_t defaultConfig; + static master_t defaultConfig; if (doDiff) { dumpMask = dumpMask | DO_DIFF; createDefaultConfig(&defaultConfig); From 5642f8bfd69a4b9990fe316b8f2a18c067e0cc14 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 11 Aug 2016 19:19:24 +0200 Subject: [PATCH 307/456] Fix crash bug on diff // dump command --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9cf5d24a83..ca016edd54 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2310,7 +2310,7 @@ static void printConfig(char *cmdline, bool doDiff) options = cmdline; } - master_t defaultConfig; + static master_t defaultConfig; if (doDiff) { dumpMask = dumpMask | DO_DIFF; createDefaultConfig(&defaultConfig); From 08776bc30984a92a8aeeff090883e1162e06e033 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 11 Aug 2016 20:07:55 +0200 Subject: [PATCH 308/456] Fix incorrect coefficient when pid_denom > 1 is used --- src/main/flight/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2e136af636..829b62c0f3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -115,13 +115,13 @@ void initFilters(const pidProfile_t *pidProfile) { if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); - for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, notchQ, FILTER_NOTCH); + for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH); dtermNotchInitialised = true; } if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) { - for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, gyro.targetLooptime); + for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); dtermBiquadLpfInitialised = true; } } From 8140959f2d94beb0a6993b3451de832f70e48132 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 11 Aug 2016 20:07:55 +0200 Subject: [PATCH 309/456] Fix incorrect coefficient when pid_denom > 1 is used --- src/main/flight/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b9d82cac35..0f8d30fb9b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -113,13 +113,13 @@ void initFilters(const pidProfile_t *pidProfile) if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) { float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); - for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, gyro.targetLooptime, notchQ, FILTER_NOTCH); + for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH); dtermNotchInitialised = true; } if (pidProfile->dterm_filter_type == FILTER_BIQUAD) { if (pidProfile->dterm_lpf_hz && !dtermBiquadLpfInitialised) { - for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, gyro.targetLooptime); + for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); dtermBiquadLpfInitialised = true; } } From 74212518a6d6f8b04ebc0d93809c6096d2bb2b8b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 9 Aug 2016 15:18:21 +0200 Subject: [PATCH 310/456] Cleanup some target junk --- src/main/target/COLIBRI_RACE/target.h | 1 - src/main/target/MOTOLAB/target.h | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index d4db45a6de..a68d7a71d9 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -113,7 +113,6 @@ #define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP -#define USE_COLIBTI_RACE_LED_DEFAULT_CONFIG #define WS2811_PIN PA6 // TIM16_CH1 #define WS2811_TIMER TIM16 diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index fe465da9c9..beb5431c7a 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -21,6 +21,7 @@ #define USE_CLI #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT +#define TARGET_CONFIG #define LED0 PB5 // Blue LEDs - PB5 //#define LED1 PB9 // Green LEDs - PB9 From 2c9776861d374d77ed1ff90d142be6c6eab4d34d Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 12 Aug 2016 05:17:54 +0200 Subject: [PATCH 311/456] AlienFlight config fix --- src/main/target/ALIENFLIGHTF1/config.c | 12 ++++++------ src/main/target/ALIENFLIGHTF3/config.c | 12 ++++++------ src/main/target/ALIENFLIGHTF4/config.c | 12 ++++++------ 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index b16a16444a..b34f9d5a37 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -77,12 +77,12 @@ void targetConfiguration(void) masterConfig.motor_pwm_rate = 32000; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; - currentProfile->pidProfile.P8[ROLL] = 90; - currentProfile->pidProfile.I8[ROLL] = 44; - currentProfile->pidProfile.D8[ROLL] = 60; - currentProfile->pidProfile.P8[PITCH] = 90; - currentProfile->pidProfile.I8[PITCH] = 44; - currentProfile->pidProfile.D8[PITCH] = 60; + masterConfig.profile[0].pidProfile.P8[ROLL] = 90; + masterConfig.profile[0].pidProfile.I8[ROLL] = 44; + masterConfig.profile[0].pidProfile.D8[ROLL] = 60; + masterConfig.profile[0].pidProfile.P8[PITCH] = 90; + masterConfig.profile[0].pidProfile.I8[PITCH] = 44; + masterConfig.profile[0].pidProfile.D8[PITCH] = 60; masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 3aed5a3ceb..650da37c11 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -85,12 +85,12 @@ void targetConfiguration(void) { masterConfig.failsafeConfig.failsafe_off_delay = 0; masterConfig.gyro_sync_denom = 2; masterConfig.pid_process_denom = 1; - currentProfile->pidProfile.P8[ROLL] = 90; - currentProfile->pidProfile.I8[ROLL] = 44; - currentProfile->pidProfile.D8[ROLL] = 60; - currentProfile->pidProfile.P8[PITCH] = 90; - currentProfile->pidProfile.I8[PITCH] = 44; - currentProfile->pidProfile.D8[PITCH] = 60; + masterConfig.profile[0].pidProfile.P8[ROLL] = 90; + masterConfig.profile[0].pidProfile.I8[ROLL] = 44; + masterConfig.profile[0].pidProfile.D8[ROLL] = 60; + masterConfig.profile[0].pidProfile.P8[PITCH] = 90; + masterConfig.profile[0].pidProfile.I8[PITCH] = 44; + masterConfig.profile[0].pidProfile.D8[PITCH] = 60; masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 5264a16735..b156230393 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -85,12 +85,12 @@ void targetConfiguration(void) { masterConfig.failsafeConfig.failsafe_off_delay = 0; masterConfig.gyro_sync_denom = 1; masterConfig.pid_process_denom = 1; - currentProfile->pidProfile.P8[ROLL] = 90; - currentProfile->pidProfile.I8[ROLL] = 44; - currentProfile->pidProfile.D8[ROLL] = 60; - currentProfile->pidProfile.P8[PITCH] = 90; - currentProfile->pidProfile.I8[PITCH] = 44; - currentProfile->pidProfile.D8[PITCH] = 60; + masterConfig.profile[0].pidProfile.P8[ROLL] = 90; + masterConfig.profile[0].pidProfile.I8[ROLL] = 44; + masterConfig.profile[0].pidProfile.D8[ROLL] = 60; + masterConfig.profile[0].pidProfile.P8[PITCH] = 90; + masterConfig.profile[0].pidProfile.I8[PITCH] = 44; + masterConfig.profile[0].pidProfile.D8[PITCH] = 60; masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R From d10e6a8f87a5536168fd440b244759f2b9b628b1 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 12 Aug 2016 05:35:00 +0200 Subject: [PATCH 312/456] Fix maxthrottle value for brushed targets --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index fd74196200..07605b3f37 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -311,9 +311,9 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) #ifdef BRUSHED_MOTORS escAndServoConfig->minthrottle = 1000; #else - escAndServoConfig->maxthrottle = 2000; escAndServoConfig->minthrottle = 1070; #endif + escAndServoConfig->maxthrottle = 2000; escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; } From ed5452d97b8f3f181ed6ed688219287f69509a8b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 12 Aug 2016 06:57:38 +0100 Subject: [PATCH 313/456] Added FIR filter --- src/main/common/filter.c | 30 ++++++++++++++++++++++++++++++ src/main/common/filter.h | 13 +++++++++++++ 2 files changed, 43 insertions(+) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 70b77ad289..3a4cbca7a1 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -17,6 +17,7 @@ #include #include +#include #include #include "common/filter.h" @@ -137,3 +138,32 @@ float filterApplyAveragef(float input, uint8_t averageCount, float averageState[ return averageSum / averageCount; } +// FIR filter +void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength) +{ + filter->buf = buf; + filter->bufLength = bufLength; + filter->coeffs = coeffs; + filter->coeffsLength = coeffsLength; + memset(filter->buf, 0, sizeof(float) * filter->bufLength); +} + +void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs) +{ + firFilterInit2(filter, buf, bufLength, coeffs, bufLength); +} + +void firFilterUpdate(firFilter_t *filter, float input) +{ + memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(float)); + filter->buf[0] = input; +} + +float firFilterApply(firFilter_t *filter) +{ + float ret = 0.0f; + for (int ii = 0; ii < filter->coeffsLength; ++ii) { + ret += filter->coeffs[ii] * filter->buf[ii]; + } + return ret; +} diff --git a/src/main/common/filter.h b/src/main/common/filter.h index c584a23c86..8e29a79270 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -32,6 +32,7 @@ typedef struct biquadFilter_s { typedef enum { FILTER_PT1 = 0, FILTER_BIQUAD, + FILTER_FIR, } filterType_e; typedef enum { @@ -39,6 +40,13 @@ typedef enum { FILTER_NOTCH } biquadFilterType_e; +typedef struct firFilter_s { + float *buf; + const float *coeffs; + uint8_t bufLength; + uint8_t coeffsLength; +} firFilter_t; + void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); float biquadFilterApply(biquadFilter_t *filter, float input); @@ -51,3 +59,8 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT) int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]); float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]); +void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs); +void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength); +void firFilterUpdate(firFilter_t *filter, float input); +float firFilterApply(firFilter_t *filter); + From cdea3adb6e1d2130d0a72e2997a0730f6179b21a Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 13 Aug 2016 00:47:19 +0200 Subject: [PATCH 314/456] Add new filters to MSP --- src/main/io/serial_msp.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index f4646c3905..3844595c3b 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1251,10 +1251,14 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(masterConfig.motor_pwm_rate); break; case MSP_FILTER_CONFIG : - headSerialReply(5); + headSerialReply(13); serialize8(masterConfig.gyro_soft_lpf_hz); serialize16(currentProfile->pidProfile.dterm_lpf_hz); serialize16(currentProfile->pidProfile.yaw_lpf_hz); + serialize16(masterConfig.gyro_soft_notch_hz); + serialize16(masterConfig.gyro_soft_notch_cutoff); + serialize16(currentProfile->pidProfile.dterm_notch_hz); + serialize16(currentProfile->pidProfile.dterm_notch_cutoff); break; case MSP_PID_ADVANCED: headSerialReply(17); @@ -1846,6 +1850,12 @@ static bool processInCommand(void) masterConfig.gyro_soft_lpf_hz = read8(); currentProfile->pidProfile.dterm_lpf_hz = read16(); currentProfile->pidProfile.yaw_lpf_hz = read16(); + if (currentPort->dataSize > 5) { + masterConfig.gyro_soft_notch_hz = read16(); + masterConfig.gyro_soft_notch_cutoff = read16(); + currentProfile->pidProfile.dterm_notch_hz = read16(); + currentProfile->pidProfile.dterm_notch_cutoff = read16(); + } break; case MSP_SET_PID_ADVANCED: currentProfile->pidProfile.rollPitchItermIgnoreRate = read16(); From ab15c5a4183314994742eb0eb2813f1d2ec4bc8b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 13 Aug 2016 00:47:19 +0200 Subject: [PATCH 315/456] Add new filters to MSP --- src/main/io/serial_msp.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index bf6e0745d6..40580cbdb0 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1258,10 +1258,14 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(masterConfig.motor_pwm_rate); break; case MSP_FILTER_CONFIG : - headSerialReply(5); + headSerialReply(13); serialize8(masterConfig.gyro_soft_lpf_hz); serialize16(currentProfile->pidProfile.dterm_lpf_hz); serialize16(currentProfile->pidProfile.yaw_lpf_hz); + serialize16(masterConfig.gyro_soft_notch_hz); + serialize16(masterConfig.gyro_soft_notch_cutoff); + serialize16(currentProfile->pidProfile.dterm_notch_hz); + serialize16(currentProfile->pidProfile.dterm_notch_cutoff); break; case MSP_PID_ADVANCED: headSerialReply(17); @@ -1857,6 +1861,12 @@ static bool processInCommand(void) masterConfig.gyro_soft_lpf_hz = read8(); currentProfile->pidProfile.dterm_lpf_hz = read16(); currentProfile->pidProfile.yaw_lpf_hz = read16(); + if (currentPort->dataSize > 5) { + masterConfig.gyro_soft_notch_hz = read16(); + masterConfig.gyro_soft_notch_cutoff = read16(); + currentProfile->pidProfile.dterm_notch_hz = read16(); + currentProfile->pidProfile.dterm_notch_cutoff = read16(); + } break; case MSP_SET_PID_ADVANCED: currentProfile->pidProfile.rollPitchItermIgnoreRate = read16(); From 9c303d666930b2407e93d543fe11dcdbe60ad773 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 13 Aug 2016 11:23:35 +1000 Subject: [PATCH 316/456] VCP improvements to remove need for delay in serial_cli for F4 targets. --- src/main/drivers/serial.c | 2 +- src/main/drivers/serial.h | 4 +-- src/main/drivers/serial_softserial.c | 2 +- src/main/drivers/serial_softserial.h | 2 +- src/main/drivers/serial_uart.c | 2 +- src/main/drivers/serial_uart.h | 2 +- src/main/drivers/serial_usb_vcp.c | 7 ++--- src/main/io/serial_cli.c | 17 ++--------- src/main/target/common.h | 15 +++++----- src/main/vcp/hw_config.c | 11 +++++++ src/main/vcp/hw_config.h | 6 ++-- src/main/vcpf4/usbd_cdc_vcp.c | 35 ++++++++++++---------- src/main/vcpf4/usbd_cdc_vcp.h | 5 ++-- src/test/unit/telemetry_hott_unittest.cc | 37 +++++++++++++++--------- 14 files changed, 82 insertions(+), 65 deletions(-) diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c index 8a17ce6793..c6cab03d77 100644 --- a/src/main/drivers/serial.c +++ b/src/main/drivers/serial.c @@ -61,7 +61,7 @@ uint32_t serialRxBytesWaiting(serialPort_t *instance) return instance->vTable->serialTotalRxWaiting(instance); } -uint8_t serialTxBytesFree(serialPort_t *instance) +uint32_t serialTxBytesFree(serialPort_t *instance) { return instance->vTable->serialTotalTxFree(instance); } diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index 1f75e931d3..fa855e835a 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -63,7 +63,7 @@ struct serialPortVTable { void (*serialWrite)(serialPort_t *instance, uint8_t ch); uint32_t (*serialTotalRxWaiting)(serialPort_t *instance); - uint8_t (*serialTotalTxFree)(serialPort_t *instance); + uint32_t (*serialTotalTxFree)(serialPort_t *instance); uint8_t (*serialRead)(serialPort_t *instance); @@ -82,7 +82,7 @@ struct serialPortVTable { void serialWrite(serialPort_t *instance, uint8_t ch); uint32_t serialRxBytesWaiting(serialPort_t *instance); -uint8_t serialTxBytesFree(serialPort_t *instance); +uint32_t serialTxBytesFree(serialPort_t *instance); void serialWriteBuf(serialPort_t *instance, uint8_t *data, int count); uint8_t serialRead(serialPort_t *instance); void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate); diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index a5e41c8ec7..03ecf57456 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -412,7 +412,7 @@ uint32_t softSerialRxBytesWaiting(serialPort_t *instance) return (s->port.rxBufferHead - s->port.rxBufferTail) & (s->port.rxBufferSize - 1); } -uint8_t softSerialTxBytesFree(serialPort_t *instance) +uint32_t softSerialTxBytesFree(serialPort_t *instance) { if ((instance->mode & MODE_TX) == 0) { return 0; diff --git a/src/main/drivers/serial_softserial.h b/src/main/drivers/serial_softserial.h index af80423b07..eb569c9fa5 100644 --- a/src/main/drivers/serial_softserial.h +++ b/src/main/drivers/serial_softserial.h @@ -29,7 +29,7 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb // serialPort API void softSerialWriteByte(serialPort_t *instance, uint8_t ch); uint32_t softSerialRxBytesWaiting(serialPort_t *instance); -uint8_t softSerialTxBytesFree(serialPort_t *instance); +uint32_t softSerialTxBytesFree(serialPort_t *instance); uint8_t softSerialReadByte(serialPort_t *instance); void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate); bool isSoftSerialTransmitBufferEmpty(serialPort_t *s); diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 68a43289e2..88f422f2e4 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -315,7 +315,7 @@ uint32_t uartTotalRxBytesWaiting(serialPort_t *instance) } } -uint8_t uartTotalTxBytesFree(serialPort_t *instance) +uint32_t uartTotalTxBytesFree(serialPort_t *instance) { uartPort_t *s = (uartPort_t*)instance; diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index 33dde32167..a8c0d64b2f 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -66,7 +66,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, // serialPort API void uartWrite(serialPort_t *instance, uint8_t ch); uint32_t uartTotalRxBytesWaiting(serialPort_t *instance); -uint8_t uartTotalTxBytesFree(serialPort_t *instance); +uint32_t uartTotalTxBytesFree(serialPort_t *instance); uint8_t uartRead(serialPort_t *instance); void uartSetBaudRate(serialPort_t *s, uint32_t baudRate); bool isUartTransmitBufferEmpty(serialPort_t *s); diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 81d71a7ed3..697f7bca37 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -69,7 +69,7 @@ static uint32_t usbVcpAvailable(serialPort_t *instance) { UNUSED(instance); - return receiveLength; + return CDC_Receive_BytesAvailable(); } static uint8_t usbVcpRead(serialPort_t *instance) @@ -150,10 +150,9 @@ static void usbVcpBeginWrite(serialPort_t *instance) port->buffering = true; } -uint8_t usbTxBytesFree() +uint32_t usbTxBytesFree() { - // Because we block upon transmit and don't buffer bytes, our "buffer" capacity is effectively unlimited. - return 255; + return CDC_Send_FreeBytes(); } static void usbVcpEndWrite(serialPort_t *instance) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f4386f9a35..73f6a936bd 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2952,12 +2952,9 @@ static void cliDefaults(char *cmdline) static void cliPrint(const char *str) { - while (*str) - bufWriterAppend(cliWriter, *str++); - -#ifdef USE_SLOW_SERIAL_CLI - delay(1); -#endif + while (*str) { + bufWriterAppend(cliWriter, *str++); + } } static void cliPutp(void *p, char ch) @@ -2979,10 +2976,6 @@ static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *form tfp_format(cliWriter, cliPutp, format, va); va_end(va); -#ifdef USE_SLOW_SERIAL_CLI - delay(1); -#endif - return true; } @@ -2995,10 +2988,6 @@ static void cliPrintf(const char *fmt, ...) va_start(va, fmt); tfp_format(cliWriter, cliPutp, fmt, va); va_end(va); - -#ifdef USE_SLOW_SERIAL_CLI - delay(1); -#endif } static void cliWrite(uint8_t ch) diff --git a/src/main/target/common.h b/src/main/target/common.h index 0e74b56c89..4e3b557191 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -24,17 +24,16 @@ /* STM32F4 specific settings that apply to all F4 targets */ #ifdef STM32F4 -#define MAX_AUX_CHANNELS 99 -#define TASK_GYROPID_DESIRED_PERIOD 125 -#define SCHEDULER_DELAY_LIMIT 10 -#define USE_SLOW_SERIAL_CLI -#define I2C3_OVERCLOCK true +#define MAX_AUX_CHANNELS 99 +#define TASK_GYROPID_DESIRED_PERIOD 125 +#define SCHEDULER_DELAY_LIMIT 10 +#define I2C3_OVERCLOCK true #else /* when not an F4 */ -#define MAX_AUX_CHANNELS 6 -#define TASK_GYROPID_DESIRED_PERIOD 1000 -#define SCHEDULER_DELAY_LIMIT 100 +#define MAX_AUX_CHANNELS 6 +#define TASK_GYROPID_DESIRED_PERIOD 1000 +#define SCHEDULER_DELAY_LIMIT 100 #endif diff --git a/src/main/vcp/hw_config.c b/src/main/vcp/hw_config.c index 1f7f471eee..db1b5d30c9 100644 --- a/src/main/vcp/hw_config.c +++ b/src/main/vcp/hw_config.c @@ -305,6 +305,12 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) return sendLength; } +uint32_t CDC_Send_FreeBytes(void) +{ + /* this driver is blocking, so the buffer is unlimited */ + return 255; +} + /******************************************************************************* * Function Name : Receive DATA . * Description : receive the data from the PC to STM32 and send it through USB @@ -338,6 +344,11 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) return len; } +uint32_t CDC_Receive_BytesAvailable(void) +{ + return receiveLength; +} + /******************************************************************************* * Function Name : usbIsConfigured. * Description : Determines if USB VCP is configured or not diff --git a/src/main/vcp/hw_config.h b/src/main/vcp/hw_config.h index de454859f3..aebac70e82 100644 --- a/src/main/vcp/hw_config.h +++ b/src/main/vcp/hw_config.h @@ -56,13 +56,15 @@ void USB_Interrupts_Config(void); void USB_Cable_Config(FunctionalState NewState); void Get_SerialNum(void); uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength); // HJI +uint32_t CDC_Send_FreeBytes(void); uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len); // HJI +uint32_t CDC_Receive_BytesAvailable(void); + uint8_t usbIsConfigured(void); // HJI uint8_t usbIsConnected(void); // HJI uint32_t CDC_BaudRate(void); -/* External variables --------------------------------------------------------*/ -extern __IO uint32_t receiveLength; // HJI +/* External variables --------------------------------------------------------*/ extern __IO uint32_t packetSent; // HJI #endif /*__HW_CONFIG_H*/ diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 99f9f5c04e..e27fd3657c 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -39,12 +39,10 @@ __IO uint32_t bDeviceState = UNCONNECTED; /* USB device status */ /* This is the buffer for data received from the MCU to APP (i.e. MCU TX, APP RX) */ extern uint8_t APP_Rx_Buffer[]; extern uint32_t APP_Rx_ptr_out; - /* Increment this buffer position or roll it back to start address when writing received data in the buffer APP_Rx_Buffer. */ extern uint32_t APP_Rx_ptr_in; -__IO uint32_t receiveLength = 0; /* APP TX is the circular buffer for data that is transmitted from the APP (host) @@ -155,7 +153,7 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len) /******************************************************************************* * Function Name : Send DATA . * Description : send the data received from the STM32 to the PC through USB - * Input : None. + * Input : buffer to send, and the length of the buffer. * Output : None. * Return : None. *******************************************************************************/ @@ -165,10 +163,15 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) return sendLength; } +uint32_t CDC_Send_FreeBytes(void) +{ + /* return the bytes free in the circular buffer */ + return ((APP_Rx_ptr_out - APP_Rx_ptr_in) + (-((int)(APP_Rx_ptr_out <= APP_Rx_ptr_in)) & APP_RX_DATA_SIZE)) - 1; +} + /** * @brief VCP_DataTx - * CDC received data to be send over USB IN endpoint are managed in - * this function. + * CDC data to be sent to the Host (app) over USB * @param Buf: Buffer of data to be sent * @param Len: Number of data to be sent (in bytes) * @retval Result of the operation: USBD_OK if all operations are OK else VCP_FAIL @@ -185,6 +188,8 @@ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) for (uint32_t i = 0; i < Len; i++) { APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; + + while (!CDC_Send_FreeBytes()); } return USBD_OK; @@ -205,16 +210,16 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) recvBuf[count] = APP_Tx_Buffer[APP_Tx_ptr_out]; APP_Tx_ptr_out = (APP_Tx_ptr_out + 1) % APP_TX_DATA_SIZE; count++; - receiveLength--; } - - if (!receiveLength) { - receiveLength = APP_Tx_ptr_out != APP_Tx_ptr_in; - } - return count; } +uint32_t CDC_Receive_BytesAvailable(void) +{ + /* return the bytes available in the receive circular buffer */ + return APP_Tx_ptr_out > APP_Tx_ptr_in ? APP_TX_DATA_SIZE - APP_Tx_ptr_out + APP_Tx_ptr_in : APP_Tx_ptr_in - APP_Tx_ptr_out; +} + /** * @brief VCP_DataRx * Data received over USB OUT endpoint are sent over CDC interface @@ -232,9 +237,12 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) */ static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) { + if (CDC_Receive_BytesAvailable() + Len > APP_TX_DATA_SIZE) { + return USBD_FAIL; + } + __disable_irq(); - receiveLength += Len; for (uint32_t i = 0; i < Len; i++) { APP_Tx_Buffer[APP_Tx_ptr_in] = Buf[i]; APP_Tx_ptr_in = (APP_Tx_ptr_in + 1) % APP_TX_DATA_SIZE; @@ -242,9 +250,6 @@ static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) __enable_irq(); - if(receiveLength > APP_TX_DATA_SIZE) - return USBD_FAIL; - return USBD_OK; } diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index daad8ad0f9..1168faf5fd 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -37,14 +37,15 @@ __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength); // HJI +uint32_t CDC_Send_FreeBytes(void); uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len); // HJI +uint32_t CDC_Receive_BytesAvailable(void); + uint8_t usbIsConfigured(void); // HJI uint8_t usbIsConnected(void); // HJI uint32_t CDC_BaudRate(void); /* External variables --------------------------------------------------------*/ - -extern __IO uint32_t receiveLength; // HJI extern __IO uint32_t bDeviceState; /* USB device status */ typedef enum _DEVICE_STATE { diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index 4a2b0dd1f7..0d080e8c84 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -177,33 +177,38 @@ uint32_t millis(void) { uint32_t micros(void) { return 0; } -uint8_t serialRxBytesWaiting(serialPort_t *instance) { +uint32_t serialRxBytesWaiting(serialPort_t *instance) +{ UNUSED(instance); return 0; } -uint8_t serialTxBytesFree(serialPort_t *instance) { +uint32_t serialTxBytesFree(serialPort_t *instance) +{ UNUSED(instance); return 0; } -uint8_t serialRead(serialPort_t *instance) { +uint8_t serialRead(serialPort_t *instance) +{ UNUSED(instance); return 0; } -void serialWrite(serialPort_t *instance, uint8_t ch) { +void serialWrite(serialPort_t *instance, uint8_t ch) +{ UNUSED(instance); UNUSED(ch); } -void serialSetMode(serialPort_t *instance, portMode_t mode) { +void serialSetMode(serialPort_t *instance, portMode_t mode) +{ UNUSED(instance); UNUSED(mode); } - -serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options) { +serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options) +{ UNUSED(identifier); UNUSED(functionMask); UNUSED(baudRate); @@ -214,30 +219,36 @@ serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFuncti return NULL; } -void closeSerialPort(serialPort_t *serialPort) { +void closeSerialPort(serialPort_t *serialPort) +{ UNUSED(serialPort); } -serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) { +serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) +{ UNUSED(function); return NULL; } -bool sensors(uint32_t mask) { +bool sensors(uint32_t mask) +{ UNUSED(mask); return false; } -bool telemetryDetermineEnabledState(portSharing_e) { +bool telemetryDetermineEnabledState(portSharing_e) +{ return true; } -portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) { +portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) +{ return PORTSHARING_NOT_SHARED; } -batteryState_e getBatteryState(void) { +batteryState_e getBatteryState(void) +{ return BATTERY_OK; } From 196baa4e8715de8edd0ef0a8f0bd8c4d34ebf828 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 13 Aug 2016 11:53:12 +1000 Subject: [PATCH 317/456] Some minor updates to the SPI MPU6500 driver to disable i2c interface --- src/main/drivers/accgyro_mpu6500.c | 22 ++++++---------------- src/main/drivers/accgyro_mpu6500.h | 4 ++++ src/main/drivers/accgyro_spi_mpu6000.c | 4 ++-- src/main/drivers/accgyro_spi_mpu6500.c | 24 ++++++++++++++++++++++-- src/main/drivers/accgyro_spi_mpu6500.h | 3 +++ src/main/target/BLUEJAYF4/target.h | 1 - 6 files changed, 37 insertions(+), 21 deletions(-) diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index 9a762dc7c4..b2316cb25c 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -73,19 +73,6 @@ void mpu6500GyroInit(uint8_t lpf) { mpuIntExtiInit(); -#ifdef NAZE - // FIXME target specific code in driver code. - - gpio_config_t gpio; - // MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices - if (hse_value == 12000000) { - gpio.pin = Pin_13; - gpio.speed = Speed_2MHz; - gpio.mode = Mode_IN_FLOATING; - gpioInit(GPIOC, &gpio); - } -#endif - mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); delay(100); mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07); @@ -105,11 +92,14 @@ void mpu6500GyroInit(uint8_t lpf) // Data ready interrupt configuration #ifdef USE_MPU9250_MAG - mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN + mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN #else - mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN + mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR #endif + delay(15); + #ifdef USE_MPU_DATA_READY_SIGNAL - mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable + mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable #endif + delay(15); } diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h index ecdd100bc0..b145c8077d 100644 --- a/src/main/drivers/accgyro_mpu6500.h +++ b/src/main/drivers/accgyro_mpu6500.h @@ -20,6 +20,10 @@ #define ICM20608G_WHO_AM_I_CONST (0xAF) #define MPU6500_BIT_RESET (0x80) +#define MPU6500_BIT_INT_ANYRD_2CLEAR 1 << 4 +#define MPU6500_BIT_BYPASS_EN 1 << 0 +#define MPU6500_BIT_I2C_IF_DIS 1 << 4 +#define MPU6500_BIT_RAW_RDY_EN (0x01) #pragma once diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index dc21901e7a..6522a282f3 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -205,8 +205,8 @@ bool mpu6000SpiDetect(void) return false; } -static void mpu6000AccAndGyroInit(void) { - +static void mpu6000AccAndGyroInit(void) +{ if (mpuSpi6000InitDone) { return; } diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index afc69fbf16..4bf1c6f678 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -91,13 +91,33 @@ bool mpu6500SpiDetect(void) return false; } +void mpu6500SpiAccInit(acc_t *acc) +{ + mpu6500AccInit(acc); +} + +void mpu6500SpiGyroInit(uint8_t lpf) +{ + spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW); + delayMicroseconds(1); + + mpu6500GyroInit(lpf); + + // Disable Primary I2C Interface + mpu6500WriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); + delay(100); + + spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_ULTRAFAST); + delayMicroseconds(1); +} + bool mpu6500SpiAccDetect(acc_t *acc) { if (mpuDetectionResult.sensor != MPU_65xx_SPI) { return false; } - acc->init = mpu6500AccInit; + acc->init = mpu6500SpiAccInit; acc->read = mpuAccRead; return true; @@ -109,7 +129,7 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro) return false; } - gyro->init = mpu6500GyroInit; + gyro->init = mpu6500SpiGyroInit; gyro->read = mpuGyroRead; gyro->intStatus = checkMPUDataReady; diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h index a8d5cf514a..5c687c6acc 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.h +++ b/src/main/drivers/accgyro_spi_mpu6500.h @@ -19,6 +19,9 @@ bool mpu6500SpiDetect(void); +void mpu6500SpiAccInit(acc_t *acc); +void mpu6500SpiGyroInit(uint8_t lpf); + bool mpu6500SpiAccDetect(acc_t *acc); bool mpu6500SpiGyroDetect(gyro_t *gyro); diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 76a494cd53..b9d24c95d1 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -43,7 +43,6 @@ // MPU6500 interrupt //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW //#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define MPU_INT_EXTI PC5 #define MPU6500_CS_PIN PC4 From 024208b7b585bcb7c8cf062b7d409ca6832aeeb6 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 13 Aug 2016 13:38:21 +1000 Subject: [PATCH 318/456] PPM fix for Revo & BlueJayF4 --- src/main/drivers/pwm_rx.c | 1 - src/main/target/BLUEJAYF4/target.c | 14 ++++++------- src/main/target/REVO/target.c | 32 +++++++++++++++--------------- 3 files changed, 23 insertions(+), 24 deletions(-) diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index cb40782c9a..36a101d2cc 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -180,7 +180,6 @@ static void ppmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t ca if (capture == PPM_TIMER_PERIOD - 1) { ppmDev.overflowed = true; } - } static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture) diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index 83e0700cd5..72a5b5ba7b 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -67,12 +67,12 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // PPM IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S1_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S2_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S3_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S4_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S6_OUT + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S1_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S2_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S3_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S4_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S6_OUT }; diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index a253a0b31f..756fa1dda6 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -23,7 +23,7 @@ #include "drivers/timer.h" const uint16_t multiPPM[] = { - PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), @@ -34,7 +34,7 @@ const uint16_t multiPPM[] = { PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; @@ -55,7 +55,7 @@ const uint16_t multiPWM[] = { }; const uint16_t airPPM[] = { - PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), PWM9 | (MAP_TO_SERVO_OUTPUT << 8), @@ -66,7 +66,7 @@ const uint16_t airPPM[] = { PWM3 | (MAP_TO_SERVO_OUTPUT << 8), PWM4 | (MAP_TO_SERVO_OUTPUT << 8), PWM5 | (MAP_TO_SERVO_OUTPUT << 8), - PWM1 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), 0xFFFF }; @@ -87,16 +87,16 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5 }, // S6_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S6_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT }; From ee42efc246a8e437f00ea4e18095a22ba1f0eede Mon Sep 17 00:00:00 2001 From: nathan Date: Fri, 12 Aug 2016 21:04:22 -0700 Subject: [PATCH 319/456] update makefile verbose option per https://github.com/betaflight/betaflight/pull/976 and @AndersHoglund --- Makefile | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/Makefile b/Makefile index 1a81c6cbbe..676b3d73d9 100644 --- a/Makefile +++ b/Makefile @@ -37,7 +37,9 @@ SERIAL_DEVICE ?= $(firstword $(wildcard /dev/ttyUSB*) no-port-found) # Flash size (KB). Some low-end chips actually have more flash than advertised, use this to override. FLASH_SIZE ?= -# Decide on a verbosity level based on the V= parameter +## Set verbosity level based on the V= parameter +## V=0 Low +## v=1 High export AT := @ ifndef V @@ -684,9 +686,9 @@ all: $(VALID_TARGETS) $(VALID_TARGETS): $(V0) echo "" && \ - $(V0) echo "Building $@" && \ - $(V0) $(MAKE) binary hex TARGET=$@ && \ - $(V0) echo "Building $@ succeeded." + echo "Building $@" && \ + $(MAKE) binary hex TARGET=$@ && \ + echo "Building $@ succeeded." @@ -759,9 +761,9 @@ help: Makefile $(V0) @echo "Makefile for the $(FORKNAME) firmware" $(V0) @echo "" $(V0) @echo "Usage:" - $(V0) @echo " make [TARGET=] [OPTIONS=\"\"]" + $(V0) @echo " make [V=] [TARGET=] [OPTIONS=\"\"]" $(V0) @echo "Or:" - $(V0) @echo " make [OPTIONS=\"\"]" + $(V0) @echo " make [V=] [OPTIONS=\"\"]" $(V0) @echo "" $(V0) @echo "Valid TARGET values are: $(VALID_TARGETS)" $(V0) @echo "" From 59e487f2e9c06e62bdfd8b46c716a6b19fc1d064 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 13 Aug 2016 16:48:07 +1000 Subject: [PATCH 320/456] Brackets safety for some BIT defines --- src/main/drivers/accgyro_mpu6500.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h index b145c8077d..32d0cefcc5 100644 --- a/src/main/drivers/accgyro_mpu6500.h +++ b/src/main/drivers/accgyro_mpu6500.h @@ -20,9 +20,9 @@ #define ICM20608G_WHO_AM_I_CONST (0xAF) #define MPU6500_BIT_RESET (0x80) -#define MPU6500_BIT_INT_ANYRD_2CLEAR 1 << 4 -#define MPU6500_BIT_BYPASS_EN 1 << 0 -#define MPU6500_BIT_I2C_IF_DIS 1 << 4 +#define MPU6500_BIT_INT_ANYRD_2CLEAR (1 << 4) +#define MPU6500_BIT_BYPASS_EN (1 << 0) +#define MPU6500_BIT_I2C_IF_DIS (1 << 4) #define MPU6500_BIT_RAW_RDY_EN (0x01) #pragma once From 414e02d9fe951d4003ef8c33f0a3bec26b9dabe9 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 13 Aug 2016 17:03:58 +1000 Subject: [PATCH 321/456] Some more comments and a little formatting clean up --- src/main/drivers/serial_usb_vcp.c | 2 +- src/main/io/serial_cli.c | 99 +++++++++++++++++-------------- src/main/target/common.h | 14 ++--- src/main/vcpf4/usbd_cdc_vcp.c | 12 +++- 4 files changed, 70 insertions(+), 57 deletions(-) diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 697f7bca37..45e5ad5427 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -69,7 +69,7 @@ static uint32_t usbVcpAvailable(serialPort_t *instance) { UNUSED(instance); - return CDC_Receive_BytesAvailable(); + return CDC_Receive_BytesAvailable(); } static uint8_t usbVcpRead(serialPort_t *instance) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 73f6a936bd..8df34d7013 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1000,8 +1000,8 @@ static void printRxFail(uint8_t dumpMask, master_t *defaultConfig) for (uint8_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[channel]; channelFailsafeConfigurationDefault = &defaultConfig->rxConfig.failsafe_channel_configurations[channel]; - equalsDefault = channelFailsafeConfiguration->mode == channelFailsafeConfigurationDefault->mode - && channelFailsafeConfiguration->step == channelFailsafeConfigurationDefault->step; + equalsDefault = channelFailsafeConfiguration->mode == channelFailsafeConfigurationDefault->mode + && channelFailsafeConfiguration->step == channelFailsafeConfigurationDefault->step; requireValue = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET; modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfiguration->mode]; if (requireValue) { @@ -1113,9 +1113,9 @@ static void printAux(uint8_t dumpMask, master_t *defaultConfig) mac = &masterConfig.modeActivationConditions[i]; macDefault = &defaultConfig->modeActivationConditions[i]; equalsDefault = mac->modeId == macDefault->modeId - && mac->auxChannelIndex == macDefault->auxChannelIndex - && mac->range.startStep == macDefault->range.startStep - && mac->range.endStep == macDefault->range.endStep; + && mac->auxChannelIndex == macDefault->auxChannelIndex + && mac->range.startStep == macDefault->range.startStep + && mac->range.endStep == macDefault->range.endStep; cliDumpPrintf(dumpMask, equalsDefault, "aux %u %u %u %u %u\r\n", i, mac->modeId, @@ -1172,17 +1172,17 @@ static void printSerial(uint8_t dumpMask, master_t *defaultConfig) serialConfig_t *serialConfigDefault; bool equalsDefault; for (int i = 0; i < SERIAL_PORT_COUNT; i++) { - serialConfig = &masterConfig.serialConfig; + serialConfig = &masterConfig.serialConfig; if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) { continue; }; - serialConfigDefault = &defaultConfig->serialConfig; - equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier - && serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask - && serialConfig->portConfigs[i].msp_baudrateIndex == serialConfigDefault->portConfigs[i].msp_baudrateIndex - && serialConfig->portConfigs[i].gps_baudrateIndex == serialConfigDefault->portConfigs[i].gps_baudrateIndex - && serialConfig->portConfigs[i].telemetry_baudrateIndex == serialConfigDefault->portConfigs[i].telemetry_baudrateIndex - && serialConfig->portConfigs[i].blackbox_baudrateIndex == serialConfigDefault->portConfigs[i].blackbox_baudrateIndex; + serialConfigDefault = &defaultConfig->serialConfig; + equalsDefault = serialConfig->portConfigs[i].identifier == serialConfigDefault->portConfigs[i].identifier + && serialConfig->portConfigs[i].functionMask == serialConfigDefault->portConfigs[i].functionMask + && serialConfig->portConfigs[i].msp_baudrateIndex == serialConfigDefault->portConfigs[i].msp_baudrateIndex + && serialConfig->portConfigs[i].gps_baudrateIndex == serialConfigDefault->portConfigs[i].gps_baudrateIndex + && serialConfig->portConfigs[i].telemetry_baudrateIndex == serialConfigDefault->portConfigs[i].telemetry_baudrateIndex + && serialConfig->portConfigs[i].blackbox_baudrateIndex == serialConfigDefault->portConfigs[i].blackbox_baudrateIndex; cliDumpPrintf(dumpMask, equalsDefault, "serial %d %d %ld %ld %ld %ld\r\n" , serialConfig->portConfigs[i].identifier, serialConfig->portConfigs[i].functionMask, @@ -1190,7 +1190,7 @@ static void printSerial(uint8_t dumpMask, master_t *defaultConfig) baudRates[serialConfig->portConfigs[i].gps_baudrateIndex], baudRates[serialConfig->portConfigs[i].telemetry_baudrateIndex], baudRates[serialConfig->portConfigs[i].blackbox_baudrateIndex] - ); + ); } } @@ -1202,7 +1202,7 @@ static void cliSerial(char *cmdline) if (isEmpty(cmdline)) { printSerial(DUMP_MASTER, &masterConfig); - return; + return; } serialPortConfig_t portConfig; memset(&portConfig, 0 , sizeof(portConfig)); @@ -1358,11 +1358,11 @@ static void printAdjustmentRange(uint8_t dumpMask, master_t *defaultConfig) for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { ar = &masterConfig.adjustmentRanges[i]; arDefault = &defaultConfig->adjustmentRanges[i]; - equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex - && ar->range.startStep == arDefault->range.startStep - && ar->range.endStep == arDefault->range.endStep - && ar->adjustmentFunction == arDefault->adjustmentFunction - && ar->auxSwitchChannelIndex == arDefault->auxSwitchChannelIndex + equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex + && ar->range.startStep == arDefault->range.startStep + && ar->range.endStep == arDefault->range.endStep + && ar->adjustmentFunction == arDefault->adjustmentFunction + && ar->auxSwitchChannelIndex == arDefault->auxSwitchChannelIndex && ar->adjustmentIndex == arDefault->adjustmentIndex; cliDumpPrintf(dumpMask, equalsDefault, "adjrange %u %u %u %u %u %u %u\r\n", i, @@ -1525,8 +1525,8 @@ static void printRxRange(uint8_t dumpMask, master_t *defaultConfig) for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { channelRangeConfiguration = &masterConfig.rxConfig.channelRanges[i]; channelRangeConfigurationDefault = &defaultConfig->rxConfig.channelRanges[i]; - equalsDefault = channelRangeConfiguration->min == channelRangeConfigurationDefault->min - && channelRangeConfiguration->max == channelRangeConfigurationDefault->max; + equalsDefault = channelRangeConfiguration->min == channelRangeConfigurationDefault->min + && channelRangeConfiguration->max == channelRangeConfigurationDefault->max; cliDumpPrintf(dumpMask, equalsDefault, "rxrange %u %u %u\r\n", i, channelRangeConfiguration->min, channelRangeConfiguration->max); } } @@ -1619,8 +1619,8 @@ static void printColor(uint8_t dumpMask, master_t *defaultConfig) color = &masterConfig.colors[i]; colorDefault = &defaultConfig->colors[i]; equalsDefault = color->h == colorDefault->h - && color->s == colorDefault->s - && color->v == colorDefault->v; + && color->s == colorDefault->s + && color->v == colorDefault->v; cliDumpPrintf(dumpMask, equalsDefault, "color %u %d,%u,%u\r\n", i, color->h, @@ -2121,11 +2121,11 @@ static void printVtx(uint8_t dumpMask, master_t *defaultConfig) for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { cac = &masterConfig.vtxChannelActivationConditions[i]; cacDefault = &defaultConfig->vtxChannelActivationConditions[i]; - equalsDefault = cac->auxChannelIndex == cacDefault->auxChannelIndex + equalsDefault = cac->auxChannelIndex == cacDefault->auxChannelIndex && cac->band == cacDefault->band - && cac->channel == cacDefault->channel - && cac->range.startStep == cacDefault->range.startStep - && cac->range.endStep == cacDefault->range.endStep; + && cac->channel == cacDefault->channel + && cac->range.startStep == cacDefault->range.startStep + && cac->range.endStep == cacDefault->range.endStep; cliDumpPrintf(dumpMask, equalsDefault, "vtx %u %u %u %u %u %u\r\n", i, cac->auxChannelIndex, @@ -2246,7 +2246,7 @@ static bool valueEqualsDefault(const clivalue_t *value, master_t *defaultConfig) case VAR_FLOAT: result = *(float *)ptr == *(float *)ptrDefault; - break; + break; } return result; } @@ -2378,11 +2378,11 @@ static void printConfig(char *cmdline, bool doDiff) equalsDefault = masterConfig.customServoMixer[i].targetChannel == defaultConfig.customServoMixer[i].targetChannel && masterConfig.customServoMixer[i].inputSource == defaultConfig.customServoMixer[i].inputSource - && masterConfig.customServoMixer[i].rate == defaultConfig.customServoMixer[i].rate - && masterConfig.customServoMixer[i].speed == defaultConfig.customServoMixer[i].speed - && masterConfig.customServoMixer[i].min == defaultConfig.customServoMixer[i].min - && masterConfig.customServoMixer[i].max == defaultConfig.customServoMixer[i].max - && masterConfig.customServoMixer[i].box == masterConfig.customServoMixer[i].box; + && masterConfig.customServoMixer[i].rate == defaultConfig.customServoMixer[i].rate + && masterConfig.customServoMixer[i].speed == defaultConfig.customServoMixer[i].speed + && masterConfig.customServoMixer[i].min == defaultConfig.customServoMixer[i].min + && masterConfig.customServoMixer[i].max == defaultConfig.customServoMixer[i].max + && masterConfig.customServoMixer[i].box == masterConfig.customServoMixer[i].box; cliDumpPrintf(dumpMask, equalsDefault,"smix %d %d %d %d %d %d %d %d\r\n", i, @@ -2431,7 +2431,7 @@ static void printConfig(char *cmdline, bool doDiff) equalsDefault = true; for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; - equalsDefault = equalsDefault && (masterConfig.rxConfig.rcmap[i] == defaultConfig.rxConfig.rcmap[i]); + equalsDefault = equalsDefault && (masterConfig.rxConfig.rcmap[i] == defaultConfig.rxConfig.rcmap[i]); } buf[i] = '\0'; cliDumpPrintf(dumpMask, equalsDefault, "map %s\r\n", buf); @@ -2483,8 +2483,9 @@ static void printConfig(char *cmdline, bool doDiff) uint8_t currentRateIndex = currentProfile->activeRateProfile; uint8_t rateCount; - for (rateCount=0; rateCount= MAX_PROFILE_COUNT) // Faulty values + if (profileIndex >= MAX_PROFILE_COUNT) { + // Faulty values return; + } changeProfile(profileIndex); cliPrint("\r\n# profile\r\n"); cliProfile(""); @@ -2523,8 +2526,10 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, master_t *def static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, master_t *defaultConfig) { - if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values + if (rateProfileIndex >= MAX_RATEPROFILES) { + // Faulty values return; + } changeControlRateProfile(rateProfileIndex); cliPrint("\r\n# rateprofile\r\n"); cliRateProfile(""); @@ -2744,8 +2749,9 @@ static void cliMap(char *cmdline) if (len == 8) { // uppercase it - for (i = 0; i < 8; i++) + for (i = 0; i < 8; i++) { cmdline[i] = toupper((unsigned char)cmdline[i]); + } for (i = 0; i < 8; i++) { if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i])) continue; @@ -2755,8 +2761,9 @@ static void cliMap(char *cmdline) parseRcChannels(cmdline, &masterConfig.rxConfig); } cliPrint("Map: "); - for (i = 0; i < 8; i++) + for (i = 0; i < 8; i++) { out[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; + } out[i] = '\0'; cliPrintf("%s\r\n", out); } @@ -2952,9 +2959,9 @@ static void cliDefaults(char *cmdline) static void cliPrint(const char *str) { - while (*str) { - bufWriterAppend(cliWriter, *str++); - } + while (*str) { + bufWriterAppend(cliWriter, *str++); + } } static void cliPutp(void *p, char ch) @@ -2976,7 +2983,7 @@ static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *form tfp_format(cliWriter, cliPutp, format, va); va_end(va); - return true; + return true; } return false; @@ -3404,11 +3411,11 @@ void cliProcess(void) cliBuffer[bufferIndex] = 0; // null terminate const clicmd_t *cmd; - char *options; + char *options; for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) { if ((options = checkCommand(cliBuffer, cmd->name))) { break; - } + } } if(cmd < cmdTable + CMD_COUNT) cmd->func(options); diff --git a/src/main/target/common.h b/src/main/target/common.h index 4e3b557191..bb7cd7f045 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -24,16 +24,16 @@ /* STM32F4 specific settings that apply to all F4 targets */ #ifdef STM32F4 -#define MAX_AUX_CHANNELS 99 -#define TASK_GYROPID_DESIRED_PERIOD 125 -#define SCHEDULER_DELAY_LIMIT 10 -#define I2C3_OVERCLOCK true +#define MAX_AUX_CHANNELS 99 +#define TASK_GYROPID_DESIRED_PERIOD 125 +#define SCHEDULER_DELAY_LIMIT 10 +#define I2C3_OVERCLOCK true #else /* when not an F4 */ -#define MAX_AUX_CHANNELS 6 -#define TASK_GYROPID_DESIRED_PERIOD 1000 -#define SCHEDULER_DELAY_LIMIT 100 +#define MAX_AUX_CHANNELS 6 +#define TASK_GYROPID_DESIRED_PERIOD 1000 +#define SCHEDULER_DELAY_LIMIT 100 #endif diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index e27fd3657c..040d5729df 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -165,7 +165,13 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) uint32_t CDC_Send_FreeBytes(void) { - /* return the bytes free in the circular buffer */ + /* + return the bytes free in the circular buffer + + functionally equivalent to: + (APP_Rx_ptr_out > APP_Rx_ptr_in ? APP_Rx_ptr_out - APP_Rx_ptr_in : APP_RX_DATA_SIZE - APP_Rx_ptr_in + APP_Rx_ptr_in) + but without the impact of the condition check. + */ return ((APP_Rx_ptr_out - APP_Rx_ptr_in) + (-((int)(APP_Rx_ptr_out <= APP_Rx_ptr_in)) & APP_RX_DATA_SIZE)) - 1; } @@ -183,13 +189,13 @@ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) could just check for: USB_CDC_ZLP, but better to be safe and wait for any existing transmission to complete. */ - while (USB_Tx_State); + while (USB_Tx_State != 0); for (uint32_t i = 0; i < Len; i++) { APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; - while (!CDC_Send_FreeBytes()); + while (CDC_Send_FreeBytes() <= 0); } return USBD_OK; From 17f1429465cd4b3db1086af6ceb36ca15265aefc Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 13 Aug 2016 17:09:54 +1000 Subject: [PATCH 322/456] Additional formatting corrections --- src/main/io/serial_cli.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 8df34d7013..f61ff89979 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1358,11 +1358,11 @@ static void printAdjustmentRange(uint8_t dumpMask, master_t *defaultConfig) for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { ar = &masterConfig.adjustmentRanges[i]; arDefault = &defaultConfig->adjustmentRanges[i]; - equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex - && ar->range.startStep == arDefault->range.startStep - && ar->range.endStep == arDefault->range.endStep - && ar->adjustmentFunction == arDefault->adjustmentFunction - && ar->auxSwitchChannelIndex == arDefault->auxSwitchChannelIndex + equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex + && ar->range.startStep == arDefault->range.startStep + && ar->range.endStep == arDefault->range.endStep + && ar->adjustmentFunction == arDefault->adjustmentFunction + && ar->auxSwitchChannelIndex == arDefault->auxSwitchChannelIndex && ar->adjustmentIndex == arDefault->adjustmentIndex; cliDumpPrintf(dumpMask, equalsDefault, "adjrange %u %u %u %u %u %u %u\r\n", i, @@ -2246,7 +2246,7 @@ static bool valueEqualsDefault(const clivalue_t *value, master_t *defaultConfig) case VAR_FLOAT: result = *(float *)ptr == *(float *)ptrDefault; - break; + break; } return result; } From e7c87c0f0f915bd89d9ef23ad4539a26271b6d9a Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 13 Aug 2016 17:18:41 +1000 Subject: [PATCH 323/456] SPI clock speed to fast (not ultrafast) --- src/main/drivers/accgyro_spi_mpu6500.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 4bf1c6f678..9c2b7a45df 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -107,7 +107,7 @@ void mpu6500SpiGyroInit(uint8_t lpf) mpu6500WriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); delay(100); - spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_ULTRAFAST); + spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); delayMicroseconds(1); } From a05f030c94230d51b24f86160adb2be9eb09144c Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 13 Aug 2016 18:07:50 +1000 Subject: [PATCH 324/456] PPM fix for Revo --- src/main/drivers/pwm_rx.c | 1 - src/main/target/BLUEJAYF4/target.c | 14 ++++++------- src/main/target/REVO/target.c | 32 +++++++++++++++--------------- 3 files changed, 23 insertions(+), 24 deletions(-) diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 3c72d104dc..24fe3e59aa 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -180,7 +180,6 @@ static void ppmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t ca if (capture == PPM_TIMER_PERIOD - 1) { ppmDev.overflowed = true; } - } static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture) diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index b85e50f539..553ebead4a 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -66,12 +66,12 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // PPM IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S1_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S2_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S3_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S4_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S6_OUT + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S1_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S2_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S3_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S4_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S6_OUT }; diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index 67f7327285..e33f1a58c8 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -22,7 +22,7 @@ #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { - PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), @@ -33,7 +33,7 @@ const uint16_t multiPPM[] = { PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; @@ -54,7 +54,7 @@ const uint16_t multiPWM[] = { }; const uint16_t airPPM[] = { - PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), PWM9 | (MAP_TO_SERVO_OUTPUT << 8), @@ -65,7 +65,7 @@ const uint16_t airPPM[] = { PWM3 | (MAP_TO_SERVO_OUTPUT << 8), PWM4 | (MAP_TO_SERVO_OUTPUT << 8), PWM5 | (MAP_TO_SERVO_OUTPUT << 8), - PWM1 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), 0xFFFF }; @@ -86,16 +86,16 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5 }, // S6_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S6_IN + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM9 }, // S3_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S6_OUT }; From 2d1a1865ded958b626c8157488b60595958a32d3 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 13 Aug 2016 20:19:18 +1200 Subject: [PATCH 325/456] Readded rateprofile setting to profile dump, as it's part of the profile. --- src/main/io/serial_cli.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 4164f4e14a..6341954374 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2516,7 +2516,9 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, master_t *def changeProfile(profileIndex); cliPrint("\r\n# profile\r\n"); cliProfile(""); + cliPrint("\r\n"); dumpValues(PROFILE_VALUE, dumpMask, defaultConfig); + cliRateProfile(""); } static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, master_t *defaultConfig) @@ -2526,6 +2528,7 @@ static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, maste changeControlRateProfile(rateProfileIndex); cliPrint("\r\n# rateprofile\r\n"); cliRateProfile(""); + cliPrint("\r\n"); dumpValues(PROFILE_RATE_VALUE, dumpMask, defaultConfig); } From 5d55a3ac13f47e1be3c3e9f96c99da57c33f09d0 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Sat, 13 Aug 2016 23:03:06 +0200 Subject: [PATCH 326/456] Target custom config update "diff" and "diff all" working now also with custom configuration --- src/main/config/config.c | 9 ++--- src/main/target/ALIENFLIGHTF1/config.c | 40 +++++++++++----------- src/main/target/ALIENFLIGHTF3/config.c | 46 +++++++++++++------------- src/main/target/ALIENFLIGHTF4/config.c | 46 +++++++++++++------------- src/main/target/BLUEJAYF4/config.c | 6 ++-- src/main/target/COLIBRI/config.c | 25 +++++++------- src/main/target/COLIBRI/target.h | 2 +- src/main/target/COLIBRI_RACE/config.c | 10 +++--- src/main/target/FURYF4/config.c | 14 ++++---- src/main/target/MOTOLAB/config.c | 6 ++-- 10 files changed, 99 insertions(+), 105 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 07605b3f37..361173a7b7 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -90,7 +90,7 @@ #endif void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); -void targetConfiguration(void); +void targetConfiguration(master_t *config); #if !defined(FLASH_SIZE) #error "Flash size not defined for target. (specify in KB)" @@ -689,12 +689,7 @@ void createDefaultConfig(master_t *config) #endif #if defined(TARGET_CONFIG) - // Don't look at target specific config settings when getting default - // values for a CLI diff. This is suboptimal, but it doesn't break the - // public API - if (config == &masterConfig) { - targetConfiguration(); - } + targetConfiguration(config); #endif diff --git a/src/main/target/ALIENFLIGHTF1/config.c b/src/main/target/ALIENFLIGHTF1/config.c index b34f9d5a37..8b74d022ef 100644 --- a/src/main/target/ALIENFLIGHTF1/config.c +++ b/src/main/target/ALIENFLIGHTF1/config.c @@ -70,26 +70,26 @@ #include "config/config_master.h" // alternative defaults settings for AlienFlight targets -void targetConfiguration(void) +void targetConfiguration(master_t *config) { - masterConfig.rxConfig.spektrum_sat_bind = 5; - masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; - masterConfig.motor_pwm_rate = 32000; - masterConfig.failsafeConfig.failsafe_delay = 2; - masterConfig.failsafeConfig.failsafe_off_delay = 0; - masterConfig.profile[0].pidProfile.P8[ROLL] = 90; - masterConfig.profile[0].pidProfile.I8[ROLL] = 44; - masterConfig.profile[0].pidProfile.D8[ROLL] = 60; - masterConfig.profile[0].pidProfile.P8[PITCH] = 90; - masterConfig.profile[0].pidProfile.I8[PITCH] = 44; - masterConfig.profile[0].pidProfile.D8[PITCH] = 60; + config->rxConfig.spektrum_sat_bind = 5; + config->rxConfig.spektrum_sat_bind_autoreset = 1; + config->motor_pwm_rate = 32000; + config->failsafeConfig.failsafe_delay = 2; + config->failsafeConfig.failsafe_off_delay = 0; + config->profile[0].pidProfile.P8[ROLL] = 90; + config->profile[0].pidProfile.I8[ROLL] = 44; + config->profile[0].pidProfile.D8[ROLL] = 60; + config->profile[0].pidProfile.P8[PITCH] = 90; + config->profile[0].pidProfile.I8[PITCH] = 44; + config->profile[0].pidProfile.D8[PITCH] = 60; - masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R - masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R - masterConfig.customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L - masterConfig.customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L - masterConfig.customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R - masterConfig.customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L - masterConfig.customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - masterConfig.customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif + config->customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + config->customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + config->customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + config->customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + config->customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif } diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 650da37c11..870518be9d 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -76,28 +76,28 @@ #include "config/config_master.h" // alternative defaults settings for AlienFlight targets -void targetConfiguration(void) { - masterConfig.mag_hardware = MAG_NONE; // disabled by default - masterConfig.rxConfig.spektrum_sat_bind = 5; - masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; - masterConfig.motor_pwm_rate = 32000; - masterConfig.failsafeConfig.failsafe_delay = 2; - masterConfig.failsafeConfig.failsafe_off_delay = 0; - masterConfig.gyro_sync_denom = 2; - masterConfig.pid_process_denom = 1; - masterConfig.profile[0].pidProfile.P8[ROLL] = 90; - masterConfig.profile[0].pidProfile.I8[ROLL] = 44; - masterConfig.profile[0].pidProfile.D8[ROLL] = 60; - masterConfig.profile[0].pidProfile.P8[PITCH] = 90; - masterConfig.profile[0].pidProfile.I8[PITCH] = 44; - masterConfig.profile[0].pidProfile.D8[PITCH] = 60; +void targetConfiguration(master_t *config) { + config->mag_hardware = MAG_NONE; // disabled by default + config->rxConfig.spektrum_sat_bind = 5; + config->rxConfig.spektrum_sat_bind_autoreset = 1; + config->motor_pwm_rate = 32000; + config->failsafeConfig.failsafe_delay = 2; + config->failsafeConfig.failsafe_off_delay = 0; + config->gyro_sync_denom = 2; + config->pid_process_denom = 1; + config->profile[0].pidProfile.P8[ROLL] = 90; + config->profile[0].pidProfile.I8[ROLL] = 44; + config->profile[0].pidProfile.D8[ROLL] = 60; + config->profile[0].pidProfile.P8[PITCH] = 90; + config->profile[0].pidProfile.I8[PITCH] = 44; + config->profile[0].pidProfile.D8[PITCH] = 60; - masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R - masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R - masterConfig.customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L - masterConfig.customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L - masterConfig.customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R - masterConfig.customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L - masterConfig.customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - masterConfig.customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif + config->customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + config->customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + config->customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + config->customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + config->customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif } diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index b156230393..d3cde21842 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -76,28 +76,28 @@ #include "config/config_master.h" // alternative defaults settings for AlienFlight targets -void targetConfiguration(void) { - masterConfig.mag_hardware = MAG_NONE; // disabled by default - masterConfig.rxConfig.spektrum_sat_bind = 5; - masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; - masterConfig.motor_pwm_rate = 32000; - masterConfig.failsafeConfig.failsafe_delay = 2; - masterConfig.failsafeConfig.failsafe_off_delay = 0; - masterConfig.gyro_sync_denom = 1; - masterConfig.pid_process_denom = 1; - masterConfig.profile[0].pidProfile.P8[ROLL] = 90; - masterConfig.profile[0].pidProfile.I8[ROLL] = 44; - masterConfig.profile[0].pidProfile.D8[ROLL] = 60; - masterConfig.profile[0].pidProfile.P8[PITCH] = 90; - masterConfig.profile[0].pidProfile.I8[PITCH] = 44; - masterConfig.profile[0].pidProfile.D8[PITCH] = 60; +void targetConfiguration(master_t *config) { + config->mag_hardware = MAG_NONE; // disabled by default + config->rxConfig.spektrum_sat_bind = 5; + config->rxConfig.spektrum_sat_bind_autoreset = 1; + config->motor_pwm_rate = 32000; + config->failsafeConfig.failsafe_delay = 2; + config->failsafeConfig.failsafe_off_delay = 0; + config->gyro_sync_denom = 1; + config->pid_process_denom = 1; + config->profile[0].pidProfile.P8[ROLL] = 90; + config->profile[0].pidProfile.I8[ROLL] = 44; + config->profile[0].pidProfile.D8[ROLL] = 60; + config->profile[0].pidProfile.P8[PITCH] = 90; + config->profile[0].pidProfile.I8[PITCH] = 44; + config->profile[0].pidProfile.D8[PITCH] = 60; - masterConfig.customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R - masterConfig.customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R - masterConfig.customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L - masterConfig.customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L - masterConfig.customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R - masterConfig.customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L - masterConfig.customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R - masterConfig.customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif + config->customMotorMixer[0] = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R + config->customMotorMixer[1] = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R + config->customMotorMixer[2] = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L + config->customMotorMixer[3] = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L + config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R + config->customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L + config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R + config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif } diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c index 71007bb806..0ebef5eee1 100644 --- a/src/main/target/BLUEJAYF4/config.c +++ b/src/main/target/BLUEJAYF4/config.c @@ -78,10 +78,10 @@ #include "hardware_revision.h" // alternative defaults settings for BlueJayF4 targets -void targetConfiguration(void) +void targetConfiguration(master_t *config) { if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) { - masterConfig.sensorAlignmentConfig.gyro_align = CW180_DEG; - masterConfig.sensorAlignmentConfig.acc_align = CW180_DEG; + config->sensorAlignmentConfig.gyro_align = CW180_DEG; + config->sensorAlignmentConfig.acc_align = CW180_DEG; } } diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c index 7d20ab1d90..def354fb40 100644 --- a/src/main/target/COLIBRI/config.c +++ b/src/main/target/COLIBRI/config.c @@ -79,21 +79,20 @@ #include "config/config_master.h" // alternative defaults settings for Colibri/Gemini targets -void targetConfiguration(void) +void targetConfiguration(master_t *config) { - masterConfig.mixerMode = MIXER_HEX6X; - masterConfig.rxConfig.serialrx_provider = 2; - featureSet(FEATURE_RX_SERIAL); + config->mixerMode = MIXER_HEX6X; + config->rxConfig.serialrx_provider = 2; - masterConfig.escAndServoConfig.minthrottle = 1070; - masterConfig.escAndServoConfig.maxthrottle = 2000; + config->escAndServoConfig.minthrottle = 1070; + config->escAndServoConfig.maxthrottle = 2000; - masterConfig.boardAlignment.pitchDegrees = 10; - //masterConfig.rcControlsConfig.deadband = 10; - //masterConfig.rcControlsConfig.yaw_deadband = 10; - masterConfig.mag_hardware = 1; + config->boardAlignment.pitchDegrees = 10; + //config->rcControlsConfig.deadband = 10; + //config->rcControlsConfig.yaw_deadband = 10; + config->mag_hardware = 1; - masterConfig.profile[0].controlRateProfile[0].dynThrPID = 45; - masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint = 1700; - masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; + config->profile[0].controlRateProfile[0].dynThrPID = 45; + config->profile[0].controlRateProfile[0].tpa_breakpoint = 1700; + config->serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; } diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index 77cc0f2f57..5fbed88248 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -135,7 +135,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_RX_SERIAL) +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/COLIBRI_RACE/config.c b/src/main/target/COLIBRI_RACE/config.c index bbd5a49c9c..027c5ad84b 100644 --- a/src/main/target/COLIBRI_RACE/config.c +++ b/src/main/target/COLIBRI_RACE/config.c @@ -76,9 +76,9 @@ #include "config/config_master.h" // alternative defaults settings for COLIBRI RACE targets -void targetConfiguration(void) { - masterConfig.escAndServoConfig.minthrottle = 1025; - masterConfig.escAndServoConfig.maxthrottle = 1980; - masterConfig.batteryConfig.vbatmaxcellvoltage = 45; - masterConfig.batteryConfig.vbatmincellvoltage = 30; +void targetConfiguration(master_t *config) { + config->escAndServoConfig.minthrottle = 1025; + config->escAndServoConfig.maxthrottle = 1980; + config->batteryConfig.vbatmaxcellvoltage = 45; + config->batteryConfig.vbatmincellvoltage = 30; } diff --git a/src/main/target/FURYF4/config.c b/src/main/target/FURYF4/config.c index 0a12258f2d..b1cf1dd780 100644 --- a/src/main/target/FURYF4/config.c +++ b/src/main/target/FURYF4/config.c @@ -76,11 +76,11 @@ #include "config/config_master.h" // alternative defaults settings for FURYF4 targets -void targetConfiguration(void) { - masterConfig.mag_hardware = MAG_NONE; // disabled by default - masterConfig.motor_pwm_rate = 32000; - masterConfig.failsafeConfig.failsafe_delay = 2; - masterConfig.failsafeConfig.failsafe_off_delay = 0; - masterConfig.gyro_sync_denom = 1; - masterConfig.pid_process_denom = 1; +void targetConfiguration(master_t *config) { + config->mag_hardware = MAG_NONE; // disabled by default + config->motor_pwm_rate = 32000; + config->failsafeConfig.failsafe_delay = 2; + config->failsafeConfig.failsafe_off_delay = 0; + config->gyro_sync_denom = 1; + config->pid_process_denom = 1; } diff --git a/src/main/target/MOTOLAB/config.c b/src/main/target/MOTOLAB/config.c index 490236ce04..3a617449e1 100755 --- a/src/main/target/MOTOLAB/config.c +++ b/src/main/target/MOTOLAB/config.c @@ -76,7 +76,7 @@ #include "config/config_master.h" // Motolab target supports 2 different type of boards Tornado / Cyclone. -void targetConfiguration(void) { - masterConfig.gyro_sync_denom = 4; - masterConfig.pid_process_denom = 1; +void targetConfiguration(master_t *config) { + config->gyro_sync_denom = 4; + config->pid_process_denom = 1; } From 912daec2ff7aad1a4be3bdb9450824d790078cb3 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 14 Aug 2016 21:13:02 +1200 Subject: [PATCH 327/456] Added settings for travis to post notifications to gitter. --- .travis.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.travis.yml b/.travis.yml index 16b89723f4..dff22bcc90 100644 --- a/.travis.yml +++ b/.travis.yml @@ -81,3 +81,11 @@ cache: apt # irc: "chat.freenode.net#cleanflight" # use_notice: true # skip_join: true + +notifications: + webhooks: + urls: + - https://webhooks.gitter.im/e/17e9af18c83945aad3f0 + on_success: change # options: [always|never|change] default: always + on_failure: always # options: [always|never|change] default: always + on_start: never # options: [always|never|change] default: always From 96f1651fde200cfd0fb21ff79d7e947d84fad8ad Mon Sep 17 00:00:00 2001 From: kc10kevin Date: Sun, 14 Aug 2016 07:06:03 -0500 Subject: [PATCH 328/456] Enable FuryF4 LED Strip --- src/main/target/FURYF4/target.h | 9 +++++++++ src/main/target/FURYF4/target.mk | 4 +++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 4006d9d665..2f0cfb8636 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -147,6 +147,15 @@ #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS +#define LED_STRIP +#define WS2811_PIN PA0 +#define WS2811_TIMER TIM5 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream2 +#define WS2811_DMA_IT DMA_IT_TCIF2 +#define WS2811_DMA_CHANNEL DMA_Channel_6 +#define WS2811_TIMER_CHANNEL TIM_Channel_1 + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define SPEKTRUM_BIND diff --git a/src/main/target/FURYF4/target.mk b/src/main/target/FURYF4/target.mk index 958e5c68de..80793c7217 100644 --- a/src/main/target/FURYF4/target.mk +++ b/src/main/target/FURYF4/target.mk @@ -5,5 +5,7 @@ TARGET_SRC = \ drivers/accgyro_spi_mpu6000.c \ drivers/accgyro_spi_mpu6500.c \ drivers/accgyro_mpu6500.c \ - drivers/barometer_ms5611.c + drivers/barometer_ms5611.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f4xx.c From c077bacee6c4c5bcf0f2d7a373f093031f4d1942 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 15 Aug 2016 07:32:06 +0100 Subject: [PATCH 329/456] Improved acc initialisation --- src/main/config/config_master.h | 2 +- src/main/io/serial_cli.c | 2 +- src/main/main.c | 17 +-------- src/main/sensors/acceleration.c | 52 +++++++++++++++++--------- src/main/sensors/acceleration.h | 9 +++-- src/main/sensors/gyro.c | 4 +- src/main/sensors/initialisation.c | 61 ++++++++++++++++++------------- 7 files changed, 82 insertions(+), 65 deletions(-) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 0f79ee2f44..ad6cc09b5b 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -78,7 +78,7 @@ typedef struct master_t { rollAndPitchTrims_t accelerometerTrims; // accelerometer trim - float acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz + uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz accDeadband_t accDeadband; barometerConfig_t barometerConfig; uint8_t acc_unarmedcal; // turn automatic acc compensation on/off diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a3463f9de8..f8491f4f2a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -820,7 +820,7 @@ const clivalue_t valueTable[] = { #endif { "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } }, - { "acc_lpf_hz", VAR_FLOAT | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0, 400 } }, + { "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0, 400 } }, { "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.xy, .config.minmax = { 0, 100 } }, { "accz_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.z, .config.minmax = { 0, 100 } }, { "acc_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } }, diff --git a/src/main/main.c b/src/main/main.c index 9c4336e7cd..94302e61da 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -683,22 +683,7 @@ void main_init(void) if (sensors(SENSOR_ACC)) { setTaskEnabled(TASK_ACCEL, true); - switch (gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future - case 500: - case 375: - case 250: - case 125: - accTargetLooptime = 1000; - break; - default: - case 1000: -#ifdef STM32F10X - accTargetLooptime = 1000; -#else - accTargetLooptime = 1000; -#endif - } - rescheduleTask(TASK_ACCEL, accTargetLooptime); + rescheduleTask(TASK_ACCEL, accSamplingInterval); } setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC)); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 3016e97325..07cd90b204 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -41,7 +41,7 @@ int32_t accSmooth[XYZ_AXIS_COUNT]; acc_t acc; // acc access functions sensor_align_e accAlign = 0; -uint32_t accTargetLooptime; +uint32_t accSamplingInterval; static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. @@ -52,9 +52,33 @@ extern bool AccInflightCalibrationActive; static flightDynamicsTrims_t *accelerationTrims; -static float accLpfCutHz = 0; +static uint16_t accLpfCutHz = 0; static biquadFilter_t accFilter[XYZ_AXIS_COUNT]; -static bool accFilterInitialised = false; + +void accInit(uint32_t gyroSamplingInverval) +{ + // set the acc sampling interval according to the gyro sampling interval + switch (gyroSamplingInverval) { // Switch statement kept in place to change acc sampling interval in the future + case 500: + case 375: + case 250: + case 125: + accSamplingInterval = 1000; + break; + case 1000: + default: +#ifdef STM32F10X + accSamplingInterval = 1000; +#else + accSamplingInterval = 1000; +#endif + } + if (accLpfCutHz) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSamplingInterval); + } + } +} void accSetCalibrationCycles(uint16_t calibrationCyclesRequired) { @@ -188,19 +212,8 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) } if (accLpfCutHz) { - if (!accFilterInitialised) { - if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */ - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime); - } - accFilterInitialised = true; - } - } - - if (accFilterInitialised) { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis])); - } + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis])); } } @@ -222,7 +235,12 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse) accelerationTrims = accelerationTrimsToUse; } -void setAccelerationFilter(float initialAccLpfCutHz) +void setAccelerationFilter(uint16_t initialAccLpfCutHz) { accLpfCutHz = initialAccLpfCutHz; + if (accSamplingInterval) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSamplingInterval); + } + } } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index dc98ae78ef..4630faa3b5 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -34,7 +34,7 @@ typedef enum { extern sensor_align_e accAlign; extern acc_t acc; -extern uint32_t accTargetLooptime; +extern uint32_t accSamplingInterval; extern int32_t accSmooth[XYZ_AXIS_COUNT]; @@ -48,9 +48,12 @@ typedef union rollAndPitchTrims_u { rollAndPitchTrims_t_def values; } rollAndPitchTrims_t; +void accInit(uint32_t gyroTargetLooptime); bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims); void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims); -void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse); -void setAccelerationFilter(float initialAccLpfCutHz); +union flightDynamicsTrims_u; +void setAccelerationTrims(union flightDynamicsTrims_u *accelerationTrimsToUse); +void setAccelerationFilter(uint16_t initialAccLpfCutHz); + diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c6d4c121a7..bc1cae6500 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -103,7 +103,7 @@ void gyroSetCalibrationCycles(void) calibratingG = gyroCalculateCalibratingCycles(); } -static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold) +static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold) { static int32_t g[3]; static stdev_t var[3]; @@ -166,7 +166,7 @@ void gyroUpdate(void) alignSensors(gyroADC, gyroADC, gyroAlign); if (!isGyroCalibrationComplete()) { - performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold); + performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold); } applyGyroZero(); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index a122c8aacc..6e96068f4f 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -277,7 +277,7 @@ bool detectGyro(void) return true; } -static void detectAcc(accelerationSensor_e accHardwareToUse) +static bool detectAcc(accelerationSensor_e accHardwareToUse) { accelerationSensor_e accHardware; @@ -407,24 +407,22 @@ retry: if (accHardware == ACC_NONE) { - return; + return false; } detectedSensors[SENSOR_INDEX_ACC] = accHardware; sensorsSet(SENSOR_ACC); + return true; } -static void detectBaro(baroSensor_e baroHardwareToUse) +#ifdef BARO +static bool detectBaro(baroSensor_e baroHardwareToUse) { -#ifndef BARO - UNUSED(baroHardwareToUse); -#else // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function baroSensor_e baroHardware = baroHardwareToUse; #ifdef USE_BARO_BMP085 - const bmp085Config_t *bmp085Config = NULL; #if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO) @@ -476,15 +474,17 @@ static void detectBaro(baroSensor_e baroHardwareToUse) } if (baroHardware == BARO_NONE) { - return; + return false; } detectedSensors[SENSOR_INDEX_BARO] = baroHardware; sensorsSet(SENSOR_BARO); -#endif + return true; } +#endif -static void detectMag(magSensor_e magHardwareToUse) +#ifdef MAG +static bool detectMag(magSensor_e magHardwareToUse) { magSensor_e magHardware; @@ -571,12 +571,14 @@ retry: } if (magHardware == MAG_NONE) { - return; + return false; } detectedSensors[SENSOR_INDEX_MAG] = magHardware; sensorsSet(SENSOR_MAG); + return true; } +#endif void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) { @@ -613,32 +615,41 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, if (!detectGyro()) { return false; } - detectAcc(accHardwareToUse); - detectBaro(baroHardwareToUse); - // Now time to init things, acc first - if (sensors(SENSOR_ACC)) { - acc.acc_1G = 256; // set default - acc.init(&acc); - } + // Now time to init things // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation - gyro.init(gyroLpf); - gyroInit(); + gyro.init(gyroLpf); // driver initialisation + gyroInit(); // sensor initialisation - detectMag(magHardwareToUse); + if (detectAcc(accHardwareToUse)) { + acc.acc_1G = 256; // set default + acc.init(&acc); // driver initialisation + accInit(gyro.targetLooptime); // sensor initialisation + } - reconfigureAlignment(sensorAlignmentConfig); + magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. +#ifdef MAG // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c - if (sensors(SENSOR_MAG)) { + if (detectMag(magHardwareToUse)) { // calculate magnetic declination const int16_t deg = magDeclinationFromConfig / 100; const int16_t min = magDeclinationFromConfig % 100; magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units - } else { - magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. } +#else + UNUSED(magHardwareToUse); + UNUSED(magDeclinationFromConfig); +#endif + +#ifdef BARO + detectBaro(baroHardwareToUse); +#else + UNUSED(baroHardwareToUse); +#endif + + reconfigureAlignment(sensorAlignmentConfig); return true; } From ac7e7ee5a90130b95c96e6fc9fd27984a2c1815b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 15 Aug 2016 12:09:17 +0200 Subject: [PATCH 330/456] Increase GPIO speed to 50Mhz for i2c as per @digitalentity inav --- src/main/drivers/bus_i2c_stm32f10x.c | 8 ++++++-- src/main/drivers/bus_i2c_stm32f30x.c | 4 ++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index 7daea6fa95..be298d3cf9 100644 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -53,19 +53,23 @@ static void i2cUnstick(IO_t scl, IO_t sda); #ifndef I2C1_SDA #define I2C1_SDA PB9 #endif + #else + #ifndef I2C1_SCL #define I2C1_SCL PB6 #endif #ifndef I2C1_SDA #define I2C1_SDA PB7 #endif +#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF_OD, GPIO_Speed_50MHz) #endif #ifndef I2C2_SCL #define I2C2_SCL PB10 #endif + #ifndef I2C2_SDA #define I2C2_SDA PB11 #endif @@ -398,8 +402,8 @@ void i2cInit(I2CDevice device) IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_I2C); IOConfigGPIOAF(sda, IOCFG_I2C, GPIO_AF_I2C); #else - IOConfigGPIO(scl, IOCFG_AF_OD); - IOConfigGPIO(sda, IOCFG_AF_OD); + IOConfigGPIO(scl, IOCFG_I2C); + IOConfigGPIO(sda, IOCFG_I2C); #endif I2C_DeInit(i2c->dev); diff --git a/src/main/drivers/bus_i2c_stm32f30x.c b/src/main/drivers/bus_i2c_stm32f30x.c index 88b267f111..d06eeece4c 100644 --- a/src/main/drivers/bus_i2c_stm32f30x.c +++ b/src/main/drivers/bus_i2c_stm32f30x.c @@ -29,9 +29,9 @@ #ifndef SOFT_I2C #if defined(USE_I2C_PULLUP) -#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_UP) +#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP) #else -#define IOCFG_I2C IOCFG_AF_OD +#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL) #endif #define I2C_HIGHSPEED_TIMING 0x00500E30 // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4. From 190b1053d49930712609af93a1b59a52e29759f2 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 15 Aug 2016 20:45:41 +1000 Subject: [PATCH 331/456] Disabling CS pin is to take it high, and enabling is to bring it low. --- src/main/target/NAZE/hardware_revision.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/NAZE/hardware_revision.c b/src/main/target/NAZE/hardware_revision.c index 3f87237dbb..3e05f20632 100755 --- a/src/main/target/NAZE/hardware_revision.c +++ b/src/main/target/NAZE/hardware_revision.c @@ -53,8 +53,8 @@ void detectHardwareRevision(void) #ifdef USE_SPI -#define DISABLE_SPI_CS IOLo(nazeSpiCsPin) -#define ENABLE_SPI_CS IOHi(nazeSpiCsPin) +#define DISABLE_SPI_CS IOHi(nazeSpiCsPin) +#define ENABLE_SPI_CS IOLo(nazeSpiCsPin) #define SPI_DEVICE_NONE (0) #define SPI_DEVICE_FLASH (1) From af8d49e5eeb7fd41cc89952779404f15ebb3aa12 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 15 Aug 2016 13:06:34 +0100 Subject: [PATCH 332/456] Added beeper to microscisky --- src/main/target/MICROSCISKY/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 9138f74e86..1d9c3fa70e 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -22,6 +22,8 @@ #define LED0 PB3 #define LED1 PB4 +#define BEEPER PA12 + #define BARO_XCLR_PIN PC13 #define BARO_EOC_PIN PC14 From 783c53a7fdd13023666349124e8a93f95af97ab0 Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Mon, 15 Aug 2016 18:01:43 +0300 Subject: [PATCH 333/456] Fix colibri input timers and add OPBL mk --- src/main/target/COLIBRI/COLIBRI_OPBL.mk | 0 src/main/target/COLIBRI/target.c | 33 ++++++++++++------------- 2 files changed, 16 insertions(+), 17 deletions(-) create mode 100644 src/main/target/COLIBRI/COLIBRI_OPBL.mk diff --git a/src/main/target/COLIBRI/COLIBRI_OPBL.mk b/src/main/target/COLIBRI/COLIBRI_OPBL.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/COLIBRI/target.c b/src/main/target/COLIBRI/target.c index dd87343119..ee8ec85288 100644 --- a/src/main/target/COLIBRI/target.c +++ b/src/main/target/COLIBRI/target.c @@ -102,22 +102,21 @@ const uint16_t airPWM[] = { }; const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM1 }, // S1_IN - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S2_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S4_IN - { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM2 }, // S5_IN - { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM2 }, // S6_IN - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM5 }, // S7_IN - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM5 }, // S8_IN + { TIM1, IO_TAG(PA10), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1 }, // S1_IN + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S2_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // S4_IN + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S5_IN + { TIM2, IO_TAG(PB3), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM2 }, // S6_IN + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S7_IN + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S8_IN - - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S2_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S3_OUT - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12 }, // S4_OUT - { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S5_OUT - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM12 }, // S6_OUT - { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM10 }, // S7_OUT - { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM11 }, // S8_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S2_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S3_OUT + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S4_OUT + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM3 }, // S5_OUT + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM12 }, // S6_OUT + { TIM10, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM10_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM10 }, // S7_OUT + { TIM11, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM11_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM11 }, // S8_OUT }; From c75201614d4602d43cf5afd66b6066fb2880d9e2 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 15 Aug 2016 13:06:34 +0100 Subject: [PATCH 334/456] Added beeper to microscisky --- src/main/target/MICROSCISKY/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 80d4f04856..815f2f6eb8 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -22,6 +22,8 @@ #define LED0 PB3 #define LED1 PB4 +#define BEEPER PA12 + #define BARO_XCLR_PIN PC13 #define BARO_EOC_PIN PC14 From d5251b868a4eb81033bb85ddc5fe234bd52ace06 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 15 Aug 2016 21:57:31 +0200 Subject: [PATCH 335/456] Restore rcCommand based cam angle mix as per 2.9 --- src/main/mw.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index 3652108743..86abfd6c63 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -203,10 +203,10 @@ void scaleRcCommandToFpvCamAngle(void) { sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD); } - int16_t roll = setpointRate[ROLL]; - int16_t yaw = setpointRate[YAW]; - setpointRate[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500); - setpointRate[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500); + int16_t roll = rcCommand[ROLL]; + int16_t yaw = rcCommand[YAW]; + rcCommand[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500); + rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500); } void processRcCommand(void) @@ -263,13 +263,13 @@ void processRcCommand(void) } if (readyToCalculateRate || isRXDataNew) { - for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); - - isRXDataNew = false; - // Scaling of AngleRate to camera angle (Mixing Roll and Yaw) if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) scaleRcCommandToFpvCamAngle(); + + for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); + + isRXDataNew = false; } } From 4695c422b8a23b8ed014b0f10ab941b0633c00a1 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 15 Aug 2016 23:07:38 +0200 Subject: [PATCH 336/456] Update gitter room --- .travis.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.travis.yml b/.travis.yml index 16b89723f4..454ee87d1c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -81,3 +81,11 @@ cache: apt # irc: "chat.freenode.net#cleanflight" # use_notice: true # skip_join: true + +notifications: + webhooks: + urls: + - https://webhooks.gitter.im/e/c99e3576fde40c823185 + on_success: change # options: [always|never|change] default: always + on_failure: always # options: [always|never|change] default: always + on_start: never # options: [always|never|change] default: always From b0d9b791255727d1907d167c75da2cd86fb5225e Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Mon, 15 Aug 2016 23:07:38 +0200 Subject: [PATCH 337/456] Update gitter room --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index dff22bcc90..454ee87d1c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -85,7 +85,7 @@ cache: apt notifications: webhooks: urls: - - https://webhooks.gitter.im/e/17e9af18c83945aad3f0 + - https://webhooks.gitter.im/e/c99e3576fde40c823185 on_success: change # options: [always|never|change] default: always on_failure: always # options: [always|never|change] default: always on_start: never # options: [always|never|change] default: always From 287d398b13290599e0ff788a749b7c9ab62fa9e9 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 13 Aug 2016 21:24:16 +1200 Subject: [PATCH 338/456] Added 'CLI_MINIMAL_VERBOSITY' option for reduced image size. Reenabled LED_STRIP for CC3D. --- src/main/io/serial_cli.c | 71 ++++++++++++++++++++++++++++++----- src/main/target/CC3D/target.h | 5 ++- src/main/target/NAZE/target.h | 1 - 3 files changed, 64 insertions(+), 13 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 4164f4e14a..7792322286 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -146,7 +146,7 @@ static void cliTasks(char *cmdline); #endif static void cliVersion(char *cmdline); static void cliRxRange(char *cmdline); -#if (FLASH_SIZE > 64) +#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) static void cliResource(char *cmdline); #endif #ifdef GPS @@ -231,7 +231,7 @@ static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT { RX_FAILSAFE_MODE_INVALID, RX_FAILSAFE_MODE_HOLD, RX_FAILSAFE_MODE_SET } }; -#if (FLASH_SIZE > 64) +#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) // sync this with sensors_e static const char * const sensorTypeNames[] = { "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL @@ -322,7 +322,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("profile", "change profile", "[]", cliProfile), CLI_COMMAND_DEF("rateprofile", "change rate profile", "[]", cliRateProfile), -#if (FLASH_SIZE > 64) +#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), #endif CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), @@ -1446,7 +1446,9 @@ static void cliMotorMix(char *cmdline) char *ptr; if (isEmpty(cmdline)) { +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("Motor\tThr\tRoll\tPitch\tYaw\r\n"); +#endif for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) { if (masterConfig.customMotorMixer[i].throttle == 0.0f) break; @@ -1831,7 +1833,9 @@ static void cliServoMix(char *cmdline) if (len == 0) { +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("Rule\tServo\tSource\tRate\tSpeed\tMin\tMax\tBox\r\n"); +#endif for (i = 0; i < MAX_SERVO_RULES; i++) { if (masterConfig.customServoMixer[i].rate == 0) @@ -2320,18 +2324,24 @@ static void printConfig(char *cmdline, bool doDiff) } if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# version\r\n"); +#endif cliVersion(NULL); +#ifndef CLI_MINIMAL_VERBOSITY if ((dumpMask & (DUMP_ALL | DO_DIFF)) == (DUMP_ALL | DO_DIFF)) { cliPrint("\r\n# reset configuration to default settings\r\ndefaults\r\n"); } cliPrint("\r\n# name\r\n"); +#endif printName(dumpMask); - cliPrint("\r\n# mixer\r\n"); #ifndef USE_QUAD_MIXER_ONLY +#ifndef CLI_MINIMAL_VERBOSITY + cliPrint("\r\n# mixer\r\n"); +#endif cliDumpPrintf(dumpMask, COMPARE_CONFIG(mixerMode), "mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]); cliDumpPrintf(dumpMask, masterConfig.customMotorMixer[0].throttle == 0.0f, "mmix reset\r\n"); @@ -2396,7 +2406,9 @@ static void printConfig(char *cmdline, bool doDiff) #endif #endif +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# feature\r\n"); +#endif mask = featureMask(); defaultMask = defaultConfig.enabledFeatures; @@ -2413,7 +2425,9 @@ static void printConfig(char *cmdline, bool doDiff) } #ifdef BEEPER +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# beeper\r\n"); +#endif uint8_t beeperCount = beeperTableEntryCount(); mask = getBeeperOffMask(); defaultMask = defaultConfig.beeper_off_flags; @@ -2425,7 +2439,9 @@ static void printConfig(char *cmdline, bool doDiff) } #endif +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# map\r\n"); +#endif equalsDefault = true; for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; @@ -2434,43 +2450,65 @@ static void printConfig(char *cmdline, bool doDiff) buf[i] = '\0'; cliDumpPrintf(dumpMask, equalsDefault, "map %s\r\n", buf); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# serial\r\n"); +#endif printSerial(dumpMask, &defaultConfig); #ifdef LED_STRIP +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# led\r\n"); +#endif printLed(dumpMask, &defaultConfig); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# color\r\n"); +#endif printColor(dumpMask, &defaultConfig); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# mode_color\r\n"); +#endif printModeColor(dumpMask, &defaultConfig); #endif +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# aux\r\n"); +#endif printAux(dumpMask, &defaultConfig); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# adjrange\r\n"); +#endif printAdjustmentRange(dumpMask, &defaultConfig); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# rxrange\r\n"); +#endif printRxRange(dumpMask, &defaultConfig); #ifdef USE_SERVOS +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# servo\r\n"); +#endif printServo(dumpMask, &defaultConfig); #endif #ifdef VTX +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# vtx\r\n"); +#endif printVtx(dumpMask, &defaultConfig); #endif +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# rxfail\r\n"); +#endif printRxFail(dumpMask, &defaultConfig); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# master\r\n"); +#endif dumpValues(MASTER_VALUE, dumpMask, &defaultConfig); if (dumpMask & DUMP_ALL) { @@ -2484,16 +2522,20 @@ static void printConfig(char *cmdline, bool doDiff) for (rateCount=0; rateCountactiveRateProfile, dumpMask, &defaultConfig); @@ -2514,7 +2556,9 @@ static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask, master_t *def if (profileIndex >= MAX_PROFILE_COUNT) // Faulty values return; changeProfile(profileIndex); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# profile\r\n"); +#endif cliProfile(""); dumpValues(PROFILE_VALUE, dumpMask, defaultConfig); } @@ -2524,7 +2568,9 @@ static void cliDumpRateProfile(uint8_t rateProfileIndex, uint8_t dumpMask, maste if (rateProfileIndex >= MAX_RATEPROFILES) // Faulty values return; changeControlRateProfile(rateProfileIndex); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# rateprofile\r\n"); +#endif cliRateProfile(""); dumpValues(PROFILE_RATE_VALUE, dumpMask, defaultConfig); } @@ -2537,7 +2583,9 @@ void cliEnter(serialPort_t *serialPort) cliWriter = bufWriterInit(cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n"); +#endif cliPrompt(); ENABLE_ARMING_FLAG(PREVENT_ARMING); } @@ -2546,7 +2594,9 @@ static void cliExit(char *cmdline) { UNUSED(cmdline); +#ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\nLeaving CLI mode, unsaved changes lost.\r\n"); +#endif bufWriterFlush(cliWriter); *cliBuffer = '\0'; @@ -2842,7 +2892,7 @@ static void cliMotor(char *cmdline) static void cliPlaySound(char *cmdline) { -#if FLASH_SIZE <= 64 +#if (FLASH_SIZE <= 64) && !defined(CLI_MINIMAL_VERBOSITY) UNUSED(cmdline); #else int i; @@ -2934,7 +2984,6 @@ static void cliSave(char *cmdline) UNUSED(cmdline); cliPrint("Saving"); - //copyCurrentProfileToProfileSlot(masterConfig.current_profile_index); writeEEPROM(); cliReboot(); } @@ -3241,7 +3290,7 @@ static void cliStatus(char *cmdline) cliPrintf("CPU Clock=%dMHz", (SystemCoreClock / 1000000)); -#if (FLASH_SIZE > 64) +#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) uint8_t i; uint32_t mask; uint32_t detectedSensorsMask = sensorsMask(); @@ -3287,7 +3336,9 @@ static void cliTasks(char *cmdline) int maxLoadSum = 0; int averageLoadSum = 0; +#ifndef CLI_MINIMAL_VERBOSITY cliPrintf("Task list rate/hz max/us avg/us maxload avgload total/ms\r\n"); +#endif for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; taskId++) { cfTaskInfo_t taskInfo; getTaskInfo(taskId, &taskInfo); @@ -3448,7 +3499,7 @@ void cliProcess(void) } } -#if (FLASH_SIZE > 64) +#if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) static void cliResource(char *cmdline) { UNUSED(cmdline); diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 342e4c0692..cfcc9b6831 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -91,7 +91,7 @@ #define VBAT_ADC_PIN PA0 #define RSSI_ADC_PIN PB0 -//#define LED_STRIP +#define LED_STRIP #define WS2811_PIN PB4 #define WS2811_TIMER TIM3 #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 @@ -108,15 +108,16 @@ //#define SONAR_TRIGGER_PIN PB5 #undef GPS +#define CLI_MINIMAL_VERBOSITY #ifdef CC3D_OPBL -#undef LED_STRIP #define SKIP_CLI_COMMAND_HELP #define SKIP_PID_FLOAT #undef BARO #undef MAG #undef SONAR #undef USE_SOFTSERIAL1 +#undef LED_STRIP #endif #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 212ea2c83f..a3b2bb1f34 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -143,7 +143,6 @@ #define RSSI_ADC_PIN PA1 #define EXTERNAL1_ADC_PIN PA5 - #define LED_STRIP #define WS2811_TIMER TIM3 #define WS2811_PIN PA6 From 8be39efc889729e6ebdd6779fcfa4b13de26005e Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 16 Aug 2016 08:59:26 +0200 Subject: [PATCH 339/456] Modify gitter --- .travis.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 454ee87d1c..851cb0ce1e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -85,7 +85,8 @@ cache: apt notifications: webhooks: urls: - - https://webhooks.gitter.im/e/c99e3576fde40c823185 - on_success: change # options: [always|never|change] default: always + - https://webhooks.gitter.im/e/0c20f7a1a7e311499a88 + on_success: always # options: [always|never|change] default: always on_failure: always # options: [always|never|change] default: always - on_start: never # options: [always|never|change] default: always + on_start: always # options: [always|never|change] default: always + From 0dae13916f7837dcb8ed08a94a38fbdca4471bab Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 16 Aug 2016 10:44:42 +0200 Subject: [PATCH 340/456] AIRHEROF3 build failed. Added missing include. Target not covered by Travis. --- src/main/target/AIRHEROF3/target.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/AIRHEROF3/target.c b/src/main/target/AIRHEROF3/target.c index 08ff2bcbce..4b516689ab 100755 --- a/src/main/target/AIRHEROF3/target.c +++ b/src/main/target/AIRHEROF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM4 | (MAP_TO_PPM_INPUT << 8), // PPM input From 849ea489b65362a4b24d1764f6d01e689ba3e408 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Tue, 16 Aug 2016 18:34:23 +0200 Subject: [PATCH 341/456] Fixed some outdated info in Vagrantfile. Updated to gcc5.4.1 and added some usage example. --- Vagrantfile | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/Vagrantfile b/Vagrantfile index ae6b26551f..af1c6a064f 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -19,8 +19,18 @@ Vagrant.configure(2) do |config| # documentation for more information about their specific syntax and use. config.vm.provision "shell", inline: <<-SHELL apt-get remove -y binutils-arm-none-eabi gcc-arm-none-eabi - add-apt-repository ppa:terry.guo/gcc-arm-embedded + add-apt-repository ppa:team-gcc-arm-embedded/ppa apt-get update - apt-get install -y git gcc-arm-none-eabi=4.9.3.2015q3-1trusty1 + apt-get install -y git ccache gcc-arm-embedded=5-2016q2-1~trusty1 SHELL end + +# Usage +# On windows start a command shell in the project root directory, where the "Vagrantfile" exists. +# "vagrant up" to start the VM. First time it takes a while..... +# "vagrant ssh" to log into your VM. +# "cd /vagrant" Here are the windows project directory mounted with all your files. +# "make all" Start working, building all targets for example. +# "exit" when done +# vagrant halt to stop your VM +# vagrant --help for more.... From 2be9e6c0fe8a5253322b57860450c5a3c27397d2 Mon Sep 17 00:00:00 2001 From: Zap Andersson Date: Wed, 17 Aug 2016 22:41:49 +0200 Subject: [PATCH 342/456] Fix throttle stick deadband behaviour in 3D Mode --- src/main/flight/mixer.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 303f6960bc..45bea6f1dc 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -802,11 +802,13 @@ void mixTable(void *pidProfilePtr) if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; - throttlePrevious = throttle = rcCommand[THROTTLE]; + throttlePrevious = rcCommand[THROTTLE]; + throttle = rcCommand[THROTTLE] + flight3DConfig->deadband3d_throttle; } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling throttleMax = escAndServoConfig->maxthrottle; throttleMin = flight3DConfig->deadband3d_high; - throttlePrevious = throttle = rcCommand[THROTTLE]; + throttlePrevious = rcCommand[THROTTLE]; + throttle = rcCommand[THROTTLE] - flight3DConfig->deadband3d_throttle; } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive throttle = throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; From 522048c8f2d3fad15f0f5abfc63c4dd9167d0290 Mon Sep 17 00:00:00 2001 From: Zap Andersson Date: Wed, 17 Aug 2016 22:45:04 +0200 Subject: [PATCH 343/456] Fixed whitespace --- src/main/flight/mixer.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 45bea6f1dc..df45ddee00 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -803,12 +803,12 @@ void mixTable(void *pidProfilePtr) throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] + flight3DConfig->deadband3d_throttle; + throttle = rcCommand[THROTTLE] + flight3DConfig->deadband3d_throttle; } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling throttleMax = escAndServoConfig->maxthrottle; throttleMin = flight3DConfig->deadband3d_high; throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] - flight3DConfig->deadband3d_throttle; + throttle = rcCommand[THROTTLE] - flight3DConfig->deadband3d_throttle; } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive throttle = throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; From f936b92c64dbeffd8b9538d8468e124e3fc4624b Mon Sep 17 00:00:00 2001 From: Zap Andersson Date: Wed, 17 Aug 2016 22:51:02 +0200 Subject: [PATCH 344/456] Fix whitespaces - again Damned editor and their damned tabs. Tabs should be nuked from orbit! --- src/main/flight/mixer.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index df45ddee00..6f6dbb3ae2 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -803,12 +803,12 @@ void mixTable(void *pidProfilePtr) throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] + flight3DConfig->deadband3d_throttle; + throttle = rcCommand[THROTTLE] + flight3DConfig->deadband3d_throttle; } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling throttleMax = escAndServoConfig->maxthrottle; throttleMin = flight3DConfig->deadband3d_high; throttlePrevious = rcCommand[THROTTLE]; - throttle = rcCommand[THROTTLE] - flight3DConfig->deadband3d_throttle; + throttle = rcCommand[THROTTLE] - flight3DConfig->deadband3d_throttle; } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive throttle = throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; From 69e2c991e36d792a22b01d133ebd364f76b31307 Mon Sep 17 00:00:00 2001 From: Gary Keeble Date: Tue, 16 Aug 2016 12:51:16 +0100 Subject: [PATCH 345/456] Test Mode Blackbox Logging Prevent pausing of the log in test mode if a mode switch is configured Add CLI Command and protect against blackbox storage full Added CLI command blackbox_on_motor_test (ON/OFF). Test Mode Blackbox Logging Prevent pausing of the log in test mode if a mode switch is configured Add CLI Command and protect against blackbox storage full Added CLI command blackbox_on_motor_test (ON/OFF). Cleanup as per BorisB recommendations Remove un-needed header file entries and convert to millis(). Updated as per latest comments --- src/main/blackbox/blackbox.c | 74 ++++++++++++++++++++++++++++++++- src/main/config/config.c | 1 + src/main/config/config_master.h | 1 + src/main/io/serial_cli.c | 2 + 4 files changed, 77 insertions(+), 1 deletion(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 09946a4ab0..d829f9babb 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -893,6 +893,61 @@ void finishBlackbox(void) } } +/** + * Test Motors Blackbox Logging + */ +bool startedLoggingInTestMode = false; + +void startInTestMode(void) +{ + if(!startedLoggingInTestMode) { + if (masterConfig.blackbox_device == BLACKBOX_DEVICE_SERIAL) { + serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); + if (sharedBlackboxAndMspPort) { + return; // When in test mode, we cannot share the MSP and serial logger port! + } + } + startBlackbox(); + startedLoggingInTestMode = true; + } +} +void stopInTestMode(void) +{ + if(startedLoggingInTestMode) { + finishBlackbox(); + startedLoggingInTestMode = false; + } +} +/** + * We are going to monitor the MSP_SET_MOTOR target variables motor_disarmed[] for values other than minthrottle + * on reading a value (i.e. the user is testing the motors), then we enable test mode logging; + * we monitor when the values return to minthrottle and start a delay timer (5 seconds); if + * the test motors are left at minimum throttle for this delay timer, then we assume we are done testing and + * shutdown the logger. + * + * Of course, after the 5 seconds and shutdown of the logger, the system will be re-enabled to allow the + * test mode to trigger again; its just that the data will be in a second, third, fourth etc log file. + */ +bool inMotorTestMode(void) { + static uint32_t resetTime = 0; + uint16_t activeMinimumCommand = (feature(FEATURE_3D) ? masterConfig.flight3DConfig.neutral3d : masterConfig.escAndServoConfig.mincommand); + int i; + bool motorsNotAtMin = false; + + // set disarmed motor values + for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) + motorsNotAtMin |= (motor_disarmed[i] != activeMinimumCommand); + + if(motorsNotAtMin) { + resetTime = millis() + 5000; // add 5 seconds + return true; + } else { + // Monitor the duration at minimum + return (millis() < resetTime); + } + return false; +} + #ifdef GPS static void writeGPSHomeFrame() { @@ -1533,7 +1588,8 @@ void handleBlackbox(void) break; case BLACKBOX_STATE_RUNNING: // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0 - if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX)) { + // Prevent the Pausing of the log on the mode switch if in Motor Test Mode + if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX) && !startedLoggingInTestMode) { blackboxSetState(BLACKBOX_STATE_PAUSED); } else { blackboxLogIteration(); @@ -1562,6 +1618,22 @@ void handleBlackbox(void) // Did we run out of room on the device? Stop! if (isBlackboxDeviceFull()) { blackboxSetState(BLACKBOX_STATE_STOPPED); + // ensure we reset the test mode flag if we stop due to full memory card + if (startedLoggingInTestMode) startedLoggingInTestMode = false; + } else { // Only log in test mode if there is room! + + if(masterConfig.blackbox_on_motor_test) { + // Handle Motor Test Mode + if(inMotorTestMode()) { + if(blackboxState==BLACKBOX_STATE_STOPPED) { + startInTestMode(); + } + } else { + if(blackboxState!=BLACKBOX_STATE_STOPPED) { + stopInTestMode(); + } + } + } } } diff --git a/src/main/config/config.c b/src/main/config/config.c index 361173a7b7..87aa098e1c 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -680,6 +680,7 @@ void createDefaultConfig(master_t *config) config->blackbox_rate_num = 1; config->blackbox_rate_denom = 1; + config->blackbox_on_motor_test = 0; // default off #endif // BLACKBOX #ifdef SERIALRX_UART diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 0f79ee2f44..aa572fac0a 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -159,6 +159,7 @@ typedef struct master_t { uint8_t blackbox_rate_num; uint8_t blackbox_rate_denom; uint8_t blackbox_device; + uint8_t blackbox_on_motor_test; #endif uint32_t beeper_off_flags; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f8e378e67b..98c92a69ca 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -885,6 +885,8 @@ const clivalue_t valueTable[] = { { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 } }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 } }, { "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_device, .config.lookup = { TABLE_BLACKBOX_DEVICE } }, + { "blackbox_on_motor_test", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_on_motor_test, .config.lookup = { TABLE_OFF_ON } }, + #endif #ifdef VTX From fe0a8d602e29ea24dd74d870f3051c4334616e58 Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 16 Aug 2016 03:18:25 +1200 Subject: [PATCH 346/456] Added default value display for CLI dump / diff. --- src/main/io/ledstrip.c | 4 +- src/main/io/ledstrip.h | 2 +- src/main/io/serial_cli.c | 988 ++++++++++++++++++++++----------------- 3 files changed, 558 insertions(+), 436 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 03c4c5d1ef..4af92de4d3 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -394,13 +394,11 @@ bool parseLedStripConfig(int ledIndex, const char *config) return true; } -void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize) +void generateLedConfig(ledConfig_t *ledConfig, char *ledConfigBuffer, size_t bufferSize) { char directions[LED_DIRECTION_COUNT + 1]; char baseFunctionOverlays[LED_OVERLAY_COUNT + 2]; - ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex]; - memset(ledConfigBuffer, 0, bufferSize); char *dptr = directions; diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 39b3cad6d2..30450cf05d 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -160,7 +160,7 @@ PG_DECLARE(specialColorIndexes_t, specialColors); bool parseColor(int index, const char *colorConfig); bool parseLedStripConfig(int ledIndex, const char *config); -void generateLedConfig(int ledIndex, char *ledConfigBuffer, size_t bufferSize); +void generateLedConfig(ledConfig_t *ledConfig, char *ledConfigBuffer, size_t bufferSize); void reevaluateLedConfig(void); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, modeColorIndexes_t *modeColorsToUse, specialColorIndexes_t *specialColorsToUse); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f8e378e67b..14356b0e29 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -197,10 +197,10 @@ typedef enum { DUMP_RATES = (1 << 2), DUMP_ALL = (1 << 3), DO_DIFF = (1 << 4), - DIFF_COMMENTED = (1 << 5), + SHOW_DEFAULTS = (1 << 5), } dumpFlags_e; -static const char* const sectionBreak = "\r\n"; +static const char* const emptyName = "-"; #ifndef USE_QUAD_MIXER_ONLY // sync this with mixerMode_e @@ -283,9 +283,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), CLI_COMMAND_DEF("diff", "list configuration changes from default", - "[master|profile|rates|all] {commented}", cliDiff), + "[master|profile|rates|all] {showdefaults}", cliDiff), CLI_COMMAND_DEF("dump", "dump configuration", - "[master|profile|rates|all]", cliDump), + "[master|profile|rates|all] {showdefaults}", cliDump), CLI_COMMAND_DEF("exit", NULL, NULL, cliExit), CLI_COMMAND_DEF("feature", "configure features", "list\r\n" @@ -934,13 +934,14 @@ typedef union { static void cliSetVar(const clivalue_t *var, const int_float_value_t value); static void cliPrintVar(const clivalue_t *var, uint32_t full); +static void cliPrintVarDefault(const clivalue_t *var, uint32_t full, master_t *defaultConfig); static void cliPrintVarRange(const clivalue_t *var); static void cliPrint(const char *str); static void cliPrintf(const char *fmt, ...); static void cliWrite(uint8_t ch); -#define COMPARE_CONFIG(value) (masterConfig.value == defaultConfig.value) static bool cliDumpPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...); +static bool cliDefaultPrintf(uint8_t dumpMask, bool equalsDefault, const char *format, ...); static void cliPrompt(void) { @@ -962,7 +963,7 @@ static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t * { int val; - for (int argIndex = 0; argIndex < 2; argIndex++) { + for (uint32_t argIndex = 0; argIndex < 2; argIndex++) { ptr = strchr(ptr, ' '); if (ptr) { val = atoi(++ptr); @@ -994,24 +995,33 @@ static void printRxFail(uint8_t dumpMask, master_t *defaultConfig) rxFailsafeChannelConfiguration_t *channelFailsafeConfigurationDefault; bool equalsDefault; bool requireValue; - char modeCharacter; - for (uint8_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { + for (uint32_t channel = 0; channel < MAX_SUPPORTED_RC_CHANNEL_COUNT; channel++) { channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[channel]; channelFailsafeConfigurationDefault = &defaultConfig->rxConfig.failsafe_channel_configurations[channel]; - equalsDefault = channelFailsafeConfiguration->mode == channelFailsafeConfigurationDefault->mode + equalsDefault = channelFailsafeConfiguration->mode == channelFailsafeConfigurationDefault->mode && channelFailsafeConfiguration->step == channelFailsafeConfigurationDefault->step; requireValue = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET; - modeCharacter = rxFailsafeModeCharacters[channelFailsafeConfiguration->mode]; if (requireValue) { - cliDumpPrintf(dumpMask, equalsDefault, "rxfail %u %c %d\r\n", + const char *format = "rxfail %u %c %d\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, format, channel, - modeCharacter, + rxFailsafeModeCharacters[channelFailsafeConfigurationDefault->mode], + RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfigurationDefault->step) + ); + cliDumpPrintf(dumpMask, equalsDefault, format, + channel, + rxFailsafeModeCharacters[channelFailsafeConfiguration->mode], RXFAIL_STEP_TO_CHANNEL_VALUE(channelFailsafeConfiguration->step) ); } else { - cliDumpPrintf(dumpMask, equalsDefault, "rxfail %u %c\r\n", + const char *format = "rxfail %u %c\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, format, channel, - modeCharacter + rxFailsafeModeCharacters[channelFailsafeConfigurationDefault->mode] + ); + cliDumpPrintf(dumpMask, equalsDefault, format, + channel, + rxFailsafeModeCharacters[channelFailsafeConfiguration->mode] ); } } @@ -1107,14 +1117,22 @@ static void printAux(uint8_t dumpMask, master_t *defaultConfig) modeActivationCondition_t *mac; modeActivationCondition_t *macDefault; bool equalsDefault; - for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { + for (uint32_t i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) { mac = &masterConfig.modeActivationConditions[i]; macDefault = &defaultConfig->modeActivationConditions[i]; equalsDefault = mac->modeId == macDefault->modeId && mac->auxChannelIndex == macDefault->auxChannelIndex && mac->range.startStep == macDefault->range.startStep && mac->range.endStep == macDefault->range.endStep; - cliDumpPrintf(dumpMask, equalsDefault, "aux %u %u %u %u %u\r\n", + const char *format = "aux %u %u %u %u %u\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, format, + i, + macDefault->modeId, + macDefault->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.endStep) + ); + cliDumpPrintf(dumpMask, equalsDefault, format, i, mac->modeId, mac->auxChannelIndex, @@ -1130,7 +1148,7 @@ static void cliAux(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - printAux(DUMP_MASTER, &masterConfig); + printAux(DUMP_MASTER, NULL); } else { ptr = cmdline; i = atoi(ptr++); @@ -1169,7 +1187,7 @@ static void printSerial(uint8_t dumpMask, master_t *defaultConfig) serialConfig_t *serialConfig; serialConfig_t *serialConfigDefault; bool equalsDefault; - for (int i = 0; i < SERIAL_PORT_COUNT; i++) { + for (uint32_t i = 0; i < SERIAL_PORT_COUNT; i++) { serialConfig = &masterConfig.serialConfig; if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) { continue; @@ -1181,7 +1199,16 @@ static void printSerial(uint8_t dumpMask, master_t *defaultConfig) && serialConfig->portConfigs[i].gps_baudrateIndex == serialConfigDefault->portConfigs[i].gps_baudrateIndex && serialConfig->portConfigs[i].telemetry_baudrateIndex == serialConfigDefault->portConfigs[i].telemetry_baudrateIndex && serialConfig->portConfigs[i].blackbox_baudrateIndex == serialConfigDefault->portConfigs[i].blackbox_baudrateIndex; - cliDumpPrintf(dumpMask, equalsDefault, "serial %d %d %ld %ld %ld %ld\r\n" , + const char *format = "serial %d %d %ld %ld %ld %ld\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, format, + serialConfigDefault->portConfigs[i].identifier, + serialConfigDefault->portConfigs[i].functionMask, + baudRates[serialConfigDefault->portConfigs[i].msp_baudrateIndex], + baudRates[serialConfigDefault->portConfigs[i].gps_baudrateIndex], + baudRates[serialConfigDefault->portConfigs[i].telemetry_baudrateIndex], + baudRates[serialConfigDefault->portConfigs[i].blackbox_baudrateIndex] + ); + cliDumpPrintf(dumpMask, equalsDefault, format, serialConfig->portConfigs[i].identifier, serialConfig->portConfigs[i].functionMask, baudRates[serialConfig->portConfigs[i].msp_baudrateIndex], @@ -1198,7 +1225,7 @@ static void cliSerial(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - printSerial(DUMP_MASTER, &masterConfig); + printSerial(DUMP_MASTER, NULL); return; } @@ -1353,7 +1380,7 @@ static void printAdjustmentRange(uint8_t dumpMask, master_t *defaultConfig) adjustmentRange_t *ar; adjustmentRange_t *arDefault; bool equalsDefault; - for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { + for (uint32_t i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) { ar = &masterConfig.adjustmentRanges[i]; arDefault = &defaultConfig->adjustmentRanges[i]; equalsDefault = ar->auxChannelIndex == arDefault->auxChannelIndex @@ -1362,7 +1389,17 @@ static void printAdjustmentRange(uint8_t dumpMask, master_t *defaultConfig) && ar->adjustmentFunction == arDefault->adjustmentFunction && ar->auxSwitchChannelIndex == arDefault->auxSwitchChannelIndex && ar->adjustmentIndex == arDefault->adjustmentIndex; - cliDumpPrintf(dumpMask, equalsDefault, "adjrange %u %u %u %u %u %u %u\r\n", + const char *format = "adjrange %u %u %u %u %u %u %u\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, format, + i, + arDefault->adjustmentIndex, + arDefault->auxChannelIndex, + MODE_STEP_TO_CHANNEL_VALUE(arDefault->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(arDefault->range.endStep), + arDefault->adjustmentFunction, + arDefault->auxSwitchChannelIndex + ); + cliDumpPrintf(dumpMask, equalsDefault, format, i, ar->adjustmentIndex, ar->auxChannelIndex, @@ -1380,7 +1417,7 @@ static void cliAdjustmentRange(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - printAdjustmentRange(DUMP_MASTER, &masterConfig); + printAdjustmentRange(DUMP_MASTER, NULL); } else { ptr = cmdline; i = atoi(ptr++); @@ -1434,41 +1471,61 @@ static void cliAdjustmentRange(char *cmdline) } } +static void printMotorMix(uint8_t dumpMask, master_t *defaultConfig) +{ + char buf0[8]; + char buf1[8]; + char buf2[8]; + char buf3[8]; + for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + if (masterConfig.customMotorMixer[i].throttle == 0.0f) + break; + float thr = masterConfig.customMotorMixer[i].throttle; + float roll = masterConfig.customMotorMixer[i].roll; + float pitch = masterConfig.customMotorMixer[i].pitch; + float yaw = masterConfig.customMotorMixer[i].yaw; + float thrDefault = defaultConfig->customMotorMixer[i].throttle; + float rollDefault = defaultConfig->customMotorMixer[i].roll; + float pitchDefault = defaultConfig->customMotorMixer[i].pitch; + float yawDefault = defaultConfig->customMotorMixer[i].yaw; + bool equalsDefault = thr == thrDefault && roll == rollDefault && pitch == pitchDefault && yaw == yawDefault; + + const char *format = "mmix %d %s %s %s %s\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, format, + i, + ftoa(thrDefault, buf0), + ftoa(rollDefault, buf1), + ftoa(pitchDefault, buf2), + ftoa(yawDefault, buf3)); + cliDumpPrintf(dumpMask, equalsDefault, format, + i, + ftoa(thr, buf0), + ftoa(roll, buf1), + ftoa(pitch, buf2), + ftoa(yaw, buf3)); + } +} + static void cliMotorMix(char *cmdline) { #ifdef USE_QUAD_MIXER_ONLY UNUSED(cmdline); #else - int i, check = 0; - int num_motors = 0; + int check = 0; uint8_t len; - char buf[16]; char *ptr; if (isEmpty(cmdline)) { -#ifndef CLI_MINIMAL_VERBOSITY - cliPrint("Motor\tThr\tRoll\tPitch\tYaw\r\n"); -#endif - for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - if (masterConfig.customMotorMixer[i].throttle == 0.0f) - break; - num_motors++; - cliPrintf("#%d:\t", i); - cliPrintf("%s\t", ftoa(masterConfig.customMotorMixer[i].throttle, buf)); - cliPrintf("%s\t", ftoa(masterConfig.customMotorMixer[i].roll, buf)); - cliPrintf("%s\t", ftoa(masterConfig.customMotorMixer[i].pitch, buf)); - cliPrintf("%s\r\n", ftoa(masterConfig.customMotorMixer[i].yaw, buf)); - } - return; + printMotorMix(DUMP_MASTER, NULL); } else if (strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer - for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) + for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) masterConfig.customMotorMixer[i].throttle = 0.0f; } else if (strncasecmp(cmdline, "load", 4) == 0) { ptr = strchr(cmdline, ' '); if (ptr) { len = strlen(++ptr); - for (i = 0; ; i++) { + for (uint32_t i = 0; ; i++) { if (mixerNames[i] == NULL) { cliPrint("Invalid name\r\n"); break; @@ -1483,7 +1540,7 @@ static void cliMotorMix(char *cmdline) } } else { ptr = cmdline; - i = atoi(ptr); // get motor number + uint32_t i = atoi(ptr); // get motor number if (i < MAX_SUPPORTED_MOTORS) { ptr = strchr(ptr, ' '); if (ptr) { @@ -1522,12 +1579,22 @@ static void printRxRange(uint8_t dumpMask, master_t *defaultConfig) rxChannelRangeConfiguration_t *channelRangeConfiguration; rxChannelRangeConfiguration_t *channelRangeConfigurationDefault; bool equalsDefault; - for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { + for (uint32_t i = 0; i < NON_AUX_CHANNEL_COUNT; i++) { channelRangeConfiguration = &masterConfig.rxConfig.channelRanges[i]; channelRangeConfigurationDefault = &defaultConfig->rxConfig.channelRanges[i]; - equalsDefault = channelRangeConfiguration->min == channelRangeConfigurationDefault->min - && channelRangeConfiguration->max == channelRangeConfigurationDefault->max; - cliDumpPrintf(dumpMask, equalsDefault, "rxrange %u %u %u\r\n", i, channelRangeConfiguration->min, channelRangeConfiguration->max); + equalsDefault = channelRangeConfiguration->min == channelRangeConfigurationDefault->min + && channelRangeConfiguration->max == channelRangeConfigurationDefault->max; + const char *format = "rxrange %u %u %u\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, format, + i, + channelRangeConfigurationDefault->min, + channelRangeConfigurationDefault->max + ); + cliDumpPrintf(dumpMask, equalsDefault, format, + i, + channelRangeConfiguration->min, + channelRangeConfiguration->max + ); } } @@ -1537,7 +1604,7 @@ static void cliRxRange(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - printRxRange(DUMP_MASTER, &masterConfig); + printRxRange(DUMP_MASTER, NULL); } else if (strcasecmp(cmdline, "reset") == 0) { resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); } else { @@ -1580,12 +1647,16 @@ static void printLed(uint8_t dumpMask, master_t *defaultConfig) ledConfig_t ledConfig; ledConfig_t ledConfigDefault; char ledConfigBuffer[20]; - for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) { + char ledConfigDefaultBuffer[20]; + for (uint32_t i = 0; i < LED_MAX_STRIP_LENGTH; i++) { ledConfig = masterConfig.ledConfigs[i]; ledConfigDefault = defaultConfig->ledConfigs[i]; equalsDefault = ledConfig == ledConfigDefault; - generateLedConfig(i, ledConfigBuffer, sizeof(ledConfigBuffer)); - cliDumpPrintf(dumpMask, equalsDefault, "led %u %s\r\n", i, ledConfigBuffer); + generateLedConfig(&ledConfig, ledConfigBuffer, sizeof(ledConfigBuffer)); + generateLedConfig(&ledConfigDefault, ledConfigDefaultBuffer, sizeof(ledConfigDefaultBuffer)); + const char *format = "led %u %s\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, format, i, ledConfigDefaultBuffer); + cliDumpPrintf(dumpMask, equalsDefault, format, i, ledConfigBuffer); } } @@ -1595,7 +1666,7 @@ static void cliLed(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - printLed(DUMP_MASTER, &masterConfig); + printLed(DUMP_MASTER, NULL); } else { ptr = cmdline; i = atoi(ptr); @@ -1615,13 +1686,20 @@ static void printColor(uint8_t dumpMask, master_t *defaultConfig) hsvColor_t *color; hsvColor_t *colorDefault; bool equalsDefault; - for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { + for (uint32_t i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) { color = &masterConfig.colors[i]; colorDefault = &defaultConfig->colors[i]; equalsDefault = color->h == colorDefault->h && color->s == colorDefault->s && color->v == colorDefault->v; - cliDumpPrintf(dumpMask, equalsDefault, "color %u %d,%u,%u\r\n", + const char *format = "color %u %d,%u,%u\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, format, + i, + colorDefault->h, + colorDefault->s, + colorDefault->v + ); + cliDumpPrintf(dumpMask, equalsDefault, format, i, color->h, color->s, @@ -1636,7 +1714,7 @@ static void cliColor(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - printColor(DUMP_MASTER, &masterConfig); + printColor(DUMP_MASTER, NULL); } else { ptr = cmdline; i = atoi(ptr); @@ -1653,25 +1731,29 @@ static void cliColor(char *cmdline) static void printModeColor(uint8_t dumpMask, master_t *defaultConfig) { - for (int i = 0; i < LED_MODE_COUNT; i++) { - for (int j = 0; j < LED_DIRECTION_COUNT; j++) { + for (uint32_t i = 0; i < LED_MODE_COUNT; i++) { + for (uint32_t j = 0; j < LED_DIRECTION_COUNT; j++) { int colorIndex = masterConfig.modeColors[i].color[j]; int colorIndexDefault = defaultConfig->modeColors[i].color[j]; - cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, "mode_color %u %u %u\r\n", i, j, colorIndex); + const char *format = "mode_color %u %u %u\r\n"; + cliDefaultPrintf(dumpMask, colorIndex == colorIndexDefault, format, i, j, colorIndexDefault); + cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, format, i, j, colorIndex); } } - for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { + for (uint32_t j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) { int colorIndex = masterConfig.specialColors.color[j]; int colorIndexDefault = defaultConfig->specialColors.color[j]; - cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, "mode_color %u %u %u\r\n", LED_SPECIAL, j, colorIndex); + const char *format = "mode_color %u %u %u\r\n"; + cliDefaultPrintf(dumpMask, colorIndex == colorIndexDefault, format, LED_SPECIAL, j, colorIndexDefault); + cliDumpPrintf(dumpMask, colorIndex == colorIndexDefault, format, LED_SPECIAL, j, colorIndex); } } static void cliModeColor(char *cmdline) { if (isEmpty(cmdline)) { - printModeColor(DUMP_MASTER, &masterConfig); + printModeColor(DUMP_MASTER, NULL); } else { enum {MODE = 0, FUNCTION, COLOR, ARGS_COUNT}; int args[ARGS_COUNT]; @@ -1704,21 +1786,27 @@ static void cliModeColor(char *cmdline) static void printServo(uint8_t dumpMask, master_t *defaultConfig) { // print out servo settings - int i; - servoParam_t *servoConf; - servoParam_t *servoConfDefault; - bool equalsDefault; - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servoConf = &masterConfig.servoConf[i]; - servoConfDefault = &defaultConfig->servoConf[i]; - equalsDefault = servoConf->min == servoConfDefault->min + for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servoParam_t *servoConf = &masterConfig.servoConf[i]; + servoParam_t *servoConfDefault = &defaultConfig->servoConf[i]; + bool equalsDefault = servoConf->min == servoConfDefault->min && servoConf->max == servoConfDefault->max && servoConf->middle == servoConfDefault->middle && servoConf->angleAtMin == servoConfDefault->angleAtMax && servoConf->rate == servoConfDefault->rate && servoConf->forwardFromChannel == servoConfDefault->forwardFromChannel; - - cliDumpPrintf(dumpMask, equalsDefault, "servo %u %d %d %d %d %d %d %d\r\n", + const char *format = "servo %u %d %d %d %d %d %d %d\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, format, + i, + servoConfDefault->min, + servoConfDefault->max, + servoConfDefault->middle, + servoConfDefault->angleAtMin, + servoConfDefault->angleAtMax, + servoConfDefault->rate, + servoConfDefault->forwardFromChannel + ); + cliDumpPrintf(dumpMask, equalsDefault, format, i, servoConf->min, servoConf->max, @@ -1729,20 +1817,6 @@ static void printServo(uint8_t dumpMask, master_t *defaultConfig) servoConf->forwardFromChannel ); } - - // print servo directions - unsigned int channel; - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servoConf = &masterConfig.servoConf[i]; - servoConfDefault = &defaultConfig->servoConf[i]; - equalsDefault = servoConf->reversedSources == servoConfDefault->reversedSources; - for (channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { - if (servoConf->reversedSources & (1 << channel)) { - equalsDefault = ~(servoConf->reversedSources ^ servoConfDefault->reversedSources) & (1 << channel); - cliDumpPrintf(dumpMask, equalsDefault, "smix reverse %d %d r\r\n", i , channel); - } - } - } } static void cliServo(char *cmdline) @@ -1756,7 +1830,7 @@ static void cliServo(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - printServo(DUMP_MASTER, &masterConfig); + printServo(DUMP_MASTER, NULL); } else { int validArgumentCount = 0; @@ -1823,48 +1897,86 @@ static void cliServo(char *cmdline) #endif #ifdef USE_SERVOS +static void printServoMix(uint8_t dumpMask, master_t *defaultConfig) +{ + for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) { + servoMixer_t customServoMixer = masterConfig.customServoMixer[i]; + servoMixer_t customServoMixerDefault = defaultConfig->customServoMixer[i]; + if (customServoMixer.rate == 0) { + break; + } + + bool equalsDefault = customServoMixer.targetChannel == customServoMixerDefault.targetChannel + && customServoMixer.inputSource == customServoMixerDefault.inputSource + && customServoMixer.rate == customServoMixerDefault.rate + && customServoMixer.speed == customServoMixerDefault.speed + && customServoMixer.min == customServoMixerDefault.min + && customServoMixer.max == customServoMixerDefault.max + && customServoMixer.box == customServoMixerDefault.box; + + const char *format = "smix %d %d %d %d %d %d %d %d\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, format, + i, + customServoMixerDefault.targetChannel, + customServoMixerDefault.inputSource, + customServoMixerDefault.rate, + customServoMixerDefault.speed, + customServoMixerDefault.min, + customServoMixerDefault.max, + customServoMixerDefault.box + ); + cliDumpPrintf(dumpMask, equalsDefault, format, + i, + customServoMixer.targetChannel, + customServoMixer.inputSource, + customServoMixer.rate, + customServoMixer.speed, + customServoMixer.min, + customServoMixer.max, + customServoMixer.box + ); + } + + cliPrint("\r\n"); + + // print servo directions + for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servoParam_t *servoConf = &masterConfig.servoConf[i]; + servoParam_t *servoConfDefault = &defaultConfig->servoConf[i]; + bool equalsDefault = servoConf->reversedSources == servoConfDefault->reversedSources; + for (uint32_t channel = 0; channel < INPUT_SOURCE_COUNT; channel++) { + equalsDefault = ~(servoConf->reversedSources ^ servoConfDefault->reversedSources) & (1 << channel); + const char *format = "smix reverse %d %d r\r\n"; + if (servoConfDefault->reversedSources & (1 << channel)) { + cliDefaultPrintf(dumpMask, equalsDefault, format, i , channel); + } + if (servoConf->reversedSources & (1 << channel)) { + cliDumpPrintf(dumpMask, equalsDefault, format, i , channel); + } + } + } +} + static void cliServoMix(char *cmdline) { - int i; uint8_t len; char *ptr; int args[8], check = 0; len = strlen(cmdline); if (len == 0) { - -#ifndef CLI_MINIMAL_VERBOSITY - cliPrint("Rule\tServo\tSource\tRate\tSpeed\tMin\tMax\tBox\r\n"); -#endif - - for (i = 0; i < MAX_SERVO_RULES; i++) { - if (masterConfig.customServoMixer[i].rate == 0) - break; - - cliPrintf("#%d:\t%d\t%d\t%d\t%d\t%d\t%d\t%d\r\n", - i, - masterConfig.customServoMixer[i].targetChannel, - masterConfig.customServoMixer[i].inputSource, - masterConfig.customServoMixer[i].rate, - masterConfig.customServoMixer[i].speed, - masterConfig.customServoMixer[i].min, - masterConfig.customServoMixer[i].max, - masterConfig.customServoMixer[i].box - ); - } - cliPrintf("\r\n"); - return; + printServoMix(DUMP_MASTER, NULL); } else if (strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer memset(masterConfig.customServoMixer, 0, sizeof(masterConfig.customServoMixer)); - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { masterConfig.servoConf[i].reversedSources = 0; } } else if (strncasecmp(cmdline, "load", 4) == 0) { ptr = strchr(cmdline, ' '); if (ptr) { len = strlen(++ptr); - for (i = 0; ; i++) { + for (uint32_t i = 0; ; i++) { if (mixerNames[i] == NULL) { cliPrintf("Invalid name\r\n"); break; @@ -1879,19 +1991,18 @@ static void cliServoMix(char *cmdline) } } else if (strncasecmp(cmdline, "reverse", 7) == 0) { enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT}; - int servoIndex, inputSource; ptr = strchr(cmdline, ' '); len = strlen(ptr); if (len == 0) { cliPrintf("s"); - for (inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) + for (uint32_t inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) cliPrintf("\ti%d", inputSource); cliPrintf("\r\n"); - for (servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { + for (uint32_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { cliPrintf("%d", servoIndex); - for (inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) + for (uint32_t inputSource = 0; inputSource < INPUT_SOURCE_COUNT; inputSource++) cliPrintf("\t%s ", (masterConfig.servoConf[servoIndex].reversedSources & (1 << inputSource)) ? "r" : "n"); cliPrintf("\r\n"); } @@ -1933,7 +2044,7 @@ static void cliServoMix(char *cmdline) return; } - i = args[RULE]; + int32_t i = args[RULE]; if (i >= 0 && i < MAX_SERVO_RULES && args[TARGET] >= 0 && args[TARGET] < MAX_SUPPORTED_SERVOS && args[INPUT] >= 0 && args[INPUT] < INPUT_SOURCE_COUNT && @@ -2076,7 +2187,6 @@ static void cliFlashRead(char *cmdline) { uint32_t address = atoi(cmdline); uint32_t length; - int i; uint8_t buffer[32]; @@ -2094,7 +2204,7 @@ static void cliFlashRead(char *cmdline) bytesRead = flashfsReadAbs(address, buffer, length < sizeof(buffer) ? length : sizeof(buffer)); - for (i = 0; i < bytesRead; i++) { + for (uint32_t i = 0; i < bytesRead; i++) { cliWrite(buffer[i]); } @@ -2120,7 +2230,7 @@ static void printVtx(uint8_t dumpMask, master_t *defaultConfig) vtxChannelActivationCondition_t *cac; vtxChannelActivationCondition_t *cacDefault; bool equalsDefault; - for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { + for (uint32_t i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; i++) { cac = &masterConfig.vtxChannelActivationConditions[i]; cacDefault = &defaultConfig->vtxChannelActivationConditions[i]; equalsDefault = cac->auxChannelIndex == cacDefault->auxChannelIndex @@ -2128,7 +2238,16 @@ static void printVtx(uint8_t dumpMask, master_t *defaultConfig) && cac->channel == cacDefault->channel && cac->range.startStep == cacDefault->range.startStep && cac->range.endStep == cacDefault->range.endStep; - cliDumpPrintf(dumpMask, equalsDefault, "vtx %u %u %u %u %u %u\r\n", + const char *format = "vtx %u %u %u %u %u %u\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, format, + i, + cacDefault->auxChannelIndex, + cacDefault->band, + cacDefault->channel, + MODE_STEP_TO_CHANNEL_VALUE(cacDefault->range.startStep), + MODE_STEP_TO_CHANNEL_VALUE(cacDefault->range.endStep) + ); + cliDumpPrintf(dumpMask, equalsDefault, format, i, cac->auxChannelIndex, cac->band, @@ -2145,7 +2264,7 @@ static void cliVtx(char *cmdline) char *ptr; if (isEmpty(cmdline)) { - printVtx(DUMP_MASTER, &masterConfig); + printVtx(DUMP_MASTER, NULL); } else { ptr = cmdline; i = atoi(ptr++); @@ -2190,7 +2309,8 @@ static void cliVtx(char *cmdline) static void printName(uint8_t dumpMask) { - cliDumpPrintf(dumpMask, strlen(masterConfig.name) == 0, "name %s\r\n", strlen(masterConfig.name) > 0 ? masterConfig.name : "-"); + bool equalsDefault = strlen(masterConfig.name) == 0; + cliDumpPrintf(dumpMask, equalsDefault, "name %s\r\n", equalsDefault ? emptyName : masterConfig.name); } static void cliName(char *cmdline) @@ -2198,11 +2318,233 @@ static void cliName(char *cmdline) uint32_t len = strlen(cmdline); if (len > 0) { memset(masterConfig.name, 0, ARRAYLEN(masterConfig.name)); - strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); + if (strncmp(cmdline, emptyName, len)) { + strncpy(masterConfig.name, cmdline, MIN(len, MAX_NAME_LENGTH)); + } } printName(DUMP_MASTER); } +static void printFeature(uint8_t dumpMask, master_t *defaultConfig) +{ + uint32_t mask = featureMask(); + uint32_t defaultMask = defaultConfig->enabledFeatures; + for (uint32_t i = 0; ; i++) { // disable all feature first + if (featureNames[i] == NULL) + break; + const char *format = "feature -%s\r\n"; + cliDefaultPrintf(dumpMask, (defaultMask | ~mask) & (1 << i), format, featureNames[i]); + cliDumpPrintf(dumpMask, (~defaultMask | mask) & (1 << i), format, featureNames[i]); + } + for (uint32_t i = 0; ; i++) { // reenable what we want. + if (featureNames[i] == NULL) + break; + const char *format = "feature %s\r\n"; + if (defaultMask & (1 << i)) { + cliDefaultPrintf(dumpMask, (~defaultMask | mask) & (1 << i), format, featureNames[i]); + } + if (mask & (1 << i)) { + cliDumpPrintf(dumpMask, (defaultMask | ~mask) & (1 << i), format, featureNames[i]); + } + } +} + +static void cliFeature(char *cmdline) +{ + uint32_t len = strlen(cmdline); + uint32_t mask = featureMask(); + + if (len == 0) { + cliPrint("Enabled: "); + for (uint32_t i = 0; ; i++) { + if (featureNames[i] == NULL) + break; + if (mask & (1 << i)) + cliPrintf("%s ", featureNames[i]); + } + cliPrint("\r\n"); + } else if (strncasecmp(cmdline, "list", len) == 0) { + cliPrint("Available: "); + for (uint32_t i = 0; ; i++) { + if (featureNames[i] == NULL) + break; + cliPrintf("%s ", featureNames[i]); + } + cliPrint("\r\n"); + return; + } else { + bool remove = false; + if (cmdline[0] == '-') { + // remove feature + remove = true; + cmdline++; // skip over - + len--; + } + + for (uint32_t i = 0; ; i++) { + if (featureNames[i] == NULL) { + cliPrint("Invalid name\r\n"); + break; + } + + if (strncasecmp(cmdline, featureNames[i], len) == 0) { + + mask = 1 << i; +#ifndef GPS + if (mask & FEATURE_GPS) { + cliPrint("unavailable\r\n"); + break; + } +#endif +#ifndef SONAR + if (mask & FEATURE_SONAR) { + cliPrint("unavailable\r\n"); + break; + } +#endif + if (remove) { + featureClear(mask); + cliPrint("Disabled"); + } else { + featureSet(mask); + cliPrint("Enabled"); + } + cliPrintf(" %s\r\n", featureNames[i]); + break; + } + } + } +} + +#ifdef BEEPER +static void printBeeper(uint8_t dumpMask, master_t *defaultConfig) +{ + uint8_t beeperCount = beeperTableEntryCount(); + uint32_t mask = getBeeperOffMask(); + uint32_t defaultMask = defaultConfig->beeper_off_flags; + for (int32_t i = 0; i < beeperCount - 2; i++) { + const char *formatOff = "beeper -%s\r\n"; + const char *formatOn = "beeper %s\r\n"; + cliDefaultPrintf(dumpMask, ~(mask ^ defaultMask) & (1 << i), mask & (1 << i) ? formatOn : formatOff, beeperNameForTableIndex(i)); + cliDumpPrintf(dumpMask, ~(mask ^ defaultMask) & (1 << i), mask & (1 << i) ? formatOff : formatOn, beeperNameForTableIndex(i)); + } +} + +static void cliBeeper(char *cmdline) +{ + uint32_t len = strlen(cmdline); + uint8_t beeperCount = beeperTableEntryCount(); + uint32_t mask = getBeeperOffMask(); + + if (len == 0) { + cliPrintf("Disabled:"); + for (int32_t i = 0; ; i++) { + if (i == beeperCount - 2){ + if (mask == 0) + cliPrint(" none"); + break; + } + if (mask & (1 << i)) + cliPrintf(" %s", beeperNameForTableIndex(i)); + } + cliPrint("\r\n"); + } else if (strncasecmp(cmdline, "list", len) == 0) { + cliPrint("Available:"); + for (uint32_t i = 0; i < beeperCount; i++) + cliPrintf(" %s", beeperNameForTableIndex(i)); + cliPrint("\r\n"); + return; + } else { + bool remove = false; + if (cmdline[0] == '-') { + remove = true; // this is for beeper OFF condition + cmdline++; + len--; + } + + for (uint32_t i = 0; ; i++) { + if (i == beeperCount) { + cliPrint("Invalid name\r\n"); + break; + } + if (strncasecmp(cmdline, beeperNameForTableIndex(i), len) == 0) { + if (remove) { // beeper off + if (i == BEEPER_ALL-1) + beeperOffSetAll(beeperCount-2); + else + if (i == BEEPER_PREFERENCE-1) + setBeeperOffMask(getPreferredBeeperOffMask()); + else { + mask = 1 << i; + beeperOffSet(mask); + } + cliPrint("Disabled"); + } + else { // beeper on + if (i == BEEPER_ALL-1) + beeperOffClearAll(); + else + if (i == BEEPER_PREFERENCE-1) + setPreferredBeeperOffMask(getBeeperOffMask()); + else { + mask = 1 << i; + beeperOffClear(mask); + } + cliPrint("Enabled"); + } + cliPrintf(" %s\r\n", beeperNameForTableIndex(i)); + break; + } + } + } +} +#endif + +static void printMap(uint8_t dumpMask, master_t *defaultConfig) +{ + bool equalsDefault = true; + char buf[16]; + char bufDefault[16]; + uint32_t i; + for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { + buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; + bufDefault[defaultConfig->rxConfig.rcmap[i]] = rcChannelLetters[i]; + equalsDefault = equalsDefault && (masterConfig.rxConfig.rcmap[i] == defaultConfig->rxConfig.rcmap[i]); + } + buf[i] = '\0'; + + const char *formatMap = "map %s\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, formatMap, bufDefault); + cliDumpPrintf(dumpMask, equalsDefault, formatMap, buf); +} + +static void cliMap(char *cmdline) +{ + uint32_t len; + char out[9]; + + len = strlen(cmdline); + + if (len == 8) { + // uppercase it + for (uint32_t i = 0; i < 8; i++) + cmdline[i] = toupper((unsigned char)cmdline[i]); + for (uint32_t i = 0; i < 8; i++) { + if (strchr(rcChannelLetters, cmdline[i]) && !strchr(cmdline + i + 1, cmdline[i])) + continue; + cliShowParseError(); + return; + } + parseRcChannels(cmdline, &masterConfig.rxConfig); + } + cliPrint("Map: "); + uint32_t i; + for (i = 0; i < 8; i++) + out[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; + out[i] = '\0'; + cliPrintf("%s\r\n", out); +} + void *getValuePointer(const clivalue_t *value) { void *ptr = value->ptr; @@ -2218,11 +2560,16 @@ void *getValuePointer(const clivalue_t *value) return ptr; } +static void *getDefaultPointer(void *valuePointer, master_t *defaultConfig) +{ + return ((uint8_t *)valuePointer) - (uint32_t)&masterConfig + (uint32_t)defaultConfig; +} + static bool valueEqualsDefault(const clivalue_t *value, master_t *defaultConfig) { void *ptr = getValuePointer(value); - void *ptrDefault = ((uint8_t *)ptr) - (uint32_t)&masterConfig + (uint32_t)defaultConfig; + void *ptrDefault = getDefaultPointer(ptr, defaultConfig); bool result = false; switch (value->type & VALUE_TYPE_MASK) { @@ -2255,16 +2602,20 @@ static bool valueEqualsDefault(const clivalue_t *value, master_t *defaultConfig) static void dumpValues(uint16_t valueSection, uint8_t dumpMask, master_t *defaultConfig) { - uint32_t i; const clivalue_t *value; - for (i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < VALUE_COUNT; i++) { value = &valueTable[i]; if ((value->type & VALUE_SECTION_MASK) != valueSection) { continue; } - if (cliDumpPrintf(dumpMask, valueEqualsDefault(value, defaultConfig), "set %s = ", valueTable[i].name)) { + const char *format = "set %s = "; + if (cliDefaultPrintf(dumpMask, valueEqualsDefault(value, defaultConfig), format, valueTable[i].name)) { + cliPrintVarDefault(value, 0, defaultConfig); + cliPrint("\r\n"); + } + if (cliDumpPrintf(dumpMask, valueEqualsDefault(value, defaultConfig), format, valueTable[i].name)) { cliPrintVar(value, 0); cliPrint("\r\n"); } @@ -2293,12 +2644,6 @@ char *checkCommand(char *cmdLine, const char *command) static void printConfig(char *cmdline, bool doDiff) { - unsigned int i; - char buf[16]; - uint32_t mask; - uint32_t defaultMask; - bool equalsDefault; - uint8_t dumpMask = DUMP_MASTER; char *options; if ((options = checkCommand(cmdline, "master"))) { @@ -2316,11 +2661,12 @@ static void printConfig(char *cmdline, bool doDiff) static master_t defaultConfig; if (doDiff) { dumpMask = dumpMask | DO_DIFF; - createDefaultConfig(&defaultConfig); + } - if (checkCommand(cmdline, "commented")) { - dumpMask = dumpMask | DIFF_COMMENTED; // add unchanged values as comments for diff - } + createDefaultConfig(&defaultConfig); + + if (checkCommand(options, "showdefaults")) { + dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values } if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { @@ -2342,113 +2688,43 @@ static void printConfig(char *cmdline, bool doDiff) #ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# mixer\r\n"); #endif - cliDumpPrintf(dumpMask, COMPARE_CONFIG(mixerMode), "mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]); + bool equalsDefault = masterConfig.mixerMode == defaultConfig.mixerMode; + const char *formatMixer = "mixer %s\r\n"; + cliDefaultPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[defaultConfig.mixerMode - 1]); + cliDumpPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[masterConfig.mixerMode - 1]); - cliDumpPrintf(dumpMask, masterConfig.customMotorMixer[0].throttle == 0.0f, "mmix reset\r\n"); + cliDumpPrintf(dumpMask, masterConfig.customMotorMixer[0].throttle == 0.0f, "\r\nmmix reset\r\n\r\n"); - float thr, roll, pitch, yaw, thrDefault, rollDefault, pitchDefault, yawDefault; - for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - if (masterConfig.customMotorMixer[i].throttle == 0.0f) - break; - thr = masterConfig.customMotorMixer[i].throttle; - roll = masterConfig.customMotorMixer[i].roll; - pitch = masterConfig.customMotorMixer[i].pitch; - yaw = masterConfig.customMotorMixer[i].yaw; - thrDefault = defaultConfig.customMotorMixer[i].throttle; - rollDefault = defaultConfig.customMotorMixer[i].roll; - pitchDefault = defaultConfig.customMotorMixer[i].pitch; - yawDefault = defaultConfig.customMotorMixer[i].yaw; - equalsDefault = thr == thrDefault && roll == rollDefault && pitch == pitchDefault && yaw == yawDefault; - - if (cliDumpPrintf(dumpMask, equalsDefault, "mmix %d", i)) { - if (thr < 0) - cliWrite(' '); - cliPrintf("%s", ftoa(thr, buf)); - if (roll < 0) - cliWrite(' '); - cliPrintf("%s", ftoa(roll, buf)); - if (pitch < 0) - cliWrite(' '); - cliPrintf("%s", ftoa(pitch, buf)); - if (yaw < 0) - cliWrite(' '); - cliPrintf("%s\r\n", ftoa(yaw, buf)); - } - } + printMotorMix(dumpMask, &defaultConfig); #ifdef USE_SERVOS + cliPrint("\r\n# servo\r\n"); + printServo(dumpMask, &defaultConfig); + + cliPrint("\r\n# servo mix\r\n"); // print custom servo mixer if exists - cliDumpPrintf(dumpMask, masterConfig.customServoMixer[i].rate == 0, "smix reset\r\n"); + cliDumpPrintf(dumpMask, masterConfig.customServoMixer[0].rate == 0, "smix reset\r\n\r\n"); + printServoMix(dumpMask, &defaultConfig); - for (i = 0; i < MAX_SERVO_RULES; i++) { - if (masterConfig.customServoMixer[i].rate == 0) - break; - - equalsDefault = masterConfig.customServoMixer[i].targetChannel == defaultConfig.customServoMixer[i].targetChannel - && masterConfig.customServoMixer[i].inputSource == defaultConfig.customServoMixer[i].inputSource - && masterConfig.customServoMixer[i].rate == defaultConfig.customServoMixer[i].rate - && masterConfig.customServoMixer[i].speed == defaultConfig.customServoMixer[i].speed - && masterConfig.customServoMixer[i].min == defaultConfig.customServoMixer[i].min - && masterConfig.customServoMixer[i].max == defaultConfig.customServoMixer[i].max - && masterConfig.customServoMixer[i].box == masterConfig.customServoMixer[i].box; - - cliDumpPrintf(dumpMask, equalsDefault,"smix %d %d %d %d %d %d %d %d\r\n", - i, - masterConfig.customServoMixer[i].targetChannel, - masterConfig.customServoMixer[i].inputSource, - masterConfig.customServoMixer[i].rate, - masterConfig.customServoMixer[i].speed, - masterConfig.customServoMixer[i].min, - masterConfig.customServoMixer[i].max, - masterConfig.customServoMixer[i].box - ); - } #endif #endif #ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# feature\r\n"); #endif - - mask = featureMask(); - defaultMask = defaultConfig.enabledFeatures; - for (i = 0; ; i++) { // disable all feature first - if (featureNames[i] == NULL) - break; - cliDumpPrintf(dumpMask, (mask | ~defaultMask) & (1 << i), "feature -%s\r\n", featureNames[i]); - } - for (i = 0; ; i++) { // reenable what we want. - if (featureNames[i] == NULL) - break; - if (mask & (1 << i)) - cliDumpPrintf(dumpMask, defaultMask & (1 << i), "feature %s\r\n", featureNames[i]); - } + printFeature(dumpMask, &defaultConfig); #ifdef BEEPER #ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# beeper\r\n"); #endif - uint8_t beeperCount = beeperTableEntryCount(); - mask = getBeeperOffMask(); - defaultMask = defaultConfig.beeper_off_flags; - for (int i = 0; i < (beeperCount-2); i++) { - if (mask & (1 << i)) - cliDumpPrintf(dumpMask, (~(mask ^ defaultMask) & (1 << i)), "beeper -%s\r\n", beeperNameForTableIndex(i)); - else - cliDumpPrintf(dumpMask, (~(mask ^ defaultMask) & (1 << i)), "beeper %s\r\n", beeperNameForTableIndex(i)); - } + printBeeper(dumpMask, &defaultConfig); #endif #ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# map\r\n"); #endif - equalsDefault = true; - for (i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) { - buf[masterConfig.rxConfig.rcmap[i]] = rcChannelLetters[i]; - equalsDefault = equalsDefault && (masterConfig.rxConfig.rcmap[i] == defaultConfig.rxConfig.rcmap[i]); - } - buf[i] = '\0'; - cliDumpPrintf(dumpMask, equalsDefault, "map %s\r\n", buf); + printMap(dumpMask, &defaultConfig); #ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# serial\r\n"); @@ -2513,13 +2789,11 @@ static void printConfig(char *cmdline, bool doDiff) if (dumpMask & DUMP_ALL) { uint8_t activeProfile = masterConfig.current_profile_index; - uint8_t profileCount; - for (profileCount=0; profileCountactiveRateProfile; - uint8_t rateCount; - for (rateCount=0; rateCounttype & VALUE_TYPE_MASK) { case VAR_UINT8: - value = *(uint8_t *)ptr; + value = *(uint8_t *)valuePointer; break; case VAR_INT8: - value = *(int8_t *)ptr; + value = *(int8_t *)valuePointer; break; case VAR_UINT16: - value = *(uint16_t *)ptr; + value = *(uint16_t *)valuePointer; break; case VAR_INT16: - value = *(int16_t *)ptr; + value = *(int16_t *)valuePointer; break; case VAR_UINT32: - value = *(uint32_t *)ptr; + value = *(uint32_t *)valuePointer; break; case VAR_FLOAT: - cliPrintf("%s", ftoa(*(float *)ptr, buf)); + cliPrintf("%s", ftoa(*(float *)valuePointer, buf)); if (full && (var->type & VALUE_MODE_MASK) == MODE_DIRECT) { cliPrintf(" %s", ftoa((float)var->config.minmax.min, buf)); cliPrintf(" %s", ftoa((float)var->config.minmax.max, buf)); @@ -3106,6 +3218,22 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full) } } +static void cliPrintVar(const clivalue_t *var, uint32_t full) +{ + void *ptr = getValuePointer(var); + + printValuePointer(var, ptr, full); +} + +static void cliPrintVarDefault(const clivalue_t *var, uint32_t full, master_t *defaultConfig) +{ + void *ptr = getValuePointer(var); + + void *defaultPtr = getDefaultPointer(ptr, defaultConfig); + + printValuePointer(var, defaultPtr, full); +} + static void cliPrintVarRange(const clivalue_t *var) { switch (var->type & VALUE_MODE_MASK) { @@ -3116,8 +3244,7 @@ static void cliPrintVarRange(const clivalue_t *var) case (MODE_LOOKUP): { const lookupTableEntry_t *tableEntry = &lookupTables[var->config.lookup.tableIndex]; cliPrint("Allowed values:"); - uint8_t i; - for (i = 0; i < tableEntry->valueCount ; i++) { + for (uint32_t i = 0; i < tableEntry->valueCount ; i++) { if (i > 0) cliPrint(","); cliPrintf(" %s", tableEntry->values[i]); @@ -3161,7 +3288,6 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value) static void cliSet(char *cmdline) { - uint32_t i; uint32_t len; const clivalue_t *val; char *eqptr = NULL; @@ -3170,7 +3296,7 @@ static void cliSet(char *cmdline) if (len == 0 || (len == 1 && cmdline[0] == '*')) { cliPrint("Current settings: \r\n"); - for (i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < VALUE_COUNT; i++) { val = &valueTable[i]; cliPrintf("%s = ", valueTable[i].name); cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui @@ -3191,7 +3317,7 @@ static void cliSet(char *cmdline) eqptr++; } - for (i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < VALUE_COUNT; i++) { val = &valueTable[i]; // ensure exact match when setting to prevent setting variables with shorter names if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) { @@ -3220,7 +3346,7 @@ static void cliSet(char *cmdline) case MODE_LOOKUP: { const lookupTableEntry_t *tableEntry = &lookupTables[valueTable[i].config.lookup.tableIndex]; bool matched = false; - for (uint8_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) { + for (uint32_t tableValueIndex = 0; tableValueIndex < tableEntry->valueCount && !matched; tableValueIndex++) { matched = strcasecmp(tableEntry->values[tableValueIndex], eqptr) == 0; if (matched) { @@ -3254,11 +3380,10 @@ static void cliSet(char *cmdline) static void cliGet(char *cmdline) { - uint32_t i; const clivalue_t *val; int matchedCommands = 0; - for (i = 0; i < VALUE_COUNT; i++) { + for (uint32_t i = 0; i < VALUE_COUNT; i++) { if (strstr(valueTable[i].name, cmdline)) { val = &valueTable[i]; cliPrintf("%s = ", valueTable[i].name); @@ -3294,11 +3419,10 @@ static void cliStatus(char *cmdline) cliPrintf("CPU Clock=%dMHz", (SystemCoreClock / 1000000)); #if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) - uint8_t i; uint32_t mask; uint32_t detectedSensorsMask = sensorsMask(); - for (i = 0; ; i++) { + for (uint32_t i = 0; ; i++) { if (sensorTypeNames[i] == NULL) break; @@ -3507,7 +3631,7 @@ static void cliResource(char *cmdline) { UNUSED(cmdline); cliPrintf("IO:\r\n----------------------\r\n"); - for (unsigned i = 0; i < DEFIO_IO_USED_COUNT; i++) { + for (uint32_t i = 0; i < DEFIO_IO_USED_COUNT; i++) { const char* owner; owner = ownerNames[ioRecs[i].owner]; From bd04f4d22b00e8dad8b2d34b6ac2e8010c5f05cf Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 18 Aug 2016 01:19:30 +0200 Subject: [PATCH 347/456] coding style // refactor --- src/main/blackbox/blackbox.c | 22 ++++++++++------------ src/main/io/serial_cli.c | 1 - 2 files changed, 10 insertions(+), 13 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index d829f9babb..b9d60e1c10 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -930,15 +930,15 @@ void stopInTestMode(void) */ bool inMotorTestMode(void) { static uint32_t resetTime = 0; - uint16_t activeMinimumCommand = (feature(FEATURE_3D) ? masterConfig.flight3DConfig.neutral3d : masterConfig.escAndServoConfig.mincommand); + uint16_t inactiveMotorCommand = (feature(FEATURE_3D) ? masterConfig.flight3DConfig.neutral3d : masterConfig.escAndServoConfig.mincommand); int i; - bool motorsNotAtMin = false; + bool atLeastOneMotorActivated = false; // set disarmed motor values - for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) - motorsNotAtMin |= (motor_disarmed[i] != activeMinimumCommand); + for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) + atLeastOneMotorActivated |= (motor_disarmed[i] != inactiveMotorCommand); - if(motorsNotAtMin) { + if(atLeastOneMotorActivated) { resetTime = millis() + 5000; // add 5 seconds return true; } else { @@ -1625,14 +1625,12 @@ void handleBlackbox(void) if(masterConfig.blackbox_on_motor_test) { // Handle Motor Test Mode if(inMotorTestMode()) { - if(blackboxState==BLACKBOX_STATE_STOPPED) { + if(blackboxState==BLACKBOX_STATE_STOPPED) startInTestMode(); - } - } else { - if(blackboxState!=BLACKBOX_STATE_STOPPED) { - stopInTestMode(); - } - } + } else { + if(blackboxState!=BLACKBOX_STATE_STOPPED) + stopInTestMode(); + } } } } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 98c92a69ca..e463b26398 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -886,7 +886,6 @@ const clivalue_t valueTable[] = { { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 } }, { "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_device, .config.lookup = { TABLE_BLACKBOX_DEVICE } }, { "blackbox_on_motor_test", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_on_motor_test, .config.lookup = { TABLE_OFF_ON } }, - #endif #ifdef VTX From 926bc982d6f83440170825e60b05478c6cfe21ef Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 18 Aug 2016 01:24:46 +0200 Subject: [PATCH 348/456] Change some commands --- src/main/config/config.c | 2 +- src/main/io/serial_cli.c | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 87aa098e1c..52f705d385 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 143; +static const uint8_t EEPROM_CONF_VERSION = 144; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e463b26398..6dfd2f7069 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -849,11 +849,11 @@ const clivalue_t valueTable[] = { { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 200 } }, - { "yaw_rate_acceleration_limit",VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, - { "rate_acceleration_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, + { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, + { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, - { "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, - { "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, + { "accumulation_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, + { "yaw_accumulation_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, From 847e2cd605e170b74f2e61d491668a1c18be3027 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 18 Aug 2016 01:36:32 +0200 Subject: [PATCH 349/456] Remove Mag for CC3D --- src/main/target/CC3D/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index cfcc9b6831..4cf679e1e8 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -109,12 +109,12 @@ #undef GPS #define CLI_MINIMAL_VERBOSITY +#undef MAG #ifdef CC3D_OPBL #define SKIP_CLI_COMMAND_HELP #define SKIP_PID_FLOAT #undef BARO -#undef MAG #undef SONAR #undef USE_SOFTSERIAL1 #undef LED_STRIP From c728aba5b0d781f2b603c5c36533ab36aa4dc45b Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 18 Aug 2016 11:50:35 +1200 Subject: [PATCH 350/456] Testing gcc 5.4 for travis builds. --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 851cb0ce1e..1e4ab7c56d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -67,10 +67,10 @@ language: cpp compiler: clang before_install: - - curl --retry 10 --retry-max-time 120 -L "https://launchpad.net/gcc-arm-embedded/4.9/4.9-2015-q2-update/+download/gcc-arm-none-eabi-4_9-2015q2-20150609-linux.tar.bz2" | tar xfj - + - curl --retry 10 --retry-max-time 120 -L "https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q2-update/+download/gcc-arm-none-eabi-5_4-2016q2-20160622-linux.tar.bz2" | tar xfj - install: - - export PATH=$PATH:$PWD/gcc-arm-none-eabi-4_9-2015q2/bin + - export PATH=$PATH:$PWD/gcc-arm-none-eabi-4-2016q2/bin before_script: arm-none-eabi-gcc --version script: ./.travis.sh From 1146e72ceac563168caaf133eaa3f40f4e8370e0 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 18 Aug 2016 12:12:33 +1200 Subject: [PATCH 351/456] Update .travis.yml Fix copy / pasta. --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 1e4ab7c56d..bee6f8274f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -70,7 +70,7 @@ before_install: - curl --retry 10 --retry-max-time 120 -L "https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q2-update/+download/gcc-arm-none-eabi-5_4-2016q2-20160622-linux.tar.bz2" | tar xfj - install: - - export PATH=$PATH:$PWD/gcc-arm-none-eabi-4-2016q2/bin + - export PATH=$PATH:$PWD/gcc-arm-none-eabi-5_4-2016q2/bin before_script: arm-none-eabi-gcc --version script: ./.travis.sh From de9f57d01606891bc8bec69de52cc058a9a425ad Mon Sep 17 00:00:00 2001 From: mikeller Date: Thu, 18 Aug 2016 21:58:07 +1200 Subject: [PATCH 352/456] Removed doubled up servo output in CLI 'diff'/'dump'. --- src/main/io/serial_cli.c | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 59da425909..9b333b35e4 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -2764,13 +2764,6 @@ static void printConfig(char *cmdline, bool doDiff) #endif printRxRange(dumpMask, &defaultConfig); -#ifdef USE_SERVOS -#ifndef CLI_MINIMAL_VERBOSITY - cliPrint("\r\n# servo\r\n"); -#endif - printServo(dumpMask, &defaultConfig); -#endif - #ifdef VTX #ifndef CLI_MINIMAL_VERBOSITY cliPrint("\r\n# vtx\r\n"); From f823a815406d2d931363bc5a7d38e197a91b1ff8 Mon Sep 17 00:00:00 2001 From: blckmn Date: Fri, 19 Aug 2016 22:51:59 +1000 Subject: [PATCH 353/456] Small PPM fixes and updates to REVO --- src/main/drivers/pwm_mapping.c | 20 +++++++++++--------- src/main/target/REVO/target.c | 11 ++++++----- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 1466aa2d25..bc9336cbee 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -109,7 +109,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) #else setup = hardwareMaps[i]; #endif - + TIM_TypeDef* ppmTimer = NULL; for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT && setup[i] != 0xFFFF; i++) { uint8_t timerIndex = setup[i] & 0x00FF; uint8_t type = (setup[i] & 0xFF00) >> 8; @@ -162,13 +162,13 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } #endif -#ifdef RSSI_ADC_GPIO +#ifdef RSSI_ADC_PIN if (init->useRSSIADC && timerHardwarePtr->tag == IO_TAG(RSSI_ADC_PIN)) { continue; } #endif -#ifdef CURRENT_METER_ADC_GPIO +#ifdef CURRENT_METER_ADC_PIN if (init->useCurrentMeterADC && timerHardwarePtr->tag == IO_TAG(CURRENT_METER_ADC_PIN)) { continue; } @@ -292,11 +292,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) if (type == MAP_TO_PPM_INPUT) { #ifndef SKIP_RX_PWM_PPM -#if defined(SPARKY) || defined(ALIENFLIGHTF3) - if (!(init->pwmProtocolType == PWM_TYPE_CONVENTIONAL)) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2, init->pwmProtocolType); - } -#endif + ppmTimer = timerHardwarePtr->tim; ppmInConfig(timerHardwarePtr); #endif } else if (type == MAP_TO_PWM_INPUT) { @@ -313,9 +309,15 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) continue; } #endif + if (init->usePPM) { + if (init->pwmProtocolType != PWM_TYPE_CONVENTIONAL && timerHardwarePtr->tim == ppmTimer) { + ppmAvoidPWMTimerClash(timerHardwarePtr, ppmTimer, init->pwmProtocolType); + } + } + if (init->useFastPwm) { pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->pwmProtocolType); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_OUTPUT_PROTOCOL_ONESHOT; + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_OUTPUT_PROTOCOL_ONESHOT; } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) { pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate); pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_MOTOR_MODE_BRUSHED; diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index e33f1a58c8..065e0d0f71 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input @@ -29,11 +30,11 @@ const uint16_t multiPPM[] = { PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; From 6345280ad77f30913f8846cca7b832e9eb1bd053 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 19 Aug 2016 17:52:11 +0100 Subject: [PATCH 354/456] Added average to FIR filter. Added int16 FIR filter. --- src/main/common/filter.c | 120 +++++++++++++++++++++++++++++++-------- src/main/common/filter.h | 26 +++++++-- 2 files changed, 117 insertions(+), 29 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 3a4cbca7a1..75763a5ade 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -116,29 +116,9 @@ float biquadFilterApply(biquadFilter_t *filter, float input) return result; } -int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]) { - int count; - int32_t averageSum = 0; - - for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1]; - averageState[0] = input; - for (count = 0; count < averageCount; count++) averageSum += averageState[count]; - - return averageSum / averageCount; -} - -float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]) { - int count; - float averageSum = 0.0f; - - for (count = averageCount-1; count > 0; count--) averageState[count] = averageState[count-1]; - averageState[0] = input; - for (count = 0; count < averageCount; count++) averageSum += averageState[count]; - - return averageSum / averageCount; -} - -// FIR filter +/* + * FIR filter + */ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength) { filter->buf = buf; @@ -148,6 +128,10 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl memset(filter->buf, 0, sizeof(float) * filter->bufLength); } +/* + * FIR filter initialisation + * If FIR filter is just used for averaging, coeffs can be set to NULL + */ void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs) { firFilterInit2(filter, buf, bufLength, coeffs, bufLength); @@ -155,11 +139,11 @@ void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const flo void firFilterUpdate(firFilter_t *filter, float input) { - memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(float)); + memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(input)); filter->buf[0] = input; } -float firFilterApply(firFilter_t *filter) +float firFilterApply(const firFilter_t *filter) { float ret = 0.0f; for (int ii = 0; ii < filter->coeffsLength; ++ii) { @@ -167,3 +151,89 @@ float firFilterApply(firFilter_t *filter) } return ret; } + +float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count) +{ + float ret = 0.0f; + for (int ii = 0; ii < count; ++ii) { + ret += filter->buf[ii]; + } + return ret / count; +} + +float firFilterCalcAverage(const firFilter_t *filter) +{ + return firFilterCalcPartialAverage(filter, filter->coeffsLength); +} + +float firFilterLastInput(const firFilter_t *filter) +{ + return filter->buf[0]; +} + +float firFilterGet(const firFilter_t *filter, int index) +{ + return filter->buf[index]; +} + +/* + * int16_t based FIR filter + * Can be directly updated from devices that produce 16-bit data, eg gyros and accelerometers + */ +void firFilterInt16Init2(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength) +{ + filter->buf = buf; + filter->bufLength = bufLength; + filter->coeffs = coeffs; + filter->coeffsLength = coeffsLength; + memset(filter->buf, 0, sizeof(int16_t) * filter->bufLength); +} + +/* + * FIR filter initialisation + * If FIR filter is just used for averaging, coeffs can be set to NULL + */ +void firFilterInt16Init(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs) +{ + firFilterInt16Init2(filter, buf, bufLength, coeffs, bufLength); +} + +void firFilterInt16Update(firFilterInt16_t *filter, int16_t input) +{ + memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(input)); + filter->buf[0] = input; +} + +float firFilterInt16Apply(const firFilterInt16_t *filter) +{ + float ret = 0.0f; + for (int ii = 0; ii < filter->coeffsLength; ++ii) { + ret += filter->coeffs[ii] * filter->buf[ii]; + } + return ret; +} + +float firFilterInt16CalcPartialAverage(const firFilterInt16_t *filter, uint8_t count) +{ + float ret = 0; + for (int ii = 0; ii < count; ++ii) { + ret += filter->buf[ii]; + } + return ret / count; +} + +float firFilterInt16CalcAverage(const firFilterInt16_t *filter) +{ + return firFilterInt16CalcPartialAverage(filter, filter->coeffsLength); +} + +int16_t firFilterInt16LastInput(const firFilterInt16_t *filter) +{ + return filter->buf[0]; +} + +int16_t firFilterInt16Get(const firFilter_t *filter, int index) +{ + return filter->buf[index]; +} + diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 8e29a79270..c63f1cad19 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -47,6 +47,14 @@ typedef struct firFilter_s { uint8_t coeffsLength; } firFilter_t; +typedef struct firFilterInt16_s { + int16_t *buf; + const float *coeffs; + uint8_t bufLength; + uint8_t coeffsLength; +} firFilterInt16_t; + + void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); float biquadFilterApply(biquadFilter_t *filter, float input); @@ -56,11 +64,21 @@ void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT); float pt1FilterApply(pt1Filter_t *filter, float input); float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT); -int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]); -float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]); - void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs); void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength); void firFilterUpdate(firFilter_t *filter, float input); -float firFilterApply(firFilter_t *filter); +float firFilterApply(const firFilter_t *filter); +float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count); +float firFilterCalcAverage(const firFilter_t *filter); +float firFilterLastInput(const firFilter_t *filter); +float firFilterGet(const firFilter_t *filter, int index); + +void firFilterInt16Init(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs); +void firFilterInt16Init2(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength); +void firFilterInt16Update(firFilterInt16_t *filter, int16_t input); +float firFilterInt16Apply(const firFilterInt16_t *filter); +float firFilterInt16CalcPartialAverage(const firFilterInt16_t *filter, uint8_t count); +float firFilterInt16CalcAverage(const firFilterInt16_t *filter); +int16_t firFilterInt16LastInput(const firFilterInt16_t *filter); +int16_t firFilterInt16Get(const firFilter_t *filter, int index); From afdb0329bb406a3549e0f787fbb21d3531dfee7d Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 20 Aug 2016 05:52:29 +0100 Subject: [PATCH 355/456] Added bounds checking in pwmInit --- src/main/drivers/pwm_mapping.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 1466aa2d25..0a517140e4 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -305,7 +305,10 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) channelIndex++; #endif } else if (type == MAP_TO_MOTOR_OUTPUT) { - + // Check if we already configured maximum supported number of motors or output ports and skip the rest + if (pwmOutputConfiguration.motorCount >= MAX_MOTORS || pwmOutputConfiguration.outputCount >= MAX_PWM_OUTPUT_PORTS) { + continue; + } #ifdef CC3D if (!(init->pwmProtocolType == PWM_TYPE_CONVENTIONAL)) { // Skip it if it would cause PPM capture timer to be reconfigured or manually overflowed @@ -329,6 +332,9 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) pwmOutputConfiguration.outputCount++; } else if (type == MAP_TO_SERVO_OUTPUT) { #ifdef USE_SERVOS + if (pwmOutputConfiguration.servoCount >= MAX_SERVOS || pwmOutputConfiguration.outputCount >= MAX_PWM_OUTPUT_PORTS) { + continue; + } pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].index = pwmOutputConfiguration.servoCount; pwmServoConfig(timerHardwarePtr, pwmOutputConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse); pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_SERVO | PWM_PF_OUTPUT_PROTOCOL_PWM; From 655c05b54c65882001815076301d7c2f8db42491 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 20 Aug 2016 06:04:56 +0100 Subject: [PATCH 356/456] Maximised hardware for STM32F3DISCOVERY target --- src/main/target/STM32F3DISCOVERY/target.h | 61 +++++++++++++++------- src/main/target/STM32F3DISCOVERY/target.mk | 17 ++++-- 2 files changed, 56 insertions(+), 22 deletions(-) diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index a75e952171..78e47bf26f 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -31,12 +31,12 @@ #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE -#define LED0 PE8 // Blue LEDs - PE8/PE12 +#define LED0 PE8 // Blue LEDs - PE8/PE12 #define LED0_INVERTED -#define LED1 PE10 // Orange LEDs - PE10/PE14 +#define LED1 PE10 // Orange LEDs - PE10/PE14 #define LED1_INVERTED -#define BEEPER PE9 // Red LEDs - PE9/PE13 +#define BEEPER PE9 // Red LEDs - PE9/PE13 #define BEEPER_INVERTED #define USE_SPI @@ -73,29 +73,47 @@ // PB12 SPI2_NSS #define GYRO +#define USE_FAKE_GYRO #define USE_GYRO_L3GD20 #define L3GD20_SPI SPI1 #define L3GD20_CS_PIN PE3 #define GYRO_L3GD20_ALIGN CW270_DEG - +#define USE_GYRO_L3G4200D +#define USE_GYRO_MPU3050 +#define USE_GYRO_MPU6050 +#define USE_GYRO_SPI_MPU6000 +#define MPU6000_CS_PIN SPI2_NSS_PIN +#define MPU6000_SPI_INSTANCE SPI2 // Support the GY-91 MPU9250 dev board #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 #define MPU6500_CS_PIN PC14 #define MPU6500_SPI_INSTANCE SPI2 #define GYRO_MPU6500_ALIGN CW270_DEG_FLIP +#define USE_GYRO_SPI_MPU9250 +#define MPU9250_CS_PIN SPI2_NSS_PIN +#define MPU9250_SPI_INSTANCE SPI2 #define ACC +#define USE_FAKE_ACC +#define USE_ACC_ADXL345 +#define USE_ACC_BMA280 +#define USE_ACC_MMA8452 +#define USE_ACC_MPU6050 #define USE_ACC_LSM303DLHC +#define USE_ACC_MPU6000 +#define USE_ACC_SPI_MPU6000 #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 +#define USE_ACC_MPU9250 +#define USE_ACC_SPI_MPU9250 #define ACC_MPU6500_ALIGN CW270_DEG_FLIP -//#define BARO -//#define BMP280_CS_PIN PB12 -//#define BMP280_SPI_INSTANCE SPI2 -//#define USE_BARO_BMP280 -//#define USE_BARO_SPI_BMP280 +#define BARO +#define USE_FAKE_BARO +#define USE_BARO_BMP085 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 #define OSD #define USE_MAX7456 @@ -120,20 +138,21 @@ // #define AFATFS_USE_INTROSPECTIVE_LOGGING #define MAG +#define USE_FAKE_MAG +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 #define USE_MAG_HMC5883 #define USE_VCP #define USE_UART1 #define USE_UART2 -#define SERIAL_PORT_COUNT 3 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define SERIAL_PORT_COUNT 6 -// uart2 gpio for shared serial rx/ppm -//#define UART2_TX_PIN GPIO_Pin_5 // PD5 -//#define UART2_RX_PIN GPIO_Pin_6 // PD6 -//#define UART2_GPIO GPIOD -//#define UART2_GPIO_AF GPIO_AF_7 -//#define UART2_TX_PINSOURCE GPIO_PinSource5 -//#define UART2_RX_PINSOURCE GPIO_PinSource6 +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) #define USE_I2C #define I2C_DEVICE (I2CDEV_1) @@ -152,9 +171,15 @@ #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER - #define LED_STRIP_TIMER TIM16 +#define SPEKTRUM_BIND +#define BIND_PIN PA3 // USART2, PA3 + +#define SONAR +#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 + #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - 303 in 100pin package diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index e3eb8bffdc..db3fa135ce 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -1,9 +1,11 @@ F3_TARGETS += $(TARGET) -FEATURES = VCP SDCARD +FEATURES = VCP SDCARD HIGHEND TARGET_SRC = \ - drivers/light_ws2811strip.c \ + drivers/accgyro_adxl345.c \ + drivers/accgyro_bma280.c \ drivers/accgyro_l3gd20.c \ + drivers/accgyro_l3g4200d.c \ drivers/accgyro_lsm303dlhc.c \ drivers/compass_hmc5883l.c \ drivers/accgyro_adxl345.c \ @@ -13,10 +15,17 @@ TARGET_SRC = \ drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu6050.c \ drivers/accgyro_mpu6500.c \ + drivers/accgyro_spi_mpu6000.c \ drivers/accgyro_spi_mpu6500.c \ - drivers/accgyro_l3g4200d.c \ - drivers/barometer_ms5611.c \ + drivers/accgyro_spi_mpu9250.c \ + drivers/barometer_bmp085.c \ drivers/barometer_bmp280.c \ + drivers/barometer_ms5611.c \ + drivers/compass_ak8963.c \ drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ drivers/max7456.c \ + drivers/sonar_hcsr04.c \ io/osd.c From 0668493ddd6da6834f7d802c54160dfbda04e230 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 20 Aug 2016 23:24:25 +0200 Subject: [PATCH 357/456] Busy wait reduction // Cleanup // New defaults --- src/main/config/config.c | 32 ++++++++++++------------- src/main/config/config_master.h | 1 - src/main/io/serial_cli.c | 13 +++++++---- src/main/main.c | 2 +- src/main/mw.c | 41 ++++++++++++++++----------------- 5 files changed, 45 insertions(+), 44 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 52f705d385..5f0cf31eae 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -185,11 +185,11 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcRate8 = 100; controlRateConfig->rcYawRate8 = 100; - controlRateConfig->rcExpo8 = 10; + controlRateConfig->rcExpo8 = 0; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; - controlRateConfig->dynThrPID = 20; - controlRateConfig->rcYawExpo8 = 10; + controlRateConfig->dynThrPID = 10; + controlRateConfig->rcYawExpo8 = 0; controlRateConfig->tpa_breakpoint = 1650; for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { @@ -205,7 +205,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->I8[ROLL] = 40; pidProfile->D8[ROLL] = 20; pidProfile->P8[PITCH] = 60; - pidProfile->I8[PITCH] = 60; + pidProfile->I8[PITCH] = 65; pidProfile->D8[PITCH] = 22; pidProfile->P8[YAW] = 80; pidProfile->I8[YAW] = 45; @@ -236,11 +236,11 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yawItermIgnoreRate = 32; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default - pidProfile->dterm_notch_hz = 0; - pidProfile->dterm_notch_cutoff = 150; + pidProfile->dterm_notch_hz = 260; + pidProfile->dterm_notch_cutoff = 160; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; - pidProfile->pidAtMinThrottle = PID_STABILISATION_OFF; + pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters pidProfile->ptermSetpointWeight = 75; @@ -504,9 +504,9 @@ void createDefaultConfig(master_t *config) config->gyroConfig.gyroMovementCalibrationThreshold = 32; // xxx_hardware: 0:default/autodetect, 1: disable - config->mag_hardware = 0; + config->mag_hardware = 1; - config->baro_hardware = 0; + config->baro_hardware = 1; resetBatteryConfig(&config->batteryConfig); @@ -585,8 +585,6 @@ void createDefaultConfig(master_t *config) resetSerialConfig(&config->serialConfig); - config->emf_avoidance = 0; // TODO - needs removal - resetProfile(&config->profile[0]); resetRollAndPitchTrims(&config->accelerometerTrims); @@ -614,12 +612,12 @@ void createDefaultConfig(master_t *config) config->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv // Failsafe Variables - config->failsafeConfig.failsafe_delay = 10; // 1sec - config->failsafeConfig.failsafe_off_delay = 10; // 1sec - config->failsafeConfig.failsafe_throttle = 1000; // default throttle off. - config->failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss - config->failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition - config->failsafeConfig.failsafe_procedure = 0; // default full failsafe procedure is 0: auto-landing + config->failsafeConfig.failsafe_delay = 10; // 1sec + config->failsafeConfig.failsafe_off_delay = 10; // 1sec + config->failsafeConfig.failsafe_throttle = 1000; // default throttle off. + config->failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss + config->failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition + config->failsafeConfig.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;// default full failsafe procedure is 0: auto-landing #ifdef USE_SERVOS // servos diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index aa572fac0a..cf93ca3b45 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -25,7 +25,6 @@ typedef struct master_t { uint8_t mixerMode; uint32_t enabledFeatures; - uint8_t emf_avoidance; // change pll settings to avoid noise in the uhf band // motor/esc/servo related stuff motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9b333b35e4..e3546636f6 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -517,6 +517,10 @@ static const char * const lookupTableLowpassType[] = { "NORMAL", "HIGH" }; +static const char * const lookupTableFailsafe[] = { + "AUTO-LAND", "DROP" +}; + typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -555,6 +559,7 @@ typedef enum { TABLE_DELTA_METHOD, TABLE_RC_INTERPOLATION, TABLE_LOWPASS_TYPE, + TABLE_FAILSAFE, #ifdef OSD TABLE_OSD, #endif @@ -593,6 +598,7 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) }, { lookupTableRcInterpolation, sizeof(lookupTableRcInterpolation) / sizeof(char *) }, { lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) }, + { lookupTableFailsafe, sizeof(lookupTableFailsafe) / sizeof(char *) }, #ifdef OSD { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) }, #endif @@ -646,7 +652,6 @@ typedef struct { } clivalue_t; const clivalue_t valueTable[] = { -// { "emf_avoidance", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.emf_avoidance, .config.lookup = { TABLE_OFF_ON } }, { "mid_rc", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.midrc, .config.minmax = { 1200, 1700 } }, { "min_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.mincheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, @@ -808,7 +813,7 @@ const clivalue_t valueTable[] = { { "failsafe_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_throttle, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX } }, { "failsafe_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.failsafeConfig.failsafe_kill_switch, .config.lookup = { TABLE_OFF_ON } }, { "failsafe_throttle_low_delay",VAR_UINT16 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_throttle_low_delay, .config.minmax = { 0, 300 } }, - { "failsafe_procedure", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.failsafeConfig.failsafe_procedure, .config.lookup = { TABLE_OFF_ON } }, + { "failsafe_procedure", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.failsafeConfig.failsafe_procedure, .config.lookup = { TABLE_FAILSAFE } }, { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } }, @@ -852,8 +857,8 @@ const clivalue_t valueTable[] = { { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, - { "accumulation_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, - { "yaw_accumulation_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, + { "accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, + { "yaw_accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, diff --git a/src/main/main.c b/src/main/main.c index 24b983308d..2d961c442c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -675,7 +675,7 @@ void main_init(void) /* Setup scheduler */ schedulerInit(); - rescheduleTask(TASK_GYROPID, gyro.targetLooptime); + rescheduleTask(TASK_GYROPID, gyro.targetLooptime + 5); // Add a littlebit of extra time to reduce busy wait setTaskEnabled(TASK_GYROPID, true); if (sensors(SENSOR_ACC)) { diff --git a/src/main/mw.c b/src/main/mw.c index 86abfd6c63..80605784e6 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -816,8 +816,6 @@ void taskMainPidLoopCheck(void) static uint32_t previousTime; static bool runTaskMainSubprocesses; - const uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF); - cycleTime = micros() - previousTime; previousTime = micros(); @@ -827,30 +825,31 @@ void taskMainPidLoopCheck(void) } const uint32_t startTime = micros(); + while (true) { - if (gyroSyncCheckUpdate(&gyro) || ((currentDeltaTime + (micros() - previousTime)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) { - static uint8_t pidUpdateCountdown; - + if (gyroSyncCheckUpdate(&gyro)) { if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting - if (runTaskMainSubprocesses) { - subTaskMainSubprocesses(); - runTaskMainSubprocesses = false; - } - - gyroUpdate(); - - if (pidUpdateCountdown) { - pidUpdateCountdown--; - } else { - pidUpdateCountdown = setPidUpdateCountDown(); - subTaskPidController(); - subTaskMotorUpdate(); - runTaskMainSubprocesses = true; - } - break; } } + + static uint8_t pidUpdateCountdown; + + if (runTaskMainSubprocesses) { + subTaskMainSubprocesses(); + runTaskMainSubprocesses = false; + } + + gyroUpdate(); + + if (pidUpdateCountdown) { + pidUpdateCountdown--; + } else { + pidUpdateCountdown = setPidUpdateCountDown(); + subTaskPidController(); + subTaskMotorUpdate(); + runTaskMainSubprocesses = true; + } } void taskUpdateAccelerometer(void) From 4e2ff7e7e9647c50671a99df6112a84f141a53db Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 21 Aug 2016 00:13:14 +0200 Subject: [PATCH 358/456] Improve dterm filter accuracy // magic number --- src/main/flight/pid.c | 4 ++-- src/main/flight/pid.h | 2 +- src/main/main.c | 6 ++++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 829b62c0f3..b6ffc3d586 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -76,9 +76,9 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using #endif -void setTargetPidLooptime(uint8_t pidProcessDenom) +void setTargetPidLooptime(uint32_t pidLooptime) { - targetPidLooptime = gyro.targetLooptime * pidProcessDenom; + targetPidLooptime = pidLooptime; } void pidResetErrorGyroState(void) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 12fa8324e8..fd1fb08f5a 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -126,5 +126,5 @@ extern uint32_t targetPidLooptime; void pidSetController(pidControllerType_e type); void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); -void setTargetPidLooptime(uint8_t pidProcessDenom); +void setTargetPidLooptime(uint32_t pidLooptime); diff --git a/src/main/main.c b/src/main/main.c index 2d961c442c..eb5e0f90c7 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -106,6 +106,8 @@ #include "config/config_profile.h" #include "config/config_master.h" +#define LOOPTIME_SUSPEND_TIME 5 // Prevent too long busy wait times + #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -597,7 +599,7 @@ void init(void) masterConfig.gyro_sync_denom = 1; } - setTargetPidLooptime(masterConfig.pid_process_denom); // Initialize pid looptime + setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime #ifdef BLACKBOX @@ -675,7 +677,7 @@ void main_init(void) /* Setup scheduler */ schedulerInit(); - rescheduleTask(TASK_GYROPID, gyro.targetLooptime + 5); // Add a littlebit of extra time to reduce busy wait + rescheduleTask(TASK_GYROPID, gyro.targetLooptime + LOOPTIME_SUSPEND_TIME); // Add a littlebit of extra time to reduce busy wait setTaskEnabled(TASK_GYROPID, true); if (sensors(SENSOR_ACC)) { From 138ade79399aec264b69fc23b3b1544602892dd9 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 21 Aug 2016 00:13:14 +0200 Subject: [PATCH 359/456] Improve dterm filter accuracy // magic number --- src/main/flight/pid.c | 4 ++-- src/main/flight/pid.h | 2 +- src/main/main.c | 6 ++++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0f8d30fb9b..9cea9bad7a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -72,9 +72,9 @@ pidControllerFuncPtr pid_controller = pidLegacy; // which pid controller are we pidControllerFuncPtr pid_controller = pidBetaflight; // which pid controller are we using #endif -void setTargetPidLooptime(uint8_t pidProcessDenom) +void setTargetPidLooptime(uint32_t pidLooptime) { - targetPidLooptime = gyro.targetLooptime * pidProcessDenom; + targetPidLooptime = pidLooptime; } void pidResetErrorGyroState(void) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7a9092b622..a13c366a27 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -133,5 +133,5 @@ extern uint8_t PIDweight[3]; void pidSetController(pidControllerType_e type); void pidResetErrorGyroState(void); void pidStabilisationState(pidStabilisationState_e pidControllerState); -void setTargetPidLooptime(uint8_t pidProcessDenom); +void setTargetPidLooptime(uint32_t pidLooptime); diff --git a/src/main/main.c b/src/main/main.c index fa00e24bcb..608486d5b6 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -109,6 +109,8 @@ #include "config/config_profile.h" #include "config/config_master.h" +#define LOOPTIME_SUSPEND_TIME 5 // Prevent too long busy wait times + #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -600,7 +602,7 @@ void init(void) masterConfig.gyro_sync_denom = 1; } - setTargetPidLooptime(masterConfig.pid_process_denom); // Initialize pid looptime + setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime #ifdef BLACKBOX @@ -678,7 +680,7 @@ void main_init(void) /* Setup scheduler */ schedulerInit(); - rescheduleTask(TASK_GYROPID, gyro.targetLooptime + 5); // Add a littlebit of extra time to reduce busy wait + rescheduleTask(TASK_GYROPID, gyro.targetLooptime + LOOPTIME_SUSPEND_TIME); // Add a littlebit of extra time to reduce busy wait setTaskEnabled(TASK_GYROPID, true); if (sensors(SENSOR_ACC)) { From 75da7ee207bc6bbb654246b9eb850b6c59095466 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 21 Aug 2016 00:22:02 +0200 Subject: [PATCH 360/456] Increase Battery Priority --- src/main/scheduler/scheduler_tasks.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/scheduler/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c index 9d5062ee21..30920724a8 100644 --- a/src/main/scheduler/scheduler_tasks.c +++ b/src/main/scheduler/scheduler_tasks.c @@ -73,7 +73,7 @@ cfTask_t cfTasks[TASK_COUNT] = { .taskName = "BATTERY", .taskFunc = taskUpdateBattery, .desiredPeriod = 1000000 / 50, // 50 Hz - .staticPriority = TASK_PRIORITY_LOW, + .staticPriority = TASK_PRIORITY_MEDIUM, }, #ifdef BEEPER From 4043f5e5b4722d90e6790f9d03762b0d1ff3c7c6 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 21 Aug 2016 00:22:02 +0200 Subject: [PATCH 361/456] Increase Battery Priority --- src/main/scheduler/scheduler_tasks.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/scheduler/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c index 9d5062ee21..30920724a8 100644 --- a/src/main/scheduler/scheduler_tasks.c +++ b/src/main/scheduler/scheduler_tasks.c @@ -73,7 +73,7 @@ cfTask_t cfTasks[TASK_COUNT] = { .taskName = "BATTERY", .taskFunc = taskUpdateBattery, .desiredPeriod = 1000000 / 50, // 50 Hz - .staticPriority = TASK_PRIORITY_LOW, + .staticPriority = TASK_PRIORITY_MEDIUM, }, #ifdef BEEPER From 2b14e5d20c58c6a13a353aac34184a9dc3d208a9 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 21 Aug 2016 00:56:08 +0200 Subject: [PATCH 362/456] Include missing headers for ISHAPEDF3 --- src/main/target/ISHAPEDF3/target.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/ISHAPEDF3/target.c b/src/main/target/ISHAPEDF3/target.c index ec9176a27f..99f79ec9b7 100644 --- a/src/main/target/ISHAPEDF3/target.c +++ b/src/main/target/ISHAPEDF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "drivers/timer.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input From a321f7f766319d024328d6164f2f418545fe4abe Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 21 Aug 2016 01:06:13 +0200 Subject: [PATCH 363/456] Reduce Suspend time --- src/main/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/main.c b/src/main/main.c index eb5e0f90c7..7949e1a560 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -106,7 +106,7 @@ #include "config/config_profile.h" #include "config/config_master.h" -#define LOOPTIME_SUSPEND_TIME 5 // Prevent too long busy wait times +#define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" From 9178ab5e6a8a15fe8def5b2d241079ffe93e16f2 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 21 Aug 2016 01:06:13 +0200 Subject: [PATCH 364/456] Reduce Suspend time --- src/main/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/main.c b/src/main/main.c index 608486d5b6..3b0a65e6bb 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -109,7 +109,7 @@ #include "config/config_profile.h" #include "config/config_master.h" -#define LOOPTIME_SUSPEND_TIME 5 // Prevent too long busy wait times +#define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" From 525dbcc3082f2d7fd04ec70d05b341834c88ac4a Mon Sep 17 00:00:00 2001 From: nathan Date: Sat, 20 Aug 2016 22:55:03 -0700 Subject: [PATCH 365/456] unswap omnibus motors 3 and 4 --- src/main/target/OMNIBUS/target.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 977d623f85..8a87602bea 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -87,8 +87,8 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM1 - PB8 { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM2 - PB9 - { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - PA2 - { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM4 - PA3 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM3 - PA3 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM4 - PA2 // UART3 RX/TX { TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PB10 - TIM2_CH3 / UART3_TX (AF7) From f15eb69d2c05a8cd045cf3a412c1edebf080d36a Mon Sep 17 00:00:00 2001 From: jflyper Date: Sun, 21 Aug 2016 22:39:00 +0900 Subject: [PATCH 366/456] Update RMDO MPU interrupt support The RMDO/target.h was under maintained that it was probably outdated by EXTI changes at some stage. --- src/main/target/RMDO/target.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 67d2232e68..57155d209b 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -26,11 +26,13 @@ #define BEEPER PC15 #define BEEPER_INVERTED +#define USE_EXTI +#define MPU_INT_EXTI PC13 #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define USE_MAG_DATA_READY_SIGNAL -#define ENSURE_MAG_DATA_READY_IS_HIGH +//#define USE_MAG_DATA_READY_SIGNAL // XXX Do RMDO has onboard mag??? +//#define ENSURE_MAG_DATA_READY_IS_HIGH #define GYRO #define USE_GYRO_MPU6050 From 174c45b67f129847c6b9154e353590740cb7235a Mon Sep 17 00:00:00 2001 From: dexX7 Date: Sun, 21 Aug 2016 21:52:29 +0200 Subject: [PATCH 367/456] Add missing pin to IRCFUSION3 target Without the pin for the IRCFUSION3 target, connecting to the FC was not possible. To fix it, the missing pin is added to the target. --- src/main/target/IRCFUSIONF3/target.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index 8dbd6e55ba..62eec67fc5 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -25,6 +25,9 @@ #define USABLE_TIMER_CHANNEL_COUNT 17 +#define USE_EXTI +#define MPU_INT_EXTI PC13 + #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready, no MAG #define USE_MPU_DATA_READY_SIGNAL From 2f14f6119039b6feee9a2388253cf608f7fbf60e Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 23 Aug 2016 01:21:19 +1200 Subject: [PATCH 368/456] Fixed lockups on LED_STRIP configuration. --- src/main/io/ledstrip.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 4af92de4d3..c41c3a884b 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -257,10 +257,10 @@ STATIC_UNIT_TESTED void determineLedStripDimensions(void) STATIC_UNIT_TESTED void determineOrientationLimits(void) { - highestYValueForNorth = (ledGridHeight / 2) - 1; - lowestYValueForSouth = ((ledGridHeight + 1) / 2); - highestXValueForWest = (ledGridWidth / 2) - 1; - lowestXValueForEast = ((ledGridWidth + 1) / 2); + highestYValueForNorth = MIN((ledGridHeight / 2) - 1, 0); + lowestYValueForSouth = (ledGridHeight + 1) / 2; + highestXValueForWest = MIN((ledGridWidth / 2) - 1, 0); + lowestXValueForEast = (ledGridWidth + 1) / 2; } STATIC_UNIT_TESTED void updateLedCount(void) @@ -533,7 +533,11 @@ static void applyLedFixedLayers() case LED_FUNCTION_FLIGHT_MODE: for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { - color = *getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]); + hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &masterConfig.modeColors[flightModeToLed[i].ledMode]); + if (directionalColor) { + color = *directionalColor; + } + break; // stop on first match } break; From 0f1b00520f1e4734c9ca308e2677e617754c17a3 Mon Sep 17 00:00:00 2001 From: Gamma-Software Date: Mon, 22 Aug 2016 16:33:23 +0200 Subject: [PATCH 369/456] Little optimisation PID.c Hi, I know, it's not a huge optimisation but I just switched the condition of the IF condition (line 425). Given that the YAW axis occur one time in the FOR loop (line 366), the IF(axis == YAW) condition will be called one time whereas the IF(axis != YAW) condition will be called two times. Check first the most used condition in an IF condition is a good thing to do in a code source. I just don't know if it will be a major improvement, I let you judge. --- src/main/flight/pid.c | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b6ffc3d586..a4f42ffedc 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -422,20 +422,7 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina ITerm = errorGyroI[axis] >> 13; //-----calculate D-term - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = PTerm + ITerm; - - if (motorCount >= 4) { - int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); - - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); - } - - DTerm = 0; // needed for blackbox - } else { + if (axis != YAW) { if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { delta = RateError - lastRateError[axis]; lastRateError[axis] = RateError; @@ -464,6 +451,19 @@ static void pidLegacy(const pidProfile_t *pidProfile, uint16_t max_angle_inclina // -----calculate total PID output axisPID[axis] = PTerm + ITerm + DTerm; + } else { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = PTerm + ITerm; + + if (motorCount >= 4) { + int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); + + // prevent "yaw jump" during yaw correction + axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); + } + + DTerm = 0; // needed for blackbox } if (!pidStabilisationEnabled) axisPID[axis] = 0; From f9c827c726b774f503232358d0fa8929872198de Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 23 Aug 2016 00:56:22 +0200 Subject: [PATCH 370/456] Remove Int based PID loop (All target support) --- src/main/mw.c | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index 80605784e6..fd4f970447 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -815,6 +815,7 @@ void taskMainPidLoopCheck(void) { static uint32_t previousTime; static bool runTaskMainSubprocesses; + static uint8_t pidUpdateCountdown; cycleTime = micros() - previousTime; previousTime = micros(); @@ -824,17 +825,6 @@ void taskMainPidLoopCheck(void) debug[1] = averageSystemLoadPercent; } - const uint32_t startTime = micros(); - - while (true) { - if (gyroSyncCheckUpdate(&gyro)) { - if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting - break; - } - } - - static uint8_t pidUpdateCountdown; - if (runTaskMainSubprocesses) { subTaskMainSubprocesses(); runTaskMainSubprocesses = false; From e6a11960fd3bb1fdbe4bd99de4b9437847b70221 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 23 Aug 2016 01:06:19 +0200 Subject: [PATCH 371/456] Equal pid.c optimalisation to betaflight pid --- src/main/flight/pid.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a4f42ffedc..8950db6ab1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -286,13 +286,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc ITerm = errorGyroIf[axis]; //-----calculate D-term (Yaw D not yet supported) - if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = lrintf(PTerm + ITerm); - - DTerm = 0.0f; // needed for blackbox - } else { + if (axis != YAW) { rD = c[axis] * setpointRate[axis] - PVRate; // cr - y delta = rD - lastRateError[axis]; lastRateError[axis] = rD; @@ -317,6 +311,12 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); + } else { + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); + + axisPID[axis] = lrintf(PTerm + ITerm); + + DTerm = 0.0f; // needed for blackbox } // Disable PID control at zero throttle From 5e5b44f5935fdffc3ef23f2e6560a6e193aa5009 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 24 Aug 2016 00:33:32 +0200 Subject: [PATCH 372/456] Various cleanups and increase in D setpoint weight range --- src/main/config/config.c | 4 ++-- src/main/io/serial_cli.c | 2 +- src/main/main.c | 7 ++----- src/main/mw.c | 4 +--- 4 files changed, 6 insertions(+), 11 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 5f0cf31eae..f74bf867f2 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -243,8 +243,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->ptermSetpointWeight = 75; - pidProfile->dtermSetpointWeight = 120; + pidProfile->ptermSetpointWeight = 80; + pidProfile->dtermSetpointWeight = 150; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; pidProfile->toleranceBand = 0; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e3546636f6..ff524656e9 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -853,7 +853,7 @@ const clivalue_t valueTable[] = { { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } }, - { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 200 } }, + { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 300 } }, { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, diff --git a/src/main/main.c b/src/main/main.c index 7949e1a560..5495856e18 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -106,8 +106,6 @@ #include "config/config_profile.h" #include "config/config_master.h" -#define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times - #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -599,8 +597,7 @@ void init(void) masterConfig.gyro_sync_denom = 1; } - setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime - + setTargetPidLooptime(gyro.targetLooptime * masterConfig.pid_process_denom); // Initialize pid looptime #ifdef BLACKBOX initBlackbox(); @@ -677,7 +674,7 @@ void main_init(void) /* Setup scheduler */ schedulerInit(); - rescheduleTask(TASK_GYROPID, gyro.targetLooptime + LOOPTIME_SUSPEND_TIME); // Add a littlebit of extra time to reduce busy wait + rescheduleTask(TASK_GYROPID, gyro.targetLooptime); // Add a littlebit of extra time to reduce busy wait setTaskEnabled(TASK_GYROPID, true); if (sensors(SENSOR_ACC)) { diff --git a/src/main/mw.c b/src/main/mw.c index fd4f970447..d616015285 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -813,12 +813,10 @@ uint8_t setPidUpdateCountDown(void) { // Function for loop trigger void taskMainPidLoopCheck(void) { - static uint32_t previousTime; static bool runTaskMainSubprocesses; static uint8_t pidUpdateCountdown; - cycleTime = micros() - previousTime; - previousTime = micros(); + cycleTime = getTaskDeltaTime(TASK_SELF); if (debugMode == DEBUG_CYCLETIME) { debug[0] = cycleTime; From f4796c3676cc8f152bb06b0a538318a1ccb74b90 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 24 Aug 2016 01:07:04 +0200 Subject: [PATCH 373/456] Drop Betaflight PIDc support for OPBL --- src/main/config/config.c | 4 ++++ src/main/io/serial_cli.c | 2 ++ src/main/io/serial_msp.c | 2 ++ src/main/target/CC3D/target.h | 1 + 4 files changed, 9 insertions(+) diff --git a/src/main/config/config.c b/src/main/config/config.c index f74bf867f2..996dd1f205 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -199,7 +199,11 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) static void resetPidProfile(pidProfile_t *pidProfile) { +#if defined(SKIP_PID_FLOAT) + pidProfile->pidController = PID_CONTROLLER_LEGACY; +#else pidProfile->pidController = PID_CONTROLLER_BETAFLIGHT; +#endif pidProfile->P8[ROLL] = 45; pidProfile->I8[ROLL] = 40; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ff524656e9..d26a310470 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -862,7 +862,9 @@ const clivalue_t valueTable[] = { { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, +#ifndef SKIP_PID_FLOAT { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, +#endif { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0, 200 } }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0, 200 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 3844595c3b..52de0e4ed1 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1353,8 +1353,10 @@ static bool processInCommand(void) read16(); break; case MSP_SET_PID_CONTROLLER: +#ifndef SKIP_PID_FLOAT currentProfile->pidProfile.pidController = constrain(read8(), 0, 1); pidSetController(currentProfile->pidProfile.pidController); +#endif break; case MSP_SET_PID: for (i = 0; i < PID_ITEM_COUNT; i++) { diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 4cf679e1e8..cdf8ceab16 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -118,6 +118,7 @@ #undef SONAR #undef USE_SOFTSERIAL1 #undef LED_STRIP +#define SKIP_PID_FLOAT #endif #define DEFAULT_RX_FEATURE FEATURE_RX_PPM From 0cb7483232fb50b5fb7af09cd55996c01cb2308f Mon Sep 17 00:00:00 2001 From: Jaakko Laurikainen Date: Wed, 6 Jul 2016 09:50:02 +0300 Subject: [PATCH 374/456] New target: RCEXPLORERF2 F3FC board from RCExplorer.se. Supports both tricopter integrated and standalone boards. --- .travis.yml | 2 + fake_travis_build.sh | 4 +- src/main/drivers/pwm_mapping.c | 7 ++ src/main/target/RCEXPLORERF3/target.c | 71 ++++++++++++ src/main/target/RCEXPLORERF3/target.h | 145 +++++++++++++++++++++++++ src/main/target/RCEXPLORERF3/target.mk | 15 +++ 6 files changed, 242 insertions(+), 2 deletions(-) create mode 100644 src/main/target/RCEXPLORERF3/target.c create mode 100644 src/main/target/RCEXPLORERF3/target.h create mode 100644 src/main/target/RCEXPLORERF3/target.mk diff --git a/.travis.yml b/.travis.yml index bee6f8274f..80b896b6a6 100644 --- a/.travis.yml +++ b/.travis.yml @@ -47,6 +47,8 @@ env: # - TARGET=VRRACE # - TARGET=X_RACERSPI # - TARGET=ZCOREF3 +# - TARGET=RCEXPLORERF3 + # use new docker environment sudo: false diff --git a/fake_travis_build.sh b/fake_travis_build.sh index 1a2c9af909..10d3e67966 100755 --- a/fake_travis_build.sh +++ b/fake_travis_build.sh @@ -21,8 +21,8 @@ targets=("PUBLISHMETA=True" \ "TARGET=DOGE" \ "TARGET=SINGULARITY" \ "TARGET=SIRINFPV" \ - "TARGET=X_RACERSPI") - + "TARGET=X_RACERSPI" \ + "TARGET=RCEXPLORERF3") #fake a travis build environment diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index bc9336cbee..2a484352e5 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -235,6 +235,13 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) type = MAP_TO_SERVO_OUTPUT; #endif +#if defined(RCEXPLORERF3) + if (timerIndex == PWM2) + { + type = MAP_TO_SERVO_OUTPUT; + } +#endif + #if (defined(STM32F3DISCOVERY) && !defined(CHEBUZZF3)) // remap PWM 5+6 or 9+10 as servos - softserial pin pairs require timer ports that use the same timer if (init->useSoftSerial) { diff --git a/src/main/target/RCEXPLORERF3/target.c b/src/main/target/RCEXPLORERF3/target.c new file mode 100644 index 0000000000..758cf0ad33 --- /dev/null +++ b/src/main/target/RCEXPLORERF3/target.c @@ -0,0 +1,71 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const uint16_t multiPPM[] = { + PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM17 - can be switched to servo + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM1 + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM17 - can be switched to servo + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM1 + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM17 - can be switched to servo + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM1 + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM17 - can be switched to servo + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM1 + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM1 - PA4 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM2 - PA7 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // PWM3 - PA8 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM4 - PB0 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - PB1 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PWM6 - PPM +}; diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h new file mode 100644 index 0000000000..46e8830b11 --- /dev/null +++ b/src/main/target/RCEXPLORERF3/target.h @@ -0,0 +1,145 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "REF3" + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + +#define LED0 PB4 +#define LED1 PB5 + +#define BEEPER PA0 +#define BEEPER_INVERTED + +#define USABLE_TIMER_CHANNEL_COUNT 6 + + +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL +#define MPU_INT_EXTI PA15 +#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define USE_ACC_SPI_MPU6000 + +#define MPU6000_CS_PIN PB12 +#define MPU6000_SPI_INSTANCE SPI2 + +#define ACC + +#define ACC_MPU6000_ALIGN CW180_DEG +#define GYRO_MPU6000_ALIGN CW180_DEG + +#define BARO +#define USE_BARO_MS5611 + +#define MAG +#define USE_MPU9250_MAG // Enables bypass configuration +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 // External + +#define MAG_AK8975_ALIGN CW180_DEG + +#define SONAR +#define SONAR_TRIGGER_PIN PA6 // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) +#define SONAR_ECHO_PIN PB1 // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) + +#define USB_IO + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define SERIAL_PORT_COUNT 4 + +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) + +#define USE_I2C +#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) + +#define I2C2_SCL PA9 +#define I2C2_SDA PA10 + +#define USE_SPI +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG) + +#define USE_ADC +#define BOARD_HAS_VOLTAGE_DIVIDER + +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA5 +#define CURRENT_METER_ADC_PIN PB2 +#define RSSI_ADC_PIN PA6 + +#define LED_STRIP // LED strip configuration using PWM motor output pin 5. + +#define USE_LED_STRIP_ON_DMA1_CHANNEL3 +#define WS2811_PIN PB8 // TIM16_CH1 +#define WS2811_TIMER TIM16 +#define WS2811_DMA_CHANNEL DMA1_Channel3 +#define WS2811_IRQ DMA1_Channel3_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER + +#define DEFAULT_FEATURES FEATURE_VBAT +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +#define NAV +#define NAV_AUTO_MAG_DECLINATION +#define NAV_GPS_GLITCH_DETECTION +#define NAV_MAX_WAYPOINTS 60 +#define GPS +#define BLACKBOX +#define TELEMETRY +#define SERIAL_RX +#define AUTOTUNE +#define DISPLAY +#define USE_SERVOS +#define USE_CLI + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PA3 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) + +#define USABLE_TIMER_CHANNEL_COUNT 6 +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(17)) diff --git a/src/main/target/RCEXPLORERF3/target.mk b/src/main/target/RCEXPLORERF3/target.mk new file mode 100644 index 0000000000..2a702724c3 --- /dev/null +++ b/src/main/target/RCEXPLORERF3/target.mk @@ -0,0 +1,15 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/barometer_ms5611.c \ + drivers/compass_hmc5883l.c \ + drivers/compass_ak8975.c \ + drivers/display_ug2864hsweg01.c \ + drivers/serial_usb_vcp.c \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/sonar_hcsr04.c From 1450b43410d7909943ba7e63933f53e863d95ec1 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Fri, 5 Aug 2016 22:18:02 +0200 Subject: [PATCH 375/456] Sparky I2C initialization fix --- src/main/target/SPARKY/target.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 582b82bf76..6e056b7269 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -72,6 +72,9 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4) +#define I2C2_SCL PA9 +#define I2C2_SDA PA10 + #define USE_ADC #define ADC_INSTANCE ADC2 #define VBAT_ADC_PIN PA4 From 863156116f0df10b5c2adce259e0aa4b4a551312 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anton=20St=C3=A5lheim?= Date: Wed, 24 Aug 2016 21:22:21 +0200 Subject: [PATCH 376/456] Added PID values over smartport telemetry --- src/main/telemetry/smartport.c | 36 ++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 4396b0c0a6..ad0415fe62 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -61,6 +61,9 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/config_profile.h" +extern profile_t *currentProfile; +extern controlRateConfig_t *currentControlRateProfile; enum { @@ -314,7 +317,9 @@ void handleSmartPortTelemetry(void) smartPortIdCnt++; int32_t tmpi; + uint32_t tmp2; static uint8_t t1Cnt = 0; + static uint8_t t2Cnt = 0; switch(id) { #ifdef GPS @@ -464,6 +469,37 @@ void handleSmartPortTelemetry(void) smartPortSendPackage(id, 0); smartPortHasRequest = 0; } + else { + switch (t2Cnt) { + case 0: + tmp2 = currentProfile->pidProfile.P8[ROLL]; + tmp2 += (currentProfile->pidProfile.P8[PITCH]<<8); + tmp2 += (currentProfile->pidProfile.P8[YAW]<<16); + break; + case 1: + tmp2 = currentProfile->pidProfile.I8[ROLL]; + tmp2 += (currentProfile->pidProfile.I8[PITCH]<<8); + tmp2 += (currentProfile->pidProfile.I8[YAW]<<16); + break; + case 2: + tmp2 = currentProfile->pidProfile.D8[ROLL]; + tmp2 += (currentProfile->pidProfile.D8[PITCH]<<8); + tmp2 += (currentProfile->pidProfile.D8[YAW]<<16); + break; + case 3: + tmp2 = currentControlRateProfile->rates[FD_ROLL]; + tmp2 += (currentControlRateProfile->rates[FD_PITCH]<<8); + tmp2 += (currentControlRateProfile->rates[FD_YAW]<<16); + break; + } + tmp2 += t2Cnt<<24; + t2Cnt++; + if (t2Cnt == 4) { + t2Cnt = 0; + } + smartPortSendPackage(id, tmp2); + smartPortHasRequest = 0; + } break; #ifdef GPS case FSSP_DATAID_GPS_ALT : From 75f6522f98541adba333e101dacc611442370f80 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 24 Aug 2016 23:50:07 +0200 Subject: [PATCH 377/456] Fix for dterm setpoint range --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index d26a310470..b434fa4300 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -853,7 +853,7 @@ const clivalue_t valueTable[] = { { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } }, - { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 300 } }, + { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, From c6a4dbd9e6bdb7535d62a32478ad51b4e59231c0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anton=20St=C3=A5lheim?= Date: Thu, 25 Aug 2016 12:10:41 +0200 Subject: [PATCH 378/456] Remove SONAR for NAZE to save FLASH --- src/main/target/NAZE/target.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 159ffa9cf5..b69f8e9129 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -104,11 +104,11 @@ #define USE_MAG_HMC5883 #define MAG_HMC5883_ALIGN CW180_DEG -#define SONAR -#define SONAR_TRIGGER_PIN PB0 -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN_PWM PB8 -#define SONAR_ECHO_PIN_PWM PB9 +//#define SONAR +//#define SONAR_TRIGGER_PIN PB0 +//#define SONAR_ECHO_PIN PB1 +//#define SONAR_TRIGGER_PIN_PWM PB8 +//#define SONAR_ECHO_PIN_PWM PB9 //#define DISPLAY From 206098e127719282afd367bdcacefa099563daa6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anton=20St=C3=A5lheim?= Date: Thu, 25 Aug 2016 12:55:14 +0200 Subject: [PATCH 379/456] Added pidAsTelemtry as option by CLI parameter --- src/main/config/config.c | 1 + src/main/io/serial_cli.c | 1 + src/main/telemetry/smartport.c | 3 ++- src/main/telemetry/telemetry.h | 1 + 4 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 1a0fc0330d..23beba43a4 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -266,6 +266,7 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) telemetryConfig->frsky_vfas_precision = 0; telemetryConfig->frsky_vfas_cell_voltage = 0; telemetryConfig->hottAlarmSoundInterval = 5; + telemetryConfig->pidValuesAsTelemetry = 0; } #endif diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 98a81b221b..00486a6470 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -746,6 +746,7 @@ const clivalue_t valueTable[] = { { "frsky_vfas_precision", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.frsky_vfas_precision, .config.minmax = { FRSKY_VFAS_PRECISION_LOW, FRSKY_VFAS_PRECISION_HIGH } }, { "frsky_vfas_cell_voltage", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.telemetryConfig.frsky_vfas_cell_voltage, .config.lookup = { TABLE_OFF_ON } }, { "hott_alarm_sound_interval", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.hottAlarmSoundInterval, .config.minmax = { 0, 120 } }, + { "pid_values_as_telemetry", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.telemetryConfig.pidValuesAsTelemetry, .config.lookup = {TABLE_OFF_ON } }, #endif { "battery_capacity", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.batteryCapacity, .config.minmax = { 0, 20000 } }, diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index ad0415fe62..fcf26b010f 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -469,7 +469,8 @@ void handleSmartPortTelemetry(void) smartPortSendPackage(id, 0); smartPortHasRequest = 0; } - else { + + else if (telemetryConfig->pidValuesAsTelemetry){ switch (t2Cnt) { case 0: tmp2 = currentProfile->pidProfile.P8[ROLL]; diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index b61aa68bd4..1d45ed5b9e 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -44,6 +44,7 @@ typedef struct telemetryConfig_s { uint8_t frsky_vfas_precision; uint8_t frsky_vfas_cell_voltage; uint8_t hottAlarmSoundInterval; + uint8_t pidValuesAsTelemetry; } telemetryConfig_t; void telemetryInit(void); From 4bd0db5a072771ed330c825d8bea3fc6c7fef8b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anton=20St=C3=A5lheim?= Date: Mon, 22 Aug 2016 22:58:43 +0200 Subject: [PATCH 380/456] Variable battery NOT PRESENT level to fix 'beeper on USB issue' --- src/main/config/config.c | 1 + src/main/io/serial_cli.c | 2 +- src/main/sensors/battery.c | 8 ++++---- src/main/sensors/battery.h | 1 + 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 1a0fc0330d..bdf6371f3e 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -282,6 +282,7 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig) batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A) batteryConfig->batteryCapacity = 0; batteryConfig->currentMeterType = CURRENT_SENSOR_ADC; + batteryConfig->batterynotpresentlevel = 55; // VBAT below 5.5 V will be igonored } #ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 98a81b221b..96374bd5dc 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -758,7 +758,7 @@ const clivalue_t valueTable[] = { { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0, 3300 } }, { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } }, { "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } }, - + { "battery_notpresent_level", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.batterynotpresentlevel, .config.minmax = { 0, 200 } }, { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.gyro_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.acc_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.mag_align, .config.lookup = { TABLE_ALIGNMENT } }, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 532f74ce79..8a1175d42b 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -39,7 +39,7 @@ #include "rx/rx.h" -#define VBATT_PRESENT_THRESHOLD_MV 10 +#define VBATT_PRESENT_THRESHOLD 10 #define VBATT_LPF_FREQ 0.4f // Battery monitoring stuff @@ -90,7 +90,7 @@ void updateBattery(void) updateBatteryVoltage(); /* battery has just been connected*/ - if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD_MV) + if (batteryState == BATTERY_NOT_PRESENT && vbat > MAX(VBATT_PRESENT_THRESHOLD, batteryConfig->batterynotpresentlevel)) { /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ batteryState = BATTERY_OK; @@ -110,8 +110,8 @@ void updateBattery(void) batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; } - /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD_MV */ - else if (batteryState != BATTERY_NOT_PRESENT && vbat <= VBATT_PRESENT_THRESHOLD_MV) + /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */ + else if (batteryState != BATTERY_NOT_PRESENT && vbat <= MAX(VBATT_PRESENT_THRESHOLD, batteryConfig->batterynotpresentlevel)) { batteryState = BATTERY_NOT_PRESENT; batteryCellCount = 0; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 3cfc2702d3..72ff54aa2e 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -50,6 +50,7 @@ typedef struct batteryConfig_s { // FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code. uint8_t multiwiiCurrentMeterOutput; // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp uint16_t batteryCapacity; // mAh + uint8_t batterynotpresentlevel; // Below this level battery is considered as not present } batteryConfig_t; typedef enum { From 3fcf206bf5fc64507d6217f6ad6b6ca8ab74b77a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anton=20St=C3=A5lheim?= Date: Thu, 25 Aug 2016 12:10:41 +0200 Subject: [PATCH 381/456] Remove SONAR for NAZE to save FLASH --- src/main/target/NAZE/target.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index a3b2bb1f34..55aa446357 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -104,11 +104,11 @@ #define USE_MAG_HMC5883 #define MAG_HMC5883_ALIGN CW180_DEG -#define SONAR -#define SONAR_TRIGGER_PIN PB0 -#define SONAR_ECHO_PIN PB1 -#define SONAR_TRIGGER_PIN_PWM PB8 -#define SONAR_ECHO_PIN_PWM PB9 +//#define SONAR +//#define SONAR_TRIGGER_PIN PB0 +//#define SONAR_ECHO_PIN PB1 +//#define SONAR_TRIGGER_PIN_PWM PB8 +//#define SONAR_ECHO_PIN_PWM PB9 //#define DISPLAY From 30175dcba0a0a8de09ced177d1b500f722c2a419 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 26 Aug 2016 02:05:28 +0200 Subject: [PATCH 382/456] Redefine rate implementation // new power expo // remove super expo feature --- src/main/config/config.c | 2 +- src/main/config/config.h | 2 +- src/main/debug.h | 1 + src/main/io/rc_controls.c | 4 ---- src/main/io/rc_controls.h | 1 - src/main/io/rc_curves.c | 6 ------ src/main/io/rc_curves.h | 1 - src/main/io/serial_cli.c | 7 ++++--- src/main/mw.c | 44 +++++++++++++++++++++++++++++---------- 9 files changed, 40 insertions(+), 28 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 996dd1f205..f5aabf3629 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -454,7 +454,7 @@ void createDefaultConfig(master_t *config) memset(config, 0, sizeof(master_t)); intFeatureClearAll(config); - intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES, config); + intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE , config); #ifdef DEFAULT_FEATURES intFeatureSet(DEFAULT_FEATURES, config); #endif diff --git a/src/main/config/config.h b/src/main/config/config.h index 6570440c35..9f7da08d5e 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -50,7 +50,7 @@ typedef enum { FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_TRANSPONDER = 1 << 21, FEATURE_AIRMODE = 1 << 22, - FEATURE_SUPEREXPO_RATES = 1 << 23, + //FEATURE_SUPEREXPO_RATES = 1 << 23, FEATURE_VTX = 1 << 24, FEATURE_RX_NRF24 = 1 << 25, FEATURE_SOFTSPI = 1 << 26, diff --git a/src/main/debug.h b/src/main/debug.h index 4431953f31..14f06fce4d 100644 --- a/src/main/debug.h +++ b/src/main/debug.h @@ -54,5 +54,6 @@ typedef enum { DEBUG_RC_INTERPOLATION, DEBUG_VELOCITY, DEBUG_DTERM_FILTER, + DEBUG_ANGLERATE, DEBUG_COUNT } debugType_e; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index c3ba253b87..39f270e0df 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -75,10 +75,6 @@ bool isAirmodeActive(void) { return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); } -bool isSuperExpoActive(void) { - return (feature(FEATURE_SUPEREXPO_RATES)); -} - void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { #ifndef BLACKBOX UNUSED(adjustmentFunction); diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index aace839d9b..38c093f537 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -254,7 +254,6 @@ typedef struct adjustmentState_s { #define MAX_ADJUSTMENT_RANGE_COUNT 15 bool isAirmodeActive(void); -bool isSuperExpoActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index 89b46522f2..6f4b31ad98 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -45,12 +45,6 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo } } -int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate) -{ - float tmpf = tmp / 100.0f; - return (int16_t)((2500.0f + (float)expo * (tmpf * tmpf - 25.0f)) * tmpf * (float)(rate) / 2500.0f ); -} - int16_t rcLookupThrottle(int32_t tmp) { const int32_t tmp2 = tmp / 100; diff --git a/src/main/io/rc_curves.h b/src/main/io/rc_curves.h index 747a934df3..6b9161fb87 100644 --- a/src/main/io/rc_curves.h +++ b/src/main/io/rc_curves.h @@ -19,6 +19,5 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig); -int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate); int16_t rcLookupThrottle(int32_t tmp); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b434fa4300..3ce15c0f75 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -487,6 +487,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "RC_INTERPOLATION", "VELOCITY", "DFILTER", + "ANGLERATE", }; #ifdef OSD @@ -801,9 +802,9 @@ const clivalue_t valueTable[] = { { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, - { "roll_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, - { "pitch_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, - { "yaw_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, + { "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, + { "pitch_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, + { "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, { "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.airModeActivateThreshold, .config.minmax = {1000, 2000 } }, diff --git a/src/main/mw.c b/src/main/mw.c index d616015285..20580b66a9 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -173,22 +173,44 @@ bool isCalibrating() return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } +#define RC_RATE_INCREMENTAL 14.54f + float calculateSetpointRate(int axis, int16_t rc) { - float angleRate; + float angleRate, rcRate, rcSuperfactor, rcCommandf; + uint8_t rcExpo; - if (isSuperExpoActive()) { - rcInput[axis] = (axis == YAW) ? (ABS(rc) / (500.0f * (currentControlRateProfile->rcYawRate8 / 100.0f))) : (ABS(rc) / (500.0f * (currentControlRateProfile->rcRate8 / 100.0f))); - float rcFactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); - - angleRate = rcFactor * ((27 * rc) / 16.0f); + if (axis != YAW) { + rcExpo = currentControlRateProfile->rcExpo8; + rcRate = currentControlRateProfile->rcRate8 / 100.0f; } else { - angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f; + rcExpo = currentControlRateProfile->rcYawExpo8; + rcRate = currentControlRateProfile->rcYawRate8 / 100.0f; + } + + if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); + rcCommandf = rc / 500.0f; + rcInput[axis] = ABS(rcCommandf); + + if (rcExpo) { + float expof = rcExpo / 100.0f; + rcCommandf = rcCommandf * (expof * (rcInput[axis] * rcInput[axis] * rcInput[axis]) + rcInput[axis]*(1-expof)); + } + + angleRate = 200.0f * rcRate * rcCommandf; + + if (currentControlRateProfile->rates[axis]) { + rcSuperfactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); + angleRate *= rcSuperfactor; + } + + if (debugMode == DEBUG_ANGLERATE) { + debug[axis] = angleRate; } if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) - return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection + return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection else - return constrainf(angleRate / 4.1f, -1997.0f, 1997.0f); // Rate limit protection (deg/sec) + return constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) } void scaleRcCommandToFpvCamAngle(void) { @@ -298,14 +320,14 @@ static void updateRcCommands(void) } else { tmp = 0; } - rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcExpo8, currentControlRateProfile->rcRate8); + rcCommand[axis] = tmp; } else if (axis == YAW) { if (tmp > masterConfig.rcControlsConfig.yaw_deadband) { tmp -= masterConfig.rcControlsConfig.yaw_deadband; } else { tmp = 0; } - rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcYawExpo8, currentControlRateProfile->rcYawRate8) * -masterConfig.yaw_control_direction;; + rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; } if (rcData[axis] < masterConfig.rxConfig.midrc) { rcCommand[axis] = -rcCommand[axis]; From 24b4036968d001bd82d7e4663d88a4fc2b44a3ba Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 26 Aug 2016 02:05:28 +0200 Subject: [PATCH 383/456] Redefine rate implementation // new power expo // remove super expo feature --- src/main/build/debug.h | 1 + src/main/config/config.c | 2 +- src/main/config/config.h | 2 +- src/main/fc/mw.c | 44 +++++++++++++++++++++++++++++---------- src/main/fc/rc_controls.c | 4 ---- src/main/fc/rc_controls.h | 1 - src/main/fc/rc_curves.c | 6 ------ src/main/fc/rc_curves.h | 1 - src/main/io/serial_cli.c | 7 ++++--- 9 files changed, 40 insertions(+), 28 deletions(-) diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 87dfa0add9..42745e9e89 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -56,5 +56,6 @@ typedef enum { DEBUG_RC_INTERPOLATION, DEBUG_VELOCITY, DEBUG_DTERM_FILTER, + DEBUG_ANGLERATE, DEBUG_COUNT } debugType_e; diff --git a/src/main/config/config.c b/src/main/config/config.c index 1a0fc0330d..6516be9474 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -378,7 +378,7 @@ void createDefaultConfig(master_t *config) memset(config, 0, sizeof(master_t)); intFeatureClearAll(config); - intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES, config); + intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE , config); #ifdef DEFAULT_FEATURES intFeatureSet(DEFAULT_FEATURES, config); #endif diff --git a/src/main/config/config.h b/src/main/config/config.h index 3092e6a6a3..9878adc876 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -49,7 +49,7 @@ typedef enum { FEATURE_CHANNEL_FORWARDING = 1 << 20, FEATURE_TRANSPONDER = 1 << 21, FEATURE_AIRMODE = 1 << 22, - FEATURE_SUPEREXPO_RATES = 1 << 23, + //FEATURE_SUPEREXPO_RATES = 1 << 23, FEATURE_VTX = 1 << 24, FEATURE_RX_NRF24 = 1 << 25, FEATURE_SOFTSPI = 1 << 26, diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 1fde950586..37082c8bd2 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -174,22 +174,44 @@ bool isCalibrating() return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); } +#define RC_RATE_INCREMENTAL 14.54f + float calculateSetpointRate(int axis, int16_t rc) { - float angleRate; + float angleRate, rcRate, rcSuperfactor, rcCommandf; + uint8_t rcExpo; - if (isSuperExpoActive()) { - rcInput[axis] = (axis == YAW) ? (ABS(rc) / (500.0f * (currentControlRateProfile->rcYawRate8 / 100.0f))) : (ABS(rc) / (500.0f * (currentControlRateProfile->rcRate8 / 100.0f))); - float rcFactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); - - angleRate = rcFactor * ((27 * rc) / 16.0f); + if (axis != YAW) { + rcExpo = currentControlRateProfile->rcExpo8; + rcRate = currentControlRateProfile->rcRate8 / 100.0f; } else { - angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f; + rcExpo = currentControlRateProfile->rcYawExpo8; + rcRate = currentControlRateProfile->rcYawRate8 / 100.0f; + } + + if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); + rcCommandf = rc / 500.0f; + rcInput[axis] = ABS(rcCommandf); + + if (rcExpo) { + float expof = rcExpo / 100.0f; + rcCommandf = rcCommandf * (expof * (rcInput[axis] * rcInput[axis] * rcInput[axis]) + rcInput[axis]*(1-expof)); + } + + angleRate = 200.0f * rcRate * rcCommandf; + + if (currentControlRateProfile->rates[axis]) { + rcSuperfactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); + angleRate *= rcSuperfactor; + } + + if (debugMode == DEBUG_ANGLERATE) { + debug[axis] = angleRate; } if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) - return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection + return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection else - return constrainf(angleRate / 4.1f, -1997.0f, 1997.0f); // Rate limit protection (deg/sec) + return constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) } void scaleRcCommandToFpvCamAngle(void) { @@ -299,14 +321,14 @@ static void updateRcCommands(void) } else { tmp = 0; } - rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcExpo8, currentControlRateProfile->rcRate8); + rcCommand[axis] = tmp; } else if (axis == YAW) { if (tmp > masterConfig.rcControlsConfig.yaw_deadband) { tmp -= masterConfig.rcControlsConfig.yaw_deadband; } else { tmp = 0; } - rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcYawExpo8, currentControlRateProfile->rcYawRate8) * -masterConfig.yaw_control_direction;; + rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; } if (rcData[axis] < masterConfig.rxConfig.midrc) { rcCommand[axis] = -rcCommand[axis]; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 4d133bb32b..dd2903a6ce 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -76,10 +76,6 @@ bool isAirmodeActive(void) { return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); } -bool isSuperExpoActive(void) { - return (feature(FEATURE_SUPEREXPO_RATES)); -} - void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { #ifndef BLACKBOX UNUSED(adjustmentFunction); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 4d547931e9..27d423e67e 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -254,7 +254,6 @@ typedef struct adjustmentState_s { #define MAX_ADJUSTMENT_RANGE_COUNT 15 bool isAirmodeActive(void); -bool isSuperExpoActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig); diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c index 534676e6d8..55cdce5247 100644 --- a/src/main/fc/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -50,12 +50,6 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo } } -int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate) -{ - float tmpf = tmp / 100.0f; - return (int16_t)((2500.0f + (float)expo * (tmpf * tmpf - 25.0f)) * tmpf * (float)(rate) / 2500.0f ); -} - int16_t rcLookupThrottle(int32_t tmp) { const int32_t tmp2 = tmp / 100; diff --git a/src/main/fc/rc_curves.h b/src/main/fc/rc_curves.h index 406218d03b..9c1b361c2e 100644 --- a/src/main/fc/rc_curves.h +++ b/src/main/fc/rc_curves.h @@ -21,6 +21,5 @@ struct controlRateConfig_s; struct escAndServoConfig_s; void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct escAndServoConfig_s *escAndServoConfig); -int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate); int16_t rcLookupThrottle(int32_t tmp); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 98a81b221b..c4e9c46a6f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -489,6 +489,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "RC_INTERPOLATION", "VELOCITY", "DFILTER", + "ANGLERATE", }; #ifdef OSD @@ -803,9 +804,9 @@ const clivalue_t valueTable[] = { { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, - { "roll_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, - { "pitch_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, - { "yaw_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, + { "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, + { "pitch_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, + { "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, { "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, { "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, { "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.airModeActivateThreshold, .config.minmax = {1000, 2000 } }, From 43b1af438c1b87a4e7361a50ca845865c61cc3cf Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 27 Aug 2016 09:12:53 +1000 Subject: [PATCH 384/456] Remnants from merge removed --- src/main/flight/pid.c | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fa6c85f8e3..9cea9bad7a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -125,25 +125,6 @@ void initFilters(const pidProfile_t *pidProfile) } } - } else { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = lrintf(PTerm + ITerm); - - DTerm = 0.0f; // needed for blackbox - } else { - if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); - - axisPID[axis] = PTerm + ITerm; - - if (motorCount >= 4) { - int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); - - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); - } - - DTerm = 0; // needed for blackbox void pidSetController(pidControllerType_e type) { switch (type) { From a01c22d7cbb00d1a78e833655ef918a2e1296792 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 27 Aug 2016 09:27:29 +1000 Subject: [PATCH 385/456] Bringing F4 targets into line with F1 and F3 for linker and startup files. --- src/main/drivers/io.c | 1 - src/main/startup/startup_stm32f40xx.s | 11 +- src/main/startup/startup_stm32f411xe.s | 11 +- src/main/target/stm32_flash_f405.ld | 169 ++--------------------- src/main/target/stm32_flash_f405_opbl.ld | 164 +--------------------- src/main/target/stm32_flash_f411.ld | 162 +--------------------- src/main/target/stm32_flash_f411_opbl.ld | 162 +--------------------- 7 files changed, 27 insertions(+), 653 deletions(-) diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index 0ba9d9169b..d42932d144 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -65,7 +65,6 @@ const char * const resourceNames[RESOURCE_TOTAL_COUNT] = { "SDA", "SCK","MOSI","MISO","CS","BATTERY","RSSI","EXT","CURRENT" }; - ioRec_t* IO_Rec(IO_t io) { return io; diff --git a/src/main/startup/startup_stm32f40xx.s b/src/main/startup/startup_stm32f40xx.s index fa98fff962..b19a8ef54f 100644 --- a/src/main/startup/startup_stm32f40xx.s +++ b/src/main/startup/startup_stm32f40xx.s @@ -44,7 +44,6 @@ .global g_pfnVectors .global Default_Handler -.global irq_stack /* start address for the initialization values of the .data section. defined in linker script */ @@ -68,7 +67,7 @@ defined in linker script */ * @retval : None */ - .section .text.Reset_Handler + .section .text.Reset_Handler .weak Reset_Handler .type Reset_Handler, %function Reset_Handler: @@ -147,22 +146,18 @@ Infinite_Loop: .size Default_Handler, .-Default_Handler /****************************************************************************** * -* The minimal vector table for a Cortex M3. Note that the proper constructs +* The minimal vector table for a Cortex M4. Note that the proper constructs * must be placed on this to ensure that it ends up at physical address * 0x0000.0000. * *******************************************************************************/ - .section .irqstack,"aw",%progbits - irq_stack: - .space 1024 - .section .isr_vector,"a",%progbits .type g_pfnVectors, %object .size g_pfnVectors, .-g_pfnVectors g_pfnVectors: - .word irq_stack+1024 + .word _estack .word Reset_Handler .word NMI_Handler .word HardFault_Handler diff --git a/src/main/startup/startup_stm32f411xe.s b/src/main/startup/startup_stm32f411xe.s index f8c460a546..f3003a67eb 100644 --- a/src/main/startup/startup_stm32f411xe.s +++ b/src/main/startup/startup_stm32f411xe.s @@ -44,7 +44,6 @@ .global g_pfnVectors .global Default_Handler -.global irq_stack /* start address for the initialization values of the .data section. defined in linker script */ @@ -68,7 +67,7 @@ defined in linker script */ * @retval : None */ - .section .text.Reset_Handler + .section .text.Reset_Handler .weak Reset_Handler .type Reset_Handler, %function Reset_Handler: @@ -146,22 +145,18 @@ Infinite_Loop: .size Default_Handler, .-Default_Handler /****************************************************************************** * -* The minimal vector table for a Cortex M3. Note that the proper constructs +* The minimal vector table for a Cortex M4. Note that the proper constructs * must be placed on this to ensure that it ends up at physical address * 0x0000.0000. * *******************************************************************************/ - .section .irqstack,"aw",%progbits - irq_stack: - .space 1024 - .section .isr_vector,"a",%progbits .type g_pfnVectors, %object .size g_pfnVectors, .-g_pfnVectors g_pfnVectors: - .word irq_stack+1024 + .word _estack .word Reset_Handler .word NMI_Handler .word HardFault_Handler diff --git a/src/main/target/stm32_flash_f405.ld b/src/main/target/stm32_flash_f405.ld index eb2a7fbf3d..a792a296c6 100644 --- a/src/main/target/stm32_flash_f405.ld +++ b/src/main/target/stm32_flash_f405.ld @@ -1,30 +1,10 @@ /* ***************************************************************************** ** -** File : stm32_flash.ld +** File : stm32_flash_f405.ld ** -** Abstract : Linker script for STM32F407VG Device with -** 1024KByte FLASH, 128KByte RAM -** -** Set heap size, stack size and stack location according -** to application requirements. -** -** Set memory bank area and size if external memory is used. -** -** Target : STMicroelectronics STM32 -** -** Environment : Atollic TrueSTUDIO(R) -** -** Distribution: The file is distributed “as is,” without any warranty -** of any kind. -** -** (c)Copyright Atollic AB. -** You may use this file as-is or modify it according to the needs of your -** project. Distribution of this file (unmodified or modified) is not -** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the -** rights to distribute the assembled, compiled & linked contents of this -** file as part of an application binary file, provided that it is built -** using the Atollic TrueSTUDIO(R) toolchain. +** Abstract : Linker script for STM32F405RG Device with +** 1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM) ** ***************************************************************************** */ @@ -32,144 +12,15 @@ /* Entry Point */ ENTRY(Reset_Handler) -/* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0; /* required amount of heap */ -_Min_Stack_Size = 0x400; /* required amount of stack */ - /* Specify the memory areas */ MEMORY { - FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 0x0e0000 - 0x64 - INFOX (rx) : ORIGIN = 0x08000000 + 0x0e0000 - 0x64, LENGTH = 0x64 - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1M + RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K + BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K + MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } +/* note CCM could be used for stack */ -/* Section Definitions */ -SECTIONS -{ - /* The program code and other data goes into FLASH */ - .text : - { - PROVIDE (isr_vector_table_base = .); - KEEP(*(.isr_vector)) /* Startup code */ - . = ALIGN(4); - *(.text) /* .text sections (code) */ - *(.text*) /* .text* sections (code) */ - *(.rodata) /* .rodata sections (constants, strings, etc.) */ - *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ - *(.glue_7) /* glue arm to thumb code */ - *(.glue_7t) /* glue thumb to arm code */ - *(.eh_frame) - - KEEP (*(.init)) - KEEP (*(.fini)) - - . = ALIGN(4); - _etext = .; /* define a global symbols at end of code */ - } >FLASH - - - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH - .ARM : { - __exidx_start = .; - *(.ARM.exidx*) - __exidx_end = .; - } >FLASH - - .preinit_array : - { - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP (*(.preinit_array*)) - PROVIDE_HIDDEN (__preinit_array_end = .); - } >FLASH - .init_array : - { - PROVIDE_HIDDEN (__init_array_start = .); - KEEP (*(SORT(.init_array.*))) - KEEP (*(.init_array*)) - PROVIDE_HIDDEN (__init_array_end = .); - } >FLASH - .fini_array : - { - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP (*(.fini_array*)) - KEEP (*(SORT(.fini_array.*))) - PROVIDE_HIDDEN (__fini_array_end = .); - } >FLASH - - - /* used by the startup to initialize data */ - _sidata = .; - - /* - * Place the IRQ/bootstrap stack at the bottom of SRAM so that an overflow - * results in a hard fault. - */ - .istack (NOLOAD) : - { - . = ALIGN(4); - _irq_stack_end = . ; - *(.irqstack) - _irq_stack_top = . ; - } > RAM - - /* Initialized data sections goes into RAM, load LMA copy after code */ - .data : - { - . = ALIGN(4); - _sdata = .; /* create a global symbol at data start */ - *(.data) /* .data sections */ - *(.data*) /* .data* sections */ - - . = ALIGN(4); - _edata = .; /* define a global symbol at data end */ - } >RAM AT> FLASH - - /* Uninitialized data section */ - . = ALIGN(4); - .bss : - { - /* This is used by the startup in order to initialize the .bss secion */ - _sbss = .; /* define a global symbol at bss start */ - __bss_start__ = _sbss; - *(.bss) - *(.bss*) - *(COMMON) - - . = ALIGN(4); - _ebss = .; /* define a global symbol at bss end */ - __bss_end__ = _ebss; - } >RAM - - /* User_heap_stack section, used to check that there is enough RAM left */ - ._user_heap_stack : - { - . = ALIGN(4); - PROVIDE ( end = . ); - PROVIDE ( _end = . ); - . = . + _Min_Heap_Size; - . = . + _Min_Stack_Size; - . = ALIGN(4); - } >RAM - - /* MEMORY_bank1 section, code must be located here explicitly */ - /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ - .memory_b1_text : - { - *(.mb1text) /* .mb1text sections (code) */ - *(.mb1text*) /* .mb1text* sections (code) */ - *(.mb1rodata) /* read-only data (constants) */ - *(.mb1rodata*) - } >MEMORY_B1 - - /* Remove information from the standard libraries */ - /DISCARD/ : - { - libc.a ( * ) - libm.a ( * ) - libgcc.a ( * ) - } - - .ARM.attributes 0 : { *(.ARM.attributes) } -} +INCLUDE "stm32_flash.ld" diff --git a/src/main/target/stm32_flash_f405_opbl.ld b/src/main/target/stm32_flash_f405_opbl.ld index e1215c110c..a9e2b7dd84 100644 --- a/src/main/target/stm32_flash_f405_opbl.ld +++ b/src/main/target/stm32_flash_f405_opbl.ld @@ -1,30 +1,10 @@ /* ***************************************************************************** ** -** File : stm32_flash.ld +** File : stm32_flash_f405.ld ** -** Abstract : Linker script for STM32F407VG Device with -** 1024KByte FLASH, 128KByte RAM -** -** Set heap size, stack size and stack location according -** to application requirements. -** -** Set memory bank area and size if external memory is used. -** -** Target : STMicroelectronics STM32 -** -** Environment : Atollic TrueSTUDIO(R) -** -** Distribution: The file is distributed “as is,” without any warranty -** of any kind. -** -** (c)Copyright Atollic AB. -** You may use this file as-is or modify it according to the needs of your -** project. Distribution of this file (unmodified or modified) is not -** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the -** rights to distribute the assembled, compiled & linked contents of this -** file as part of an application binary file, provided that it is built -** using the Atollic TrueSTUDIO(R) toolchain. +** Abstract : Linker script for STM32F405RG Device with +** 1024KByte FLASH, 128KByte RAM 64KByte CCM (RAM) ** ***************************************************************************** */ @@ -32,10 +12,6 @@ /* Entry Point */ ENTRY(Reset_Handler) -/* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0x2000; /* required amount of heap */ -_Min_Stack_Size = 0x4000; /* required amount of stack */ - /* Specify the memory areas */ /* @@ -48,139 +24,9 @@ _Min_Stack_Size = 0x4000; /* required amount of stack */ MEMORY { FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 0x000A0000 - CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K + CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 0x00020000 MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } -REGION_ALIAS("CCSRAM", CCM); -REGION_ALIAS("SRAM", RAM); - -/* Section Definitions */ -SECTIONS -{ - /* The program code and other data goes into FLASH */ - .text : - { - PROVIDE (isr_vector_table_base = .); - KEEP(*(.isr_vector)) /* Startup code */ - . = ALIGN(4); - *(.text) /* .text sections (code) */ - *(.text*) /* .text* sections (code) */ - *(.rodata) /* .rodata sections (constants, strings, etc.) */ - *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ - *(.glue_7) /* glue arm to thumb code */ - *(.glue_7t) /* glue thumb to arm code */ - *(.eh_frame) - - KEEP (*(.init)) - KEEP (*(.fini)) - - . = ALIGN(4); - _etext = .; /* define a global symbols at end of code */ - } >FLASH - - - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH - .ARM : { - __exidx_start = .; - *(.ARM.exidx*) - __exidx_end = .; - } >FLASH - - .preinit_array : - { - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP (*(.preinit_array*)) - PROVIDE_HIDDEN (__preinit_array_end = .); - } >FLASH - .init_array : - { - PROVIDE_HIDDEN (__init_array_start = .); - KEEP (*(SORT(.init_array.*))) - KEEP (*(.init_array*)) - PROVIDE_HIDDEN (__init_array_end = .); - } >FLASH - .fini_array : - { - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP (*(.fini_array*)) - KEEP (*(SORT(.fini_array.*))) - PROVIDE_HIDDEN (__fini_array_end = .); - } >FLASH - - - /* used by the startup to initialize data */ - _sidata = .; - - /* - * Place the IRQ/bootstrap stack at the bottom of SRAM so that an overflow - * results in a hard fault. - */ - .istack (NOLOAD) : - { - . = ALIGN(4); - _irq_stack_end = . ; - *(.irqstack) - _irq_stack_top = . ; - } > RAM - - /* Initialized data sections goes into RAM, load LMA copy after code */ - .data : - { - . = ALIGN(4); - _sdata = .; /* create a global symbol at data start */ - *(.data) /* .data sections */ - *(.data*) /* .data* sections */ - - . = ALIGN(4); - _edata = .; /* define a global symbol at data end */ - } >RAM AT> FLASH - - /* Uninitialized data section */ - . = ALIGN(4); - .bss : - { - /* This is used by the startup in order to initialize the .bss secion */ - _sbss = .; /* define a global symbol at bss start */ - __bss_start__ = _sbss; - *(.bss) - *(.bss*) - *(COMMON) - - . = ALIGN(4); - _ebss = .; /* define a global symbol at bss end */ - __bss_end__ = _ebss; - } >RAM - - /* User_heap_stack section, used to check that there is enough RAM left */ - ._user_heap_stack : - { - . = ALIGN(4); - PROVIDE ( end = . ); - PROVIDE ( _end = . ); - . = . + _Min_Heap_Size; - . = . + _Min_Stack_Size; - . = ALIGN(4); - } >RAM - - /* MEMORY_bank1 section, code must be located here explicitly */ - /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ - .memory_b1_text : - { - *(.mb1text) /* .mb1text sections (code) */ - *(.mb1text*) /* .mb1text* sections (code) */ - *(.mb1rodata) /* read-only data (constants) */ - *(.mb1rodata*) - } >MEMORY_B1 - - /* Remove information from the standard libraries */ - /DISCARD/ : - { - libc.a ( * ) - libm.a ( * ) - libgcc.a ( * ) - } - - .ARM.attributes 0 : { *(.ARM.attributes) } -} +INCLUDE "stm32_flash.ld" \ No newline at end of file diff --git a/src/main/target/stm32_flash_f411.ld b/src/main/target/stm32_flash_f411.ld index a3d834a188..d145ff3afc 100644 --- a/src/main/target/stm32_flash_f411.ld +++ b/src/main/target/stm32_flash_f411.ld @@ -1,49 +1,24 @@ /* ***************************************************************************** ** -** File : stm32_flash.ld +** File : stm32_flash_f411.ld ** ** Abstract : Linker script for STM32F11 Device with ** 512KByte FLASH, 128KByte RAM ** -** Set heap size, stack size and stack location according -** to application requirements. -** -** Set memory bank area and size if external memory is used. -** -** Target : STMicroelectronics STM32 -** -** Environment : Atollic TrueSTUDIO(R) -** -** Distribution: The file is distributed “as is,” without any warranty -** of any kind. -** -** (c)Copyright Atollic AB. -** You may use this file as-is or modify it according to the needs of your -** project. Distribution of this file (unmodified or modified) is not -** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the -** rights to distribute the assembled, compiled & linked contents of this -** file as part of an application binary file, provided that it is built -** using the Atollic TrueSTUDIO(R) toolchain. -** ***************************************************************************** */ /* Entry Point */ ENTRY(Reset_Handler) -/* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0x2000; /* required amount of heap */ -_Min_Stack_Size = 0x4000; /* required amount of stack */ - -/* Specify the memory areas */ - /* 0x08000000 to 0x08080000 512kb full flash, 0x08000000 to 0x08060000 384kb firmware, 0x08060000 to 0x08080000 128kb config, */ +/* Specify the memory areas */ MEMORY { FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 0x00080000 @@ -51,135 +26,4 @@ MEMORY MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } -REGION_ALIAS("CCSRAM", RAM); -REGION_ALIAS("SRAM", RAM); - - -/* Section Definitions */ -SECTIONS -{ - /* The program code and other data goes into FLASH */ - .text : - { - PROVIDE (isr_vector_table_base = .); - KEEP(*(.isr_vector)) /* Startup code */ - . = ALIGN(4); - *(.text) /* .text sections (code) */ - *(.text*) /* .text* sections (code) */ - *(.rodata) /* .rodata sections (constants, strings, etc.) */ - *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ - *(.glue_7) /* glue arm to thumb code */ - *(.glue_7t) /* glue thumb to arm code */ - *(.eh_frame) - - KEEP (*(.init)) - KEEP (*(.fini)) - - . = ALIGN(4); - _etext = .; /* define a global symbols at end of code */ - } >FLASH - - - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH - .ARM : { - __exidx_start = .; - *(.ARM.exidx*) - __exidx_end = .; - } >FLASH - - .preinit_array : - { - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP (*(.preinit_array*)) - PROVIDE_HIDDEN (__preinit_array_end = .); - } >FLASH - .init_array : - { - PROVIDE_HIDDEN (__init_array_start = .); - KEEP (*(SORT(.init_array.*))) - KEEP (*(.init_array*)) - PROVIDE_HIDDEN (__init_array_end = .); - } >FLASH - .fini_array : - { - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP (*(.fini_array*)) - KEEP (*(SORT(.fini_array.*))) - PROVIDE_HIDDEN (__fini_array_end = .); - } >FLASH - - - /* used by the startup to initialize data */ - _sidata = .; - - /* - * Place the IRQ/bootstrap stack at the bottom of SRAM so that an overflow - * results in a hard fault. - */ - .istack (NOLOAD) : - { - . = ALIGN(4); - _irq_stack_end = . ; - *(.irqstack) - _irq_stack_top = . ; - } > RAM - - /* Initialized data sections goes into RAM, load LMA copy after code */ - .data : - { - . = ALIGN(4); - _sdata = .; /* create a global symbol at data start */ - *(.data) /* .data sections */ - *(.data*) /* .data* sections */ - - . = ALIGN(4); - _edata = .; /* define a global symbol at data end */ - } >RAM AT> FLASH - - /* Uninitialized data section */ - . = ALIGN(4); - .bss : - { - /* This is used by the startup in order to initialize the .bss secion */ - _sbss = .; /* define a global symbol at bss start */ - __bss_start__ = _sbss; - *(.bss) - *(.bss*) - *(COMMON) - - . = ALIGN(4); - _ebss = .; /* define a global symbol at bss end */ - __bss_end__ = _ebss; - } >RAM - - /* User_heap_stack section, used to check that there is enough RAM left */ - ._user_heap_stack : - { - . = ALIGN(4); - PROVIDE ( end = . ); - PROVIDE ( _end = . ); - . = . + _Min_Heap_Size; - . = . + _Min_Stack_Size; - . = ALIGN(4); - } >RAM - - /* MEMORY_bank1 section, code must be located here explicitly */ - /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ - .memory_b1_text : - { - *(.mb1text) /* .mb1text sections (code) */ - *(.mb1text*) /* .mb1text* sections (code) */ - *(.mb1rodata) /* read-only data (constants) */ - *(.mb1rodata*) - } >MEMORY_B1 - - /* Remove information from the standard libraries */ - /DISCARD/ : - { - libc.a ( * ) - libm.a ( * ) - libgcc.a ( * ) - } - - .ARM.attributes 0 : { *(.ARM.attributes) } -} +INCLUDE "stm32_flash.ld" diff --git a/src/main/target/stm32_flash_f411_opbl.ld b/src/main/target/stm32_flash_f411_opbl.ld index 7ba674a6d5..5787e6d20b 100644 --- a/src/main/target/stm32_flash_f411_opbl.ld +++ b/src/main/target/stm32_flash_f411_opbl.ld @@ -1,43 +1,17 @@ /* ***************************************************************************** ** -** File : stm32_flash.ld +** File : stm32_flash_f411.ld ** ** Abstract : Linker script for STM32F11 Device with ** 512KByte FLASH, 128KByte RAM ** -** Set heap size, stack size and stack location according -** to application requirements. -** -** Set memory bank area and size if external memory is used. -** -** Target : STMicroelectronics STM32 -** -** Environment : Atollic TrueSTUDIO(R) -** -** Distribution: The file is distributed “as is,” without any warranty -** of any kind. -** -** (c)Copyright Atollic AB. -** You may use this file as-is or modify it according to the needs of your -** project. Distribution of this file (unmodified or modified) is not -** permitted. Atollic AB permit registered Atollic TrueSTUDIO(R) users the -** rights to distribute the assembled, compiled & linked contents of this -** file as part of an application binary file, provided that it is built -** using the Atollic TrueSTUDIO(R) toolchain. -** ***************************************************************************** */ /* Entry Point */ ENTRY(Reset_Handler) -/* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0x2000; /* required amount of heap */ -_Min_Stack_Size = 0x4000; /* required amount of stack */ - -/* Specify the memory areas */ - /* 0x08000000 to 0x08080000 512kb full flash, 0x08000000 to 0x08010000 64kb OPBL, @@ -45,6 +19,7 @@ _Min_Stack_Size = 0x4000; /* required amount of stack */ 0x08060000 to 0x08080000 128kb config, */ +/* Specify the memory areas */ MEMORY { FLASH (rx) : ORIGIN = 0x08010000, LENGTH = 0x00070000 @@ -52,135 +27,4 @@ MEMORY MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } -REGION_ALIAS("CCSRAM", RAM); -REGION_ALIAS("SRAM", RAM); - - -/* Section Definitions */ -SECTIONS -{ - /* The program code and other data goes into FLASH */ - .text : - { - PROVIDE (isr_vector_table_base = .); - KEEP(*(.isr_vector)) /* Startup code */ - . = ALIGN(4); - *(.text) /* .text sections (code) */ - *(.text*) /* .text* sections (code) */ - *(.rodata) /* .rodata sections (constants, strings, etc.) */ - *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ - *(.glue_7) /* glue arm to thumb code */ - *(.glue_7t) /* glue thumb to arm code */ - *(.eh_frame) - - KEEP (*(.init)) - KEEP (*(.fini)) - - . = ALIGN(4); - _etext = .; /* define a global symbols at end of code */ - } >FLASH - - - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH - .ARM : { - __exidx_start = .; - *(.ARM.exidx*) - __exidx_end = .; - } >FLASH - - .preinit_array : - { - PROVIDE_HIDDEN (__preinit_array_start = .); - KEEP (*(.preinit_array*)) - PROVIDE_HIDDEN (__preinit_array_end = .); - } >FLASH - .init_array : - { - PROVIDE_HIDDEN (__init_array_start = .); - KEEP (*(SORT(.init_array.*))) - KEEP (*(.init_array*)) - PROVIDE_HIDDEN (__init_array_end = .); - } >FLASH - .fini_array : - { - PROVIDE_HIDDEN (__fini_array_start = .); - KEEP (*(.fini_array*)) - KEEP (*(SORT(.fini_array.*))) - PROVIDE_HIDDEN (__fini_array_end = .); - } >FLASH - - - /* used by the startup to initialize data */ - _sidata = .; - - /* - * Place the IRQ/bootstrap stack at the bottom of SRAM so that an overflow - * results in a hard fault. - */ - .istack (NOLOAD) : - { - . = ALIGN(4); - _irq_stack_end = . ; - *(.irqstack) - _irq_stack_top = . ; - } > RAM - - /* Initialized data sections goes into RAM, load LMA copy after code */ - .data : - { - . = ALIGN(4); - _sdata = .; /* create a global symbol at data start */ - *(.data) /* .data sections */ - *(.data*) /* .data* sections */ - - . = ALIGN(4); - _edata = .; /* define a global symbol at data end */ - } >RAM AT> FLASH - - /* Uninitialized data section */ - . = ALIGN(4); - .bss : - { - /* This is used by the startup in order to initialize the .bss secion */ - _sbss = .; /* define a global symbol at bss start */ - __bss_start__ = _sbss; - *(.bss) - *(.bss*) - *(COMMON) - - . = ALIGN(4); - _ebss = .; /* define a global symbol at bss end */ - __bss_end__ = _ebss; - } >RAM - - /* User_heap_stack section, used to check that there is enough RAM left */ - ._user_heap_stack : - { - . = ALIGN(4); - PROVIDE ( end = . ); - PROVIDE ( _end = . ); - . = . + _Min_Heap_Size; - . = . + _Min_Stack_Size; - . = ALIGN(4); - } >RAM - - /* MEMORY_bank1 section, code must be located here explicitly */ - /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ - .memory_b1_text : - { - *(.mb1text) /* .mb1text sections (code) */ - *(.mb1text*) /* .mb1text* sections (code) */ - *(.mb1rodata) /* read-only data (constants) */ - *(.mb1rodata*) - } >MEMORY_B1 - - /* Remove information from the standard libraries */ - /DISCARD/ : - { - libc.a ( * ) - libm.a ( * ) - libgcc.a ( * ) - } - - .ARM.attributes 0 : { *(.ARM.attributes) } -} +INCLUDE "stm32_flash.ld" From 9db76414b4b80bb6b547d174110fc0189b48fe8c Mon Sep 17 00:00:00 2001 From: J Blackman Date: Sat, 27 Aug 2016 09:55:01 +1000 Subject: [PATCH 386/456] Update README.md --- README.md | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/README.md b/README.md index 9b88d1fbe6..2805c60527 100644 --- a/README.md +++ b/README.md @@ -116,3 +116,40 @@ https://travis-ci.org/betaflight/betaflight https://github.com/betaflight/betaflight/releases +## Open Source Notice + +Betaflight is software that is **open source** and is available free of charge without warranty to all users. + +Betaflight is forked from Cleanflight, so thanks goes to all those whom have contributed to Cleanflight and its origins. + +Origins for this fork (Thanks!): +* **Alexinparis** (for MultiWii), +* **timecop** (for Baseflight), +* **Dominic Clifton** (for Cleanflight), and +* **Sambas** (for the original STM32F4 port). + +The Baseflight Configurator is forked from Cleanflight Configurator and its origins. + +Origins for Betaflight Configurator: +* **Dominic Clifton** (for Cleanflight configurator), and +* **ctn** (for the original Configurator). + +Big thanks to current and past contributors: +Budden, Martin (martinbudden) +Bardwell, Joshua (joshuabardwell) +Blackman, Jason (blckmn) +ctzsnooze +Höglund, Anders (andershoglund) +Ledvin, Peter (ledvinap) **IO code awesomeness!** +kc10kevin +Keeble, Gary (MadmanK) +Keller, Michael (mikeller) +Kravcov, Albert (skaman82) +MJ666 +Nathan (nathantsoi) +ravnav +sambas +savaga +StÃ¥lheim, Anton (KiteAnton) + +And many many others who haven't been mentioned.... From 53a225059b25ec11d6afff7661a13070de5237ce Mon Sep 17 00:00:00 2001 From: J Blackman Date: Sat, 27 Aug 2016 09:57:11 +1000 Subject: [PATCH 387/456] Update README.md --- README.md | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/README.md b/README.md index 2805c60527..9126c51873 100644 --- a/README.md +++ b/README.md @@ -128,28 +128,28 @@ Origins for this fork (Thanks!): * **Dominic Clifton** (for Cleanflight), and * **Sambas** (for the original STM32F4 port). -The Baseflight Configurator is forked from Cleanflight Configurator and its origins. +The Betaflight Configurator is forked from Cleanflight Configurator and its origins. Origins for Betaflight Configurator: * **Dominic Clifton** (for Cleanflight configurator), and * **ctn** (for the original Configurator). Big thanks to current and past contributors: -Budden, Martin (martinbudden) -Bardwell, Joshua (joshuabardwell) -Blackman, Jason (blckmn) -ctzsnooze -Höglund, Anders (andershoglund) -Ledvin, Peter (ledvinap) **IO code awesomeness!** -kc10kevin -Keeble, Gary (MadmanK) -Keller, Michael (mikeller) -Kravcov, Albert (skaman82) -MJ666 -Nathan (nathantsoi) -ravnav -sambas -savaga -StÃ¥lheim, Anton (KiteAnton) +* Budden, Martin (martinbudden) +* Bardwell, Joshua (joshuabardwell) +* Blackman, Jason (blckmn) +* ctzsnooze +* Höglund, Anders (andershoglund) +* Ledvin, Peter (ledvinap) **IO code awesomeness!** +* kc10kevin +* Keeble, Gary (MadmanK) +* Keller, Michael (mikeller) **Configurator brilliance** +* Kravcov, Albert (skaman82) **Configurator brilliance** +* MJ666 +* Nathan (nathantsoi) +* ravnav +* sambas **bringing us the F4** +* savaga +* StÃ¥lheim, Anton (KiteAnton) And many many others who haven't been mentioned.... From 50f7ab711ee1fe21766e8bed344839c41fc031fe Mon Sep 17 00:00:00 2001 From: J Blackman Date: Sat, 27 Aug 2016 09:58:07 +1000 Subject: [PATCH 388/456] Update README.md --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 9126c51873..6fb992a35a 100644 --- a/README.md +++ b/README.md @@ -140,15 +140,15 @@ Big thanks to current and past contributors: * Blackman, Jason (blckmn) * ctzsnooze * Höglund, Anders (andershoglund) -* Ledvin, Peter (ledvinap) **IO code awesomeness!** +* Ledvin, Peter (ledvinap) - **IO code awesomeness!** * kc10kevin * Keeble, Gary (MadmanK) -* Keller, Michael (mikeller) **Configurator brilliance** -* Kravcov, Albert (skaman82) **Configurator brilliance** +* Keller, Michael (mikeller) - **Configurator brilliance** +* Kravcov, Albert (skaman82) - **Configurator brilliance** * MJ666 * Nathan (nathantsoi) * ravnav -* sambas **bringing us the F4** +* sambas - **bringing us the F4** * savaga * StÃ¥lheim, Anton (KiteAnton) From 50119a43eae3877d581c859a95b0b42a312ab246 Mon Sep 17 00:00:00 2001 From: J Blackman Date: Sat, 27 Aug 2016 12:47:51 +1000 Subject: [PATCH 389/456] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 6fb992a35a..471729b398 100644 --- a/README.md +++ b/README.md @@ -116,7 +116,7 @@ https://travis-ci.org/betaflight/betaflight https://github.com/betaflight/betaflight/releases -## Open Source Notice +## Open Source / Contributors Betaflight is software that is **open source** and is available free of charge without warranty to all users. From f208a7772398f0cc76a0f4286f76e2f5c1900dfa Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 27 Aug 2016 23:20:44 +1000 Subject: [PATCH 390/456] Repurpose some outputs on the SPRACINGF3EVO for Servo outputs. --- src/main/target/SPRACINGF3EVO/target.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 843ad3d9aa..d128f7e8b3 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -30,10 +30,10 @@ const uint16_t multiPPM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), 0xFFFF }; @@ -44,10 +44,10 @@ const uint16_t multiPWM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), 0xFFFF }; From a8bfa54ae07cdfc37e3d3b81ac351629dc5c94a2 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 28 Aug 2016 01:07:01 +1200 Subject: [PATCH 391/456] Changed CLI parser to allow multiple whitespaces. Fixes problem with 'mmix' dump. --- src/main/io/serial_cli.c | 108 +++++++++++++++++++++------------------ 1 file changed, 59 insertions(+), 49 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3ce15c0f75..b6c0aa0841 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -968,14 +968,24 @@ static void cliShowArgumentRangeError(char *name, int min, int max) cliPrintf("%s must be between %d and %d\r\n", name, min, max); } +static char *nextArg(char *currentArg) +{ + char *ptr = strchr(currentArg, ' '); + while (ptr && *ptr == ' ') { + ptr++; + } + + return ptr; +} + static char *processChannelRangeArgs(char *ptr, channelRange_t *range, uint8_t *validArgumentCount) { int val; for (uint32_t argIndex = 0; argIndex < 2; argIndex++) { - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); val = CHANNEL_VALUE_TO_STEP(val); if (val >= MIN_MODE_RANGE_STEP && val <= MAX_MODE_RANGE_STEP) { if (argIndex == 0) { @@ -1058,9 +1068,9 @@ static void cliRxFail(char *cmdline) rxFailsafeChannelMode_e mode = channelFailsafeConfiguration->mode; bool requireValue = channelFailsafeConfiguration->mode == RX_FAILSAFE_MODE_SET; - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - char *p = strchr(rxFailsafeModeCharacters, *(++ptr)); + char *p = strchr(rxFailsafeModeCharacters, *(ptr)); if (p) { uint8_t requestedMode = p - rxFailsafeModeCharacters; mode = rxFailsafeModesTable[type][requestedMode]; @@ -1074,13 +1084,13 @@ static void cliRxFail(char *cmdline) requireValue = mode == RX_FAILSAFE_MODE_SET; - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { if (!requireValue) { cliShowParseError(); return; } - value = atoi(++ptr); + value = atoi(ptr); value = CHANNEL_VALUE_TO_RXFAIL_STEP(value); if (value > MAX_RXFAIL_RANGE_STEP) { cliPrint("Value out of range\r\n"); @@ -1164,17 +1174,17 @@ static void cliAux(char *cmdline) if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) { modeActivationCondition_t *mac = &masterConfig.modeActivationConditions[i]; uint8_t validArgumentCount = 0; - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < CHECKBOX_ITEM_COUNT) { mac->modeId = val; validArgumentCount++; } } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { mac->auxChannelIndex = val; validArgumentCount++; @@ -1254,20 +1264,20 @@ static void cliSerial(char *cmdline) validArgumentCount++; } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); portConfig.functionMask = val & 0xFFFF; validArgumentCount++; } for (i = 0; i < 4; i ++) { - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (!ptr) { break; } - val = atoi(++ptr); + val = atoi(ptr); uint8_t baudRateIndex = lookupBaudRateIndex(val); if (baudRates[baudRateIndex] != (uint32_t) val) { @@ -1434,17 +1444,17 @@ static void cliAdjustmentRange(char *cmdline) adjustmentRange_t *ar = &masterConfig.adjustmentRanges[i]; uint8_t validArgumentCount = 0; - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) { ar->adjustmentIndex = val; validArgumentCount++; } } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { ar->auxChannelIndex = val; validArgumentCount++; @@ -1453,17 +1463,17 @@ static void cliAdjustmentRange(char *cmdline) ptr = processChannelRangeArgs(ptr, &ar->range, &validArgumentCount); - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < ADJUSTMENT_FUNCTION_COUNT) { ar->adjustmentFunction = val; validArgumentCount++; } } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { ar->auxSwitchChannelIndex = val; validArgumentCount++; @@ -1531,9 +1541,9 @@ static void cliMotorMix(char *cmdline) for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) masterConfig.customMotorMixer[i].throttle = 0.0f; } else if (strncasecmp(cmdline, "load", 4) == 0) { - ptr = strchr(cmdline, ' '); + ptr = nextArg(cmdline); if (ptr) { - len = strlen(++ptr); + len = strlen(ptr); for (uint32_t i = 0; ; i++) { if (mixerNames[i] == NULL) { cliPrint("Invalid name\r\n"); @@ -1551,30 +1561,30 @@ static void cliMotorMix(char *cmdline) ptr = cmdline; uint32_t i = atoi(ptr); // get motor number if (i < MAX_SUPPORTED_MOTORS) { - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - masterConfig.customMotorMixer[i].throttle = fastA2F(++ptr); + masterConfig.customMotorMixer[i].throttle = fastA2F(ptr); check++; } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - masterConfig.customMotorMixer[i].roll = fastA2F(++ptr); + masterConfig.customMotorMixer[i].roll = fastA2F(ptr); check++; } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - masterConfig.customMotorMixer[i].pitch = fastA2F(++ptr); + masterConfig.customMotorMixer[i].pitch = fastA2F(ptr); check++; } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - masterConfig.customMotorMixer[i].yaw = fastA2F(++ptr); + masterConfig.customMotorMixer[i].yaw = fastA2F(ptr); check++; } if (check != 4) { cliShowParseError(); } else { - cliMotorMix(""); + printMotorMix(DUMP_MASTER, NULL); } } else { cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS - 1); @@ -1622,15 +1632,15 @@ static void cliRxRange(char *cmdline) if (i >= 0 && i < NON_AUX_CHANNEL_COUNT) { int rangeMin, rangeMax; - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - rangeMin = atoi(++ptr); + rangeMin = atoi(ptr); validArgumentCount++; } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - rangeMax = atoi(++ptr); + rangeMax = atoi(ptr); validArgumentCount++; } @@ -1680,8 +1690,8 @@ static void cliLed(char *cmdline) ptr = cmdline; i = atoi(ptr); if (i < LED_MAX_STRIP_LENGTH) { - ptr = strchr(cmdline, ' '); - if (!parseLedStripConfig(i, ++ptr)) { + ptr = nextArg(cmdline); + if (!parseLedStripConfig(i, ptr)) { cliShowParseError(); } } else { @@ -1728,8 +1738,8 @@ static void cliColor(char *cmdline) ptr = cmdline; i = atoi(ptr); if (i < LED_CONFIGURABLE_COLOR_COUNT) { - ptr = strchr(cmdline, ' '); - if (!parseColor(i, ++ptr)) { + ptr = nextArg(cmdline); + if (!parseColor(i, ptr)) { cliShowParseError(); } } else { @@ -1982,9 +1992,9 @@ static void cliServoMix(char *cmdline) masterConfig.servoConf[i].reversedSources = 0; } } else if (strncasecmp(cmdline, "load", 4) == 0) { - ptr = strchr(cmdline, ' '); + ptr = nextArg(cmdline); if (ptr) { - len = strlen(++ptr); + len = strlen(ptr); for (uint32_t i = 0; ; i++) { if (mixerNames[i] == NULL) { cliPrintf("Invalid name\r\n"); @@ -2280,25 +2290,25 @@ static void cliVtx(char *cmdline) if (i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT) { vtxChannelActivationCondition_t *cac = &masterConfig.vtxChannelActivationConditions[i]; uint8_t validArgumentCount = 0; - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= 0 && val < MAX_AUX_CHANNEL_COUNT) { cac->auxChannelIndex = val; validArgumentCount++; } } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= VTX_BAND_MIN && val <= VTX_BAND_MAX) { cac->band = val; validArgumentCount++; } } - ptr = strchr(ptr, ' '); + ptr = nextArg(ptr); if (ptr) { - val = atoi(++ptr); + val = atoi(ptr); if (val >= VTX_CHANNEL_MIN && val <= VTX_CHANNEL_MAX) { cac->channel = val; validArgumentCount++; From 75e07c1bb7a2aa2ce90edbd66da379dc5b006e18 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 27 Aug 2016 20:22:09 +0200 Subject: [PATCH 392/456] Cleanup super expo feature --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b6c0aa0841..9509d0caa2 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -220,7 +220,7 @@ static const char * const featureNames[] = { "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD", - "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO_RATES", NULL + "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", NULL }; // sync this with rxFailsafeChannelMode_e From 85222c80fab448ecdba07401708f69c181424867 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Sun, 28 Aug 2016 23:53:10 +0200 Subject: [PATCH 393/456] New files for target RACEBASE FC from shop.rotoracer.com --- src/main/target/RACEBASE/config.c | 86 ++++++++++++++++++++ src/main/target/RACEBASE/readme.txt | 0 src/main/target/RACEBASE/target.c | 59 ++++++++++++++ src/main/target/RACEBASE/target.h | 120 ++++++++++++++++++++++++++++ src/main/target/RACEBASE/target.mk | 12 +++ 5 files changed, 277 insertions(+) create mode 100755 src/main/target/RACEBASE/config.c create mode 100755 src/main/target/RACEBASE/readme.txt create mode 100755 src/main/target/RACEBASE/target.c create mode 100755 src/main/target/RACEBASE/target.h create mode 100755 src/main/target/RACEBASE/target.mk diff --git a/src/main/target/RACEBASE/config.c b/src/main/target/RACEBASE/config.c new file mode 100755 index 0000000000..95e678acac --- /dev/null +++ b/src/main/target/RACEBASE/config.c @@ -0,0 +1,86 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include + +#include "build_config.h" + +#include "blackbox/blackbox_io.h" + +#include "common/color.h" +#include "common/axis.h" +#include "common/filter.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" +#include "drivers/pwm_output.h" +#include "drivers/max7456.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "io/rc_controls.h" +#include "io/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "config/config_profile.h" +#include "config/config_master.h" + +// alternative defaults settings for COLIBRI RACE targets +void targetConfiguration(master_t *config) { + config->rxConfig.sbus_inversion = 0; + config->rxConfig.rssi_scale = 19; + config->escAndServoConfig.minthrottle = 1060; + config->escAndServoConfig.maxthrottle = 1940; + config->serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; + config->rxConfig.serialrx_provider = SERIALRX_SBUS; +} diff --git a/src/main/target/RACEBASE/readme.txt b/src/main/target/RACEBASE/readme.txt new file mode 100755 index 0000000000..e69de29bb2 diff --git a/src/main/target/RACEBASE/target.c b/src/main/target/RACEBASE/target.c new file mode 100755 index 0000000000..0b6a8f1695 --- /dev/null +++ b/src/main/target/RACEBASE/target.c @@ -0,0 +1,59 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + 0xFFFF +}; + +const uint16_t airPWM[] = { + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, + + { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM2 - PC6 + { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM3 - PC7 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PMW4 - PC8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2}, // PWM5 - PC9 +}; + + diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h new file mode 100755 index 0000000000..d36dd41cfb --- /dev/null +++ b/src/main/target/RACEBASE/target.h @@ -0,0 +1,120 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "RBFC" +#define TARGET_CONFIG + +#define LED0 PB3 +#define LED0_INVERTED + +#define LED1 PB4 +#define LED1_INVERTED + +#define BEEPER PA12 +#define BEEPER_INVERTED + +#define USE_EXTI +#define MPU_INT_EXTI PC5 +#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + + +#define MPU6000_CS_PIN PB5 +#define MPU6000_SPI_INSTANCE SPI2 + +#define GYRO +#define USE_GYRO_SPI_MPU6000 + +#define ACC +#define USE_ACC_SPI_MPU6000 + +#define ACC_MPU6000_ALIGN CW90_DEG +#define GYRO_MPU6000_ALIGN CW90_DEG + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define SERIAL_PORT_COUNT 3 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_SPI +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN PA7 + +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define BOARD_HAS_VOLTAGE_DIVIDER +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PA6 + +#define LED_STRIP +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_PIN PA8 +#define WS2811_TIMER TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define OSD + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT + +#define USE_SERVOS + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_VBAT | FEATURE_RSSI_ADC) + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// IO - stm32f303cc in 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(5)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) + +#define USABLE_TIMER_CHANNEL_COUNT 5 +#define USED_TIMERS (TIM_N(2) | TIM_N(4)) +#define TIMER_APB1_PERIPHERALS (RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM4) +#define TIMER_AHB_PERIPHERALS (RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB) + + diff --git a/src/main/target/RACEBASE/target.mk b/src/main/target/RACEBASE/target.mk new file mode 100755 index 0000000000..6e4711c5da --- /dev/null +++ b/src/main/target/RACEBASE/target.mk @@ -0,0 +1,12 @@ +F3_TARGETS += $(TARGET) + +FEATURES = ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/max7456.c \ + io/osd.c From 7ff10640f5d2042f6d79d5578c35d92f4d60accb Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Mon, 29 Aug 2016 15:48:31 +0200 Subject: [PATCH 394/456] Wrong check in MAX7456 status register --- src/main/drivers/max7456.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) mode change 100644 => 100755 src/main/drivers/max7456.c diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c old mode 100644 new mode 100755 index 82f3f29af9..9fcf45e033 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -296,7 +296,7 @@ void max7456_write_nvm(uint8_t char_address, uint8_t *font_data) { max7456_send(MAX7456ADD_CMM, WRITE_NVR); // wait until bit 5 in the status register returns to 0 (12ms) - while ((spiTransferByte(MAX7456_SPI_INSTANCE, MAX7456ADD_STAT) & STATUS_REG_NVR_BUSY) != 0); + while ((max7456_send(MAX7456ADD_STAT, 0) & STATUS_REG_NVR_BUSY) != 0x00); max7456_send(VM0_REG, video_signal_type | 0x0C); DISABLE_MAX7456; From f49a396a7458288e0b7b933bfd7980c551c569d3 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Mon, 29 Aug 2016 15:54:43 +0200 Subject: [PATCH 395/456] Upps.. I did not save readme file :/ --- src/main/target/RACEBASE/readme.txt | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/main/target/RACEBASE/readme.txt b/src/main/target/RACEBASE/readme.txt index e69de29bb2..a2507bcd3e 100755 --- a/src/main/target/RACEBASE/readme.txt +++ b/src/main/target/RACEBASE/readme.txt @@ -0,0 +1,24 @@ +==RACABASE FC== + +Owner: Marcin Baliniak (marcin baliniak.pl) + +Available at: shop.rotoracer.com + +Board information: + +- CPU - STM32F303CCT6 +- MPU-6000 - connected to SPI2 +- SPI Flash - connected to SPI2 +- MAX7456 - connected to SPI2 (NO DMA) +- Build in 5V BEC + LC filter - board can be powered from main battery +- Input voltage: 5V - 17.4V +- RC input: PPM, S.BUS (build in inverter) - Uart2, Spectrum-Uart3 +- Other connectors: RSSI, Current sensor, LED strip, Buzzer, Video IN/Out + LC filter +- Weight: 6 g +- Size: 36 mm x 36 mm x 5 mm + +Photo: +https://cdn.shoplo.com/0639/products/th1024/aaaw/838-rr_flight_controller_rotoracer_fc_rotoracer_racebase.jpg + +Port description: +https://cdn.shoplo.com/0639/products/th1024/aaaw/844-racebase-render-en.png \ No newline at end of file From 7f325373d1004899166ea14b60aa656ddc8c2f31 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Andr=C3=A9?= Date: Mon, 29 Aug 2016 16:48:33 +0200 Subject: [PATCH 396/456] Change baro detect order Probe BMP085/BMP180 before MS5611, because BMP085 can be misdetected as a MS5611. --- src/main/sensors/initialisation.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 2529a8c330..edddb18421 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -460,7 +460,14 @@ static void detectBaro(baroSensor_e baroHardwareToUse) switch (baroHardware) { case BARO_DEFAULT: ; // fallthough - + case BARO_BMP085: +#ifdef USE_BARO_BMP085 + if (bmp085Detect(bmp085Config, &baro)) { + baroHardware = BARO_BMP085; + break; + } +#endif + ; // fallthough case BARO_MS5611: #ifdef USE_BARO_MS5611 if (ms5611Detect(&baro)) { @@ -469,14 +476,6 @@ static void detectBaro(baroSensor_e baroHardwareToUse) } #endif ; // fallthough - case BARO_BMP085: -#ifdef USE_BARO_BMP085 - if (bmp085Detect(bmp085Config, &baro)) { - baroHardware = BARO_BMP085; - break; - } -#endif - ; // fallthough case BARO_BMP280: #ifdef USE_BARO_BMP280 if (bmp280Detect(&baro)) { @@ -484,6 +483,7 @@ static void detectBaro(baroSensor_e baroHardwareToUse) break; } #endif + ; // fallthough case BARO_NONE: baroHardware = BARO_NONE; break; From 2f46b8b1cd030597eadd5794cd0187c9b953c3fa Mon Sep 17 00:00:00 2001 From: DTF UHF Date: Mon, 29 Aug 2016 19:26:03 -0400 Subject: [PATCH 397/456] Add support for MPU6000 (SPI) to DOGE target. --- src/main/target/DOGE/target.h | 13 +++++++++---- src/main/target/DOGE/target.mk | 1 + 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 3f40c4ab43..80a65fb1d3 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -56,6 +56,8 @@ // tqfp48 pin 3 #define MPU6500_CS_PIN SPI1_NSS_PIN #define MPU6500_SPI_INSTANCE SPI1 +#define MPU6000_CS_PIN SPI1_NSS_PIN +#define MPU6000_SPI_INSTANCE SPI1 // tqfp48 pin 25 #define BMP280_CS_PIN SPI2_NSS_PIN @@ -71,16 +73,19 @@ #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1 #define GYRO -// #define USE_FAKE_GYRO #define USE_GYRO_MPU6500 #define USE_GYRO_SPI_MPU6500 -#define GYRO_MPU6500_ALIGN CW270_DEG // ?? +#define GYRO_MPU6500_ALIGN CW270_DEG + +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW270_DEG #define ACC -// #define USE_FAKE_ACC #define USE_ACC_MPU6500 #define USE_ACC_SPI_MPU6500 -#define ACC_MPU6500_ALIGN CW270_DEG // ?? +#define ACC_MPU6500_ALIGN CW270_DEG +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW270_DEG #define BARO #define USE_BARO_BMP280 diff --git a/src/main/target/DOGE/target.mk b/src/main/target/DOGE/target.mk index 8699473185..392c55b3f5 100644 --- a/src/main/target/DOGE/target.mk +++ b/src/main/target/DOGE/target.mk @@ -3,6 +3,7 @@ FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ drivers/accgyro_mpu6500.c \ drivers/accgyro_spi_mpu6500.c \ drivers/barometer_bmp280.c \ From 4423d20acc02b00dcc11226e53ab18a01bce5c8c Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Wed, 31 Aug 2016 13:33:26 +1200 Subject: [PATCH 398/456] Made AIRMODE and 3D override 'auto_disarm_delay', to make it consistent with handling of MOTOR_STOP. --- src/main/mw.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/mw.c b/src/main/mw.c index 20580b66a9..08bf53420c 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -572,6 +572,8 @@ void processRx(void) if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) + && !feature(FEATURE_3D) + && !isAirmodeActive() ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { From 5d463ffabf7a1a2de129de14e4f81f89ad2a7cd7 Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Wed, 31 Aug 2016 13:50:56 +0200 Subject: [PATCH 399/456] Motor Outputs back again redo latest commit #1092 ... motor 1 and 2 not working correct (begin rotate only at mid throttle and above) --- src/main/target/SPRACINGF3EVO/target.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index d128f7e8b3..843ad3d9aa 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -30,10 +30,10 @@ const uint16_t multiPPM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; @@ -44,10 +44,10 @@ const uint16_t multiPWM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; From 4634fb0787e893c09f1ed00741b84700d2ef575d Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Wed, 31 Aug 2016 14:52:50 +0200 Subject: [PATCH 400/456] map only pwm8 and pwm9 to servo outputs see this in cleanflight --- src/main/target/SPRACINGF3EVO/target.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 843ad3d9aa..488e9a7a9c 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -30,8 +30,8 @@ const uint16_t multiPPM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF @@ -44,8 +44,8 @@ const uint16_t multiPWM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF From 7963e0518f486030b6b6250d70bf6fcd76607743 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Thu, 1 Sep 2016 12:16:55 +0200 Subject: [PATCH 401/456] Remove unnecessary redefinition, and force SPI flash to use standard clock on this target --- src/main/target/RACEBASE/config.c | 2 -- src/main/target/RACEBASE/target.h | 1 + 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/target/RACEBASE/config.c b/src/main/target/RACEBASE/config.c index 95e678acac..b9e0e15702 100755 --- a/src/main/target/RACEBASE/config.c +++ b/src/main/target/RACEBASE/config.c @@ -79,8 +79,6 @@ void targetConfiguration(master_t *config) { config->rxConfig.sbus_inversion = 0; config->rxConfig.rssi_scale = 19; - config->escAndServoConfig.minthrottle = 1060; - config->escAndServoConfig.maxthrottle = 1940; config->serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; config->rxConfig.serialrx_provider = SERIALRX_SBUS; } diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index d36dd41cfb..484237c5e3 100755 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -76,6 +76,7 @@ #define M25P16_CS_PIN PB12 #define M25P16_SPI_INSTANCE SPI2 +#define M25P16_SPI_SHARED #define USE_FLASHFS #define USE_FLASH_M25P16 From f6564c1bc259faefdbe44078ecba22a4fa5316c9 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Thu, 1 Sep 2016 13:42:06 +0200 Subject: [PATCH 402/456] Definition for serial RX changed - moved to target.h --- src/main/target/RACEBASE/config.c | 1 - src/main/target/RACEBASE/target.h | 3 +++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/target/RACEBASE/config.c b/src/main/target/RACEBASE/config.c index b9e0e15702..0ad304995c 100755 --- a/src/main/target/RACEBASE/config.c +++ b/src/main/target/RACEBASE/config.c @@ -79,6 +79,5 @@ void targetConfiguration(master_t *config) { config->rxConfig.sbus_inversion = 0; config->rxConfig.rssi_scale = 19; - config->serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; config->rxConfig.serialrx_provider = SERIALRX_SBUS; } diff --git a/src/main/target/RACEBASE/target.h b/src/main/target/RACEBASE/target.h index 484237c5e3..8e892151d4 100755 --- a/src/main/target/RACEBASE/target.h +++ b/src/main/target/RACEBASE/target.h @@ -62,6 +62,9 @@ #define UART3_TX_PIN PB10 #define UART3_RX_PIN PB11 +#define SERIALRX_UART SERIAL_PORT_USART2 + + #define USE_SPI #define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 From b3ac8972396f40ebf49c11a608573061f64a9a46 Mon Sep 17 00:00:00 2001 From: Marcin Baliniak Date: Thu, 1 Sep 2016 13:50:27 +0200 Subject: [PATCH 403/456] Markdown format for REDME.md --- src/main/target/RACEBASE/README.md | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100755 src/main/target/RACEBASE/README.md diff --git a/src/main/target/RACEBASE/README.md b/src/main/target/RACEBASE/README.md new file mode 100755 index 0000000000..1a06de5f27 --- /dev/null +++ b/src/main/target/RACEBASE/README.md @@ -0,0 +1,25 @@ +##RACABASE FC + +Owner: Marcin Baliniak (marcin baliniak.pl) + +Available at: shop.rotoracer.com + +###Board information: + +- CPU - STM32F303CCT6 +- MPU-6000 - connected to SPI2 +- SPI Flash - connected to SPI2 +- MAX7456 - connected to SPI2 (NO DMA) +- Build in 5V BEC + LC filter - board can be powered from main battery +- Input voltage: 5V - 17.4V +- RC input: PPM, S.BUS (build in inverter) - Uart2, Spectrum-Uart3 +- Other connectors: RSSI, Current sensor, LED strip, Buzzer, Video IN/Out + LC filter +- Weight: 6 g +- Size: 36 mm x 36 mm x 5 mm + +###Photo +![Board photo](https://cdn.shoplo.com/0639/products/th1024/aaaw/838-rr_flight_controller_rotoracer_fc_rotoracer_racebase.jpg) + + +###Port description +![Port description](https://cdn.shoplo.com/0639/products/th1024/aaaw/844-racebase-render-en.png) From 3f6860b83bbd78c2c7e0989fead48052e6137834 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 2 Sep 2016 22:04:53 +0200 Subject: [PATCH 404/456] Fix error in rc expo calculation // add configurable expo power --- src/main/common/maths.c | 7 +++++++ src/main/common/maths.h | 1 + src/main/config/config.c | 1 + src/main/io/rc_controls.h | 1 + src/main/io/serial_cli.c | 1 + src/main/io/serial_msp.c | 6 +++++- src/main/mw.c | 6 ++++-- 7 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 52990c6ac8..ee1b65ce67 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -99,6 +99,13 @@ float acos_approx(float x) } #endif +float powerf(float base, int exp) { + float result = base; + for (int count = 1; count < exp; count++) result *= base; + + return result; +} + int32_t applyDeadband(int32_t value, int32_t deadband) { if (ABS(value) < deadband) { diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 3251c237a8..3b24dd734d 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -69,6 +69,7 @@ typedef union { fp_angles_def angles; } fp_angles_t; +float powerf(float base, int exp); int32_t applyDeadband(int32_t value, int32_t deadband); void devClear(stdev_t *dev); diff --git a/src/main/config/config.c b/src/main/config/config.c index f5aabf3629..69b221b000 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -191,6 +191,7 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) controlRateConfig->dynThrPID = 10; controlRateConfig->rcYawExpo8 = 0; controlRateConfig->tpa_breakpoint = 1650; + controlRateConfig->rcExpoPwr = 3; for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { controlRateConfig->rates[axis] = 70; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 38c093f537..55e3cecbaf 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -152,6 +152,7 @@ typedef struct controlRateConfig_s { uint8_t dynThrPID; uint8_t rcYawExpo8; uint16_t tpa_breakpoint; // Breakpoint where TPA is activated + uint8_t rcExpoPwr; } controlRateConfig_t; extern int16_t rcCommand[4]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9509d0caa2..8a163f6ce3 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -800,6 +800,7 @@ const clivalue_t valueTable[] = { { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 255 } }, { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, + { "rc_expo_power", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 2, 4 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, { "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 52de0e4ed1..1e1c9bed4c 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -820,7 +820,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16((uint16_t)gyro.targetLooptime); break; case MSP_RC_TUNING: - headSerialReply(12); + headSerialReply(13); serialize8(currentControlRateProfile->rcRate8); serialize8(currentControlRateProfile->rcExpo8); for (i = 0 ; i < 3; i++) { @@ -832,6 +832,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentControlRateProfile->tpa_breakpoint); serialize8(currentControlRateProfile->rcYawExpo8); serialize8(currentControlRateProfile->rcYawRate8); + serialize8(currentControlRateProfile->rcExpoPwr); break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); @@ -1424,6 +1425,9 @@ static bool processInCommand(void) if (currentPort->dataSize >= 12) { currentControlRateProfile->rcYawRate8 = read8(); } + if (currentPort->dataSize >=13) { + currentControlRateProfile->rcExpoPwr = read8(); + } } else { headSerialError(0); } diff --git a/src/main/mw.c b/src/main/mw.c index 08bf53420c..01322d98ca 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -189,13 +189,15 @@ float calculateSetpointRate(int axis, int16_t rc) { if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); rcCommandf = rc / 500.0f; - rcInput[axis] = ABS(rcCommandf); if (rcExpo) { float expof = rcExpo / 100.0f; - rcCommandf = rcCommandf * (expof * (rcInput[axis] * rcInput[axis] * rcInput[axis]) + rcInput[axis]*(1-expof)); + float absRc = ABS(rcCommandf); + rcCommandf = rcCommandf * (expof * (powerf(absRc, currentControlRateProfile->rcExpoPwr)) + absRc*(1-expof)); } + rcInput[axis] = ABS(rcCommandf); + angleRate = 200.0f * rcRate * rcCommandf; if (currentControlRateProfile->rates[axis]) { From 9e5f66909031d57e2f9d007a80fac568550c4dcc Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 2 Sep 2016 22:15:43 +0200 Subject: [PATCH 405/456] Bump EEPROM --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 69b221b000..8917f2c4df 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 144; +static const uint8_t EEPROM_CONF_VERSION = 145; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { From b6b0e57b52894fa363104e4027d09a3032f6c4b2 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 3 Sep 2016 02:20:20 +0200 Subject: [PATCH 406/456] Change rcInput to store actual stick input --- src/main/mw.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index 01322d98ca..05f20cb918 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -189,19 +189,17 @@ float calculateSetpointRate(int axis, int16_t rc) { if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f)); rcCommandf = rc / 500.0f; + rcInput[axis] = ABS(rcCommandf); if (rcExpo) { float expof = rcExpo / 100.0f; - float absRc = ABS(rcCommandf); - rcCommandf = rcCommandf * (expof * (powerf(absRc, currentControlRateProfile->rcExpoPwr)) + absRc*(1-expof)); + rcCommandf = rcCommandf * (expof * (powerf(rcInput[axis], currentControlRateProfile->rcExpoPwr)) + rcInput[axis]*(1-expof)); } - rcInput[axis] = ABS(rcCommandf); - angleRate = 200.0f * rcRate * rcCommandf; if (currentControlRateProfile->rates[axis]) { - rcSuperfactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); + rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); angleRate *= rcSuperfactor; } @@ -210,7 +208,7 @@ float calculateSetpointRate(int axis, int16_t rc) { } if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) - return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection + return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection else return constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) } From 13b58cd64947cfccdb0f07d08df376d8573327dd Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 3 Sep 2016 14:33:15 +1200 Subject: [PATCH 407/456] Fixed RC Expo Power in CLI. --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 8a163f6ce3..5095ef0fe7 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -800,7 +800,7 @@ const clivalue_t valueTable[] = { { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 255 } }, { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, - { "rc_expo_power", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 2, 4 } }, + { "rc_expo_power", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpoPwr, .config.minmax = { 2, 4 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, { "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, From c50e48736f4ac4e64a797c5026015f16701afb65 Mon Sep 17 00:00:00 2001 From: nathan Date: Fri, 2 Sep 2016 19:56:15 -0700 Subject: [PATCH 408/456] include the make arm_sdk_install target, for easy building --- .gitignore | 4 + Makefile | 34 ++++- make/linux.mk | 35 +++++ make/macosx.mk | 35 +++++ make/system-id.mk | 50 +++++++ make/tools.mk | 330 ++++++++++++++++++++++++++++++++++++++++++++++ make/windows.mk | 14 ++ 7 files changed, 499 insertions(+), 3 deletions(-) create mode 100644 make/linux.mk create mode 100644 make/macosx.mk create mode 100644 make/system-id.mk create mode 100644 make/tools.mk create mode 100644 make/windows.mk diff --git a/.gitignore b/.gitignore index 245d33be92..054f2748e4 100644 --- a/.gitignore +++ b/.gitignore @@ -18,3 +18,7 @@ startup_stm32f10x_md_gcc.s docs/Manual.pdf README.pdf +# artifacts of top-level Makefile +/downloads +/tools +/build diff --git a/Makefile b/Makefile index 8cb92e667d..585592946a 100644 --- a/Makefile +++ b/Makefile @@ -67,6 +67,23 @@ INCLUDE_DIRS = $(SRC_DIR) \ $(ROOT)/src/main/target LINKER_DIR = $(ROOT)/src/main/target +## Build tools, so we all share the same versions +# import macros common to all supported build systems +include $(ROOT)/make/system-id.mk + +# configure some directories that are relative to wherever ROOT_DIR is located +TOOLS_DIR := $(ROOT)/tools +BUILD_DIR := $(ROOT)/build +DL_DIR := $(ROOT)/downloads + +export RM := rm + +# import macros that are OS specific +include $(ROOT)/make/$(OSFAMILY).mk + +# include the tools makefile +include $(ROOT)/make/tools.mk + # default xtal value for F4 targets HSE_VALUE = 8000000 @@ -572,9 +589,10 @@ CCACHE := endif # Tool names -CC := $(CCACHE) arm-none-eabi-gcc -OBJCOPY := arm-none-eabi-objcopy -SIZE := arm-none-eabi-size +CC := $(CCACHE) $(ARM_SDK_DIR)/bin/arm-none-eabi-gcc +CPP := $(CCACHE) $(ARM_SDK_DIR)/bin/arm-none-eabi-g++ +OBJCOPY := $(ARM_SDK_DIR)/bin/arm-none-eabi-objcopy +SIZE := $(ARM_SDK_DIR)/bin/arm-none-eabi-size # # Tool options. @@ -760,6 +778,16 @@ cppcheck: $(CSOURCES) cppcheck-result.xml: $(CSOURCES) $(V0) $(CPPCHECK) --xml-version=2 2> cppcheck-result.xml +## mkdirs +$(DL_DIR): + mkdir -p $@ + +$(TOOLS_DIR): + mkdir -p $@ + +$(BUILD_DIR): + mkdir -p $@ + ## help : print this help message and exit help: Makefile $(V0) @echo "" diff --git a/make/linux.mk b/make/linux.mk new file mode 100644 index 0000000000..ba1744fcbd --- /dev/null +++ b/make/linux.mk @@ -0,0 +1,35 @@ +# linux.mk +# +# Goals: +# Configure an environment that will allow Taulabs GCS and firmware to be built +# on a Linux system. The environment will support the current versions of Qt SDK +# and the ARM toolchain installed to either the Taulabs/tools directory, their +# respective default installation locations, or made available on the system path. + +# Check for and find Python 2 + +# Get Python version, separate major/minor/patch, then put into wordlist +PYTHON_VERSION_=$(wordlist 2,4,$(subst ., ,$(shell python -V 2>&1))) +# Get major version from aforementioned list +PYTHON_MAJOR_VERSION_=$(word 1,$(PYTHON_VERSION_)) +# Just in case Make has some weird scope stuff +PYTHON=0 +# If the major Python version is the one we want.. +ifeq ($(PYTHON_MAJOR_VERSION_),2) + # Then we can just use the normal Python executable + PYTHON:=python +else + # However, this isn't always the case.. + # Let's look for `python2`. If `which` doesn't return a null value, then + # it exists! + ifneq ($(shell which python2), "") + PYTHON:=python2 + else + # And if it doesn't exist, let's use the default Python, and warn the user. + # PYTHON NOT FOUND. + PYTHON:=python + echo "Python not found." + endif +endif + +export PYTHON diff --git a/make/macosx.mk b/make/macosx.mk new file mode 100644 index 0000000000..7f15c4fba1 --- /dev/null +++ b/make/macosx.mk @@ -0,0 +1,35 @@ +# macosx.mk +# +# Goals: +# Configure an environment that will allow Taulabs GCS and firmware to be built +# on a Mac OSX system. The environment will support the current version of the +# ARM toolchain installed to either their respective default installation +# locations, the tools directory or made available on the system path. + +# Check for and find Python 2 + +# Get Python version, separate major/minor/patch, then put into wordlist +PYTHON_VERSION_=$(wordlist 2,4,$(subst ., ,$(shell python -V 2>&1))) +# Get major version from aforementioned list +PYTHON_MAJOR_VERSION_=$(word 1,$(PYTHON_VERSION_)) +# Just in case Make has some weird scope stuff +PYTHON=0 +# If the major Python version is the one we want.. +ifeq ($(PYTHON_MAJOR_VERSION_),2) + # Then we can just use the normal Python executable + PYTHON:=python +else + # However, this isn't always the case.. + # Let's look for `python2`. If `which` doesn't return a null value, then + # it exists! + ifneq ($(shell which python2), "") + PYTHON:=python2 + else + # And if it doesn't exist, let's use the default Python, and warn the user. + # PYTHON NOT FOUND. + PYTHON:=python + echo "Python not found." + endif +endif + +export PYTHON diff --git a/make/system-id.mk b/make/system-id.mk new file mode 100644 index 0000000000..fc28a955e1 --- /dev/null +++ b/make/system-id.mk @@ -0,0 +1,50 @@ +# shared.mk +# +# environment variables common to all operating systems supported by the make system + +# Make sure we know a few things about the architecture +UNAME := $(shell uname) +ARCH := $(shell uname -m) +ifneq (,$(filter $(ARCH), x86_64 amd64)) + X86-64 := 1 + X86_64 := 1 + AMD64 := 1 + ARCHFAMILY := x86_64 +else + ARCHFAMILY := $(ARCH) +endif + +# configure some variables dependent upon what type of system this is + +# Linux +ifeq ($(UNAME), Linux) + OSFAMILY := linux + LINUX := 1 +endif + +# Mac OSX +ifeq ($(UNAME), Darwin) + OSFAMILY := macosx + MACOSX := 1 +endif + +# Windows using MinGW shell +ifeq (MINGW, $(findstring MINGW,$(UNAME))) + OSFAMILY := windows + MINGW := 1 + WINDOWS := 1 +endif + +# Windows using Cygwin shell +ifeq (CYGWIN ,$(findstring CYGWIN,$(UNAME))) + OSFAMILY := windows + WINDOWS := 1 + CYGWIN := 1 +endif + +# report an error if we couldn't work out what OS this is running on +ifndef OSFAMILY + $(info uname reports $(UNAME)) + $(info uname -m reports $(ARCH)) + $(error failed to detect operating system) +endif diff --git a/make/tools.mk b/make/tools.mk new file mode 100644 index 0000000000..77aa88a6cb --- /dev/null +++ b/make/tools.mk @@ -0,0 +1,330 @@ +############################################################### +# +# Installers for tools +# +# NOTE: These are not tied to the default goals +# and must be invoked manually +# +############################################################### + +############################## +# +# Check that environmental variables are sane +# +############################## + +# Set up ARM (STM32) SDK +ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-5_4-2016q2 + +.PHONY: arm_sdk_install + +# source: https://launchpad.net/gcc-arm-embedded/5.0/ +ifdef LINUX + arm_sdk_install: ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q2-update/+download/gcc-arm-none-eabi-5_4-2016q2-20160622-linux.tar.bz2 +endif + +ifdef MACOSX + arm_sdk_install: ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q2-update/+download/gcc-arm-none-eabi-5_4-2016q2-20160622-mac.tar.bz2 +endif + +ifdef WINDOWS + arm_sdk_install: ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q2-update/+download/gcc-arm-none-eabi-5_4-2016q2-20160622-win32.zip +endif + +arm_sdk_install: ARM_SDK_FILE := $(notdir $(ARM_SDK_URL)) +# order-only prereq on directory existance: +arm_sdk_install: | $(DL_DIR) $(TOOLS_DIR) +arm_sdk_install: arm_sdk_clean +ifneq ($(OSFAMILY), windows) + # download the source only if it's newer than what we already have + $(V1) wget --no-check-certificate -N -P "$(DL_DIR)" "$(ARM_SDK_URL)" + + # binary only release so just extract it + $(V1) tar -C $(TOOLS_DIR) -xjf "$(DL_DIR)/$(ARM_SDK_FILE)" +else + $(V1) curl -L -k -o "$(DL_DIR)/$(ARM_SDK_FILE)" "$(ARM_SDK_URL)" + $(V1) unzip -q -d $(ARM_SDK_DIR) "$(DL_DIR)/$(ARM_SDK_FILE)" +endif + +.PHONY: arm_sdk_clean +arm_sdk_clean: + $(V1) [ ! -d "$(ARM_SDK_DIR)" ] || $(RM) -r $(ARM_SDK_DIR) + +.PHONY: openocd_win_install + +openocd_win_install: | $(DL_DIR) $(TOOLS_DIR) +openocd_win_install: OPENOCD_URL := git://git.code.sf.net/p/openocd/code +openocd_win_install: OPENOCD_REV := cf1418e9a85013bbf8dbcc2d2e9985695993d9f4 +openocd_win_install: OPENOCD_OPTIONS := + +ifeq ($(OPENOCD_FTDI), yes) +openocd_win_install: OPENOCD_OPTIONS := $(OPENOCD_OPTIONS) --enable-ft2232_ftd2xx --with-ftd2xx-win32-zipdir=$(FTD2XX_DIR) +endif + +openocd_win_install: openocd_win_clean libusb_win_install ftd2xx_install + # download the source + $(V0) @echo " DOWNLOAD $(OPENOCD_URL) @ $(OPENOCD_REV)" + $(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)" + $(V1) mkdir -p "$(OPENOCD_BUILD_DIR)" + $(V1) git clone --no-checkout $(OPENOCD_URL) "$(DL_DIR)/openocd-build" + $(V1) ( \ + cd $(OPENOCD_BUILD_DIR) ; \ + git checkout -q $(OPENOCD_REV) ; \ + ) + + # apply patches + $(V0) @echo " PATCH $(OPENOCD_BUILD_DIR)" + $(V1) ( \ + cd $(OPENOCD_BUILD_DIR) ; \ + git apply < $(ROOT_DIR)/flight/Project/OpenOCD/0003-freertos-cm4f-fpu-support.patch ; \ + git apply < $(ROOT_DIR)/flight/Project/OpenOCD/0004-st-icdi-disable.patch ; \ + ) + + # build and install + $(V0) @echo " BUILD $(OPENOCD_WIN_DIR)" + $(V1) mkdir -p "$(OPENOCD_WIN_DIR)" + $(V1) ( \ + cd $(OPENOCD_BUILD_DIR) ; \ + ./bootstrap ; \ + ./configure --enable-maintainer-mode --prefix="$(OPENOCD_WIN_DIR)" \ + --build=i686-pc-linux-gnu --host=i586-mingw32msvc \ + CPPFLAGS=-I$(LIBUSB_WIN_DIR)/include \ + LDFLAGS=-L$(LIBUSB_WIN_DIR)/lib/gcc \ + $(OPENOCD_OPTIONS) \ + --disable-werror \ + --enable-stlink ; \ + $(MAKE) ; \ + $(MAKE) install ; \ + ) + + # delete the extracted source when we're done + $(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)" + +.PHONY: openocd_win_clean +openocd_win_clean: + $(V0) @echo " CLEAN $(OPENOCD_WIN_DIR)" + $(V1) [ ! -d "$(OPENOCD_WIN_DIR)" ] || $(RM) -r "$(OPENOCD_WIN_DIR)" + +# Set up openocd tools +OPENOCD_DIR := $(TOOLS_DIR)/openocd +OPENOCD_WIN_DIR := $(TOOLS_DIR)/openocd_win +OPENOCD_BUILD_DIR := $(DL_DIR)/openocd-build + +.PHONY: openocd_install + +openocd_install: | $(DL_DIR) $(TOOLS_DIR) +openocd_install: OPENOCD_URL := git://git.code.sf.net/p/openocd/code +openocd_install: OPENOCD_TAG := v0.9.0 +openocd_install: OPENOCD_OPTIONS := --enable-maintainer-mode --prefix="$(OPENOCD_DIR)" --enable-buspirate --enable-stlink + +ifeq ($(OPENOCD_FTDI), yes) +openocd_install: OPENOCD_OPTIONS := $(OPENOCD_OPTIONS) --enable-ftdi +endif + +ifeq ($(UNAME), Darwin) +openocd_install: OPENOCD_OPTIONS := $(OPENOCD_OPTIONS) --disable-option-checking +endif + +openocd_install: openocd_clean + # download the source + $(V0) @echo " DOWNLOAD $(OPENOCD_URL) @ $(OPENOCD_TAG)" + $(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)" + $(V1) mkdir -p "$(OPENOCD_BUILD_DIR)" + $(V1) git clone --no-checkout $(OPENOCD_URL) "$(OPENOCD_BUILD_DIR)" + $(V1) ( \ + cd $(OPENOCD_BUILD_DIR) ; \ + git checkout -q tags/$(OPENOCD_TAG) ; \ + ) + + # build and install + $(V0) @echo " BUILD $(OPENOCD_DIR)" + $(V1) mkdir -p "$(OPENOCD_DIR)" + $(V1) ( \ + cd $(OPENOCD_BUILD_DIR) ; \ + ./bootstrap ; \ + ./configure $(OPENOCD_OPTIONS) ; \ + $(MAKE) ; \ + $(MAKE) install ; \ + ) + + # delete the extracted source when we're done + $(V1) [ ! -d "$(OPENOCD_BUILD_DIR)" ] || $(RM) -rf "$(OPENOCD_BUILD_DIR)" + +.PHONY: openocd_clean +openocd_clean: + $(V0) @echo " CLEAN $(OPENOCD_DIR)" + $(V1) [ ! -d "$(OPENOCD_DIR)" ] || $(RM) -r "$(OPENOCD_DIR)" + +STM32FLASH_DIR := $(TOOLS_DIR)/stm32flash + +.PHONY: stm32flash_install +stm32flash_install: STM32FLASH_URL := http://stm32flash.googlecode.com/svn/trunk +stm32flash_install: STM32FLASH_REV := 61 +stm32flash_install: stm32flash_clean + # download the source + $(V0) @echo " DOWNLOAD $(STM32FLASH_URL) @ r$(STM32FLASH_REV)" + $(V1) svn export -q -r "$(STM32FLASH_REV)" "$(STM32FLASH_URL)" "$(STM32FLASH_DIR)" + + # build + $(V0) @echo " BUILD $(STM32FLASH_DIR)" + $(V1) $(MAKE) --silent -C $(STM32FLASH_DIR) all + +.PHONY: stm32flash_clean +stm32flash_clean: + $(V0) @echo " CLEAN $(STM32FLASH_DIR)" + $(V1) [ ! -d "$(STM32FLASH_DIR)" ] || $(RM) -r "$(STM32FLASH_DIR)" + +DFUUTIL_DIR := $(TOOLS_DIR)/dfu-util + +.PHONY: dfuutil_install +dfuutil_install: DFUUTIL_URL := http://dfu-util.sourceforge.net/releases/dfu-util-0.8.tar.gz +dfuutil_install: DFUUTIL_FILE := $(notdir $(DFUUTIL_URL)) +dfuutil_install: | $(DL_DIR) $(TOOLS_DIR) +dfuutil_install: dfuutil_clean + # download the source + $(V0) @echo " DOWNLOAD $(DFUUTIL_URL)" + $(V1) wget -N -P "$(DL_DIR)" "$(DFUUTIL_URL)" + + # extract the source + $(V0) @echo " EXTRACT $(DFUUTIL_FILE)" + $(V1) [ ! -d "$(DL_DIR)/dfuutil-build" ] || $(RM) -r "$(DL_DIR)/dfuutil-build" + $(V1) mkdir -p "$(DL_DIR)/dfuutil-build" + $(V1) tar -C $(DL_DIR)/dfuutil-build -xf "$(DL_DIR)/$(DFUUTIL_FILE)" + + # build + $(V0) @echo " BUILD $(DFUUTIL_DIR)" + $(V1) mkdir -p "$(DFUUTIL_DIR)" + $(V1) ( \ + cd $(DL_DIR)/dfuutil-build/dfu-util-0.8 ; \ + ./configure --prefix="$(DFUUTIL_DIR)" ; \ + $(MAKE) ; \ + $(MAKE) install ; \ + ) + +.PHONY: dfuutil_clean +dfuutil_clean: + $(V0) @echo " CLEAN $(DFUUTIL_DIR)" + $(V1) [ ! -d "$(DFUUTIL_DIR)" ] || $(RM) -r "$(DFUUTIL_DIR)" + +# Set up uncrustify tools +UNCRUSTIFY_DIR := $(TOOLS_DIR)/uncrustify-0.61 +UNCRUSTIFY_BUILD_DIR := $(DL_DIR)/uncrustify + +.PHONY: uncrustify_install +uncrustify_install: | $(DL_DIR) $(TOOLS_DIR) +uncrustify_install: UNCRUSTIFY_URL := http://downloads.sourceforge.net/project/uncrustify/uncrustify/uncrustify-0.61/uncrustify-0.61.tar.gz +uncrustify_install: UNCRUSTIFY_FILE := uncrustify-0.61.tar.gz +uncrustify_install: UNCRUSTIFY_OPTIONS := prefix=$(UNCRUSTIFY_DIR) +uncrustify_install: uncrustify_clean +ifneq ($(OSFAMILY), windows) + $(V0) @echo " DOWNLOAD $(UNCRUSTIFY_URL)" + $(V1) wget --no-check-certificate -N -P "$(DL_DIR)" "$(UNCRUSTIFY_URL)" +else + $(V1) curl -L -k -o "$(DL_DIR)/$(UNCRUSTIFY_FILE)" "$(UNCRUSTIFY_URL)" +endif + # extract the src + $(V0) @echo " EXTRACT $(UNCRUSTIFY_FILE)" + $(V1) tar -C $(TOOLS_DIR) -xf "$(DL_DIR)/$(UNCRUSTIFY_FILE)" + + $(V0) @echo " BUILD $(UNCRUSTIFY_DIR)" + $(V1) ( \ + cd $(UNCRUSTIFY_DIR) ; \ + ./configure --prefix="$(UNCRUSTIFY_DIR)" ; \ + $(MAKE) ; \ + $(MAKE) install ; \ + ) + # delete the extracted source when we're done + $(V1) [ ! -d "$(UNCRUSTIFY_BUILD_DIR)" ] || $(RM) -r "$(UNCRUSTIFY_BUILD_DIR)" + +.PHONY: uncrustify_clean +uncrustify_clean: + $(V0) @echo " CLEAN $(UNCRUSTIFY_DIR)" + $(V1) [ ! -d "$(UNCRUSTIFY_DIR)" ] || $(RM) -r "$(UNCRUSTIFY_DIR)" + $(V0) @echo " CLEAN $(UNCRUSTIFY_BUILD_DIR)" + $(V1) [ ! -d "$(UNCRUSTIFY_BUILD_DIR)" ] || $(RM) -r "$(UNCRUSTIFY_BUILD_DIR)" + +# ZIP download URL +zip_install: ZIP_URL := http://pkgs.fedoraproject.org/repo/pkgs/zip/zip30.tar.gz/7b74551e63f8ee6aab6fbc86676c0d37/zip30.tar.gz + +zip_install: ZIP_FILE := $(notdir $(ZIP_URL)) + +ZIP_DIR = $(TOOLS_DIR)/zip30 + +# order-only prereq on directory existance: +zip_install : | $(DL_DIR) $(TOOLS_DIR) +zip_install: zip_clean + $(V1) curl -L -k -o "$(DL_DIR)/$(ZIP_FILE)" "$(ZIP_URL)" + $(V1) tar --force-local -C $(TOOLS_DIR) -xzf "$(DL_DIR)/$(ZIP_FILE)" +ifneq ($(OSFAMILY), windows) + $(V1) cd "$(ZIP_DIR)" && $(MAKE) -f unix/Makefile generic_gcc +else + $(V1) cd "$(ZIP_DIR)" && $(MAKE) -f win32/makefile.gcc +endif + +.PHONY: zip_clean +zip_clean: + $(V1) [ ! -d "$(ZIP_DIR)" ] || $(RM) -rf $(ZIP_DIR) + +############################## +# +# Set up paths to tools +# +############################## + +ifeq ($(shell [ -d "$(ARM_SDK_DIR)" ] && echo "exists"), exists) + ARM_SDK_PREFIX := $(ARM_SDK_DIR)/bin/arm-none-eabi- +else + ifndef IGNORE_MISSING_TOOLCHAIN + ifeq (,$(findstring _install,$(MAKECMDGOALS))) + $(error **WARNING** ARM-SDK not in $(ARM_SDK_DIR) Please run 'make arm_sdk_install') + endif + endif + # not installed, hope it's in the path... + ARM_SDK_PREFIX ?= arm-none-eabi- +endif + +ifeq ($(shell [ -d "$(ZIP_DIR)" ] && echo "exists"), exists) + export ZIPBIN := $(ZIP_DIR)/zip +else + export ZIPBIN := zip +endif + +ifeq ($(shell [ -d "$(OPENOCD_DIR)" ] && echo "exists"), exists) + OPENOCD := $(OPENOCD_DIR)/bin/openocd +else + # not installed, hope it's in the path... + OPENOCD ?= openocd +endif + +ifeq ($(shell [ -d "$(UNCRUSTIFY_DIR)" ] && echo "exists"), exists) + UNCRUSTIFY := $(UNCRUSTIFY_DIR)/bin/uncrustify +else + # not installed, hope it's in the path... + UNCRUSTIFY ?= uncrustify +endif + +# Google Breakpad +DUMP_SYMBOLS_TOOL := $(TOOLS_DIR)/breakpad/$(OSFAMILY)-$(ARCHFAMILY)/dump_syms +BREAKPAD_URL := http://dronin.tracer.nz/tools/breakpad.zip +BREAKPAD_DL_FILE := $(DL_DIR)/$(notdir $(BREAKPAD_URL)) +BREAKPAD_DIR := $(TOOLS_DIR)/breakpad + +.PHONY: breakpad_install +breakpad_install: | $(DL_DIR) $(TOOLS_DIR) +breakpad_install: breakpad_clean + $(V0) @echo " DOWNLOAD $(BREAKPAD_URL)" + $(V1) $(V1) curl -L -k -z "$(BREAKPAD_DL_FILE)" -o "$(BREAKPAD_DL_FILE)" "$(BREAKPAD_URL)" + $(V0) @echo " EXTRACT $(notdir $(BREAKPAD_DL_FILE))" + $(V1) mkdir -p "$(BREAKPAD_DIR)" + $(V1) unzip -q -d $(BREAKPAD_DIR) "$(BREAKPAD_DL_FILE)" +ifeq ($(OSFAMILY), windows) + $(V1) ln -s "$(TOOLS_DIR)/breakpad/$(OSFAMILY)-i686" "$(TOOLS_DIR)/breakpad/$(OSFAMILY)-x86_64" +endif + +.PHONY: breakpad_clean +breakpad_clean: + $(V0) @echo " CLEAN $(BREAKPAD_DIR)" + $(V1) [ ! -d "$(BREAKPAD_DIR)" ] || $(RM) -rf $(BREAKPAD_DIR) + $(V0) @echo " CLEAN $(BREAKPAD_DL_FILE)" + $(V1) $(RM) -f $(BREAKPAD_DL_FILE) + diff --git a/make/windows.mk b/make/windows.mk new file mode 100644 index 0000000000..ceb968bbbc --- /dev/null +++ b/make/windows.mk @@ -0,0 +1,14 @@ +# windows.mk +# +# Goals: +# Configure an environment that will allow Taulabs GCS and firmware to be built +# on a Windows system. The environment will support the current version of the +# ARM toolchain installed to either their respective default installation +# locations, the tools directory or made available on the system path. +# +# Requirements: +# msysGit +# Python + +PYTHON := python +export PYTHON From e98acee7cf4acea069c35485c3ce0212c7ac103d Mon Sep 17 00:00:00 2001 From: nathan Date: Fri, 2 Sep 2016 20:13:32 -0700 Subject: [PATCH 409/456] fix travis --- .travis.yml | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index 80b896b6a6..61f556c440 100644 --- a/.travis.yml +++ b/.travis.yml @@ -68,13 +68,10 @@ addons: language: cpp compiler: clang -before_install: - - curl --retry 10 --retry-max-time 120 -L "https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q2-update/+download/gcc-arm-none-eabi-5_4-2016q2-20160622-linux.tar.bz2" | tar xfj - - install: - - export PATH=$PATH:$PWD/gcc-arm-none-eabi-5_4-2016q2/bin + - make arm_sdk_install -before_script: arm-none-eabi-gcc --version +before_script: tools/gcc-arm-none-*/bin/arm-none-eabi-gcc --version script: ./.travis.sh cache: apt From e338279be418a6d360621dfa4dd712e8ca61a950 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sat, 3 Sep 2016 19:04:58 +1000 Subject: [PATCH 410/456] DFU mode via CLI for F1 and F3 --- src/main/drivers/system_stm32f10x.c | 13 +++++++++++++ src/main/drivers/system_stm32f30x.c | 13 +++++++++++++ src/main/drivers/system_stm32f4xx.c | 4 ++-- 3 files changed, 28 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index 3e33c545b8..d77f9fec55 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -114,4 +114,17 @@ void systemInit(void) void checkForBootLoaderRequest(void) { + void(*bootJump)(void); + + if (*((uint32_t *)0x20004FF0) == 0xDEADBEEF) { + + *((uint32_t *)0x20004FF0) = 0x0; + + __enable_irq(); + __set_MSP(*((uint32_t *)0x1FFFF000)); + + bootJump = (void(*)(void))(*((uint32_t *) 0x1FFFF004)); + bootJump(); + while (1); + } } diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index fb30936968..507e0811c5 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -107,4 +107,17 @@ void systemInit(void) void checkForBootLoaderRequest(void) { + void(*bootJump)(void); + + if (*((uint32_t *)0x20009FFC) == 0xDEADBEEF) { + + *((uint32_t *)0x20009FFC) = 0x0; + + __enable_irq(); + __set_MSP(*((uint32_t *)0x1FFFD800)); + + bootJump = (void(*)(void))(*((uint32_t *) 0x1FFFD804)); + bootJump(); + while (1); + } } diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 5c09ebdf6a..69c22c3ffc 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -196,9 +196,9 @@ void checkForBootLoaderRequest(void) *((uint32_t *)0x2001FFFC) = 0x0; __enable_irq(); - __set_MSP(0x20001000); + __set_MSP(*((uint32_t *)0x1FFF0000)); - bootJump = (void(*)(void))(*((uint32_t *) 0x1fff0004)); + bootJump = (void(*)(void))(*((uint32_t *) 0x1FFF0004)); bootJump(); while (1); } From f1a2d58cff7f46c93d154c82f6e451caa815c32b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anton=20St=C3=A5lheim?= Date: Sun, 4 Sep 2016 19:34:13 +0200 Subject: [PATCH 411/456] Removed VBATT_PRESENT_THRESHOLD --- src/main/sensors/battery.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 8a1175d42b..9426c3898b 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -39,7 +39,6 @@ #include "rx/rx.h" -#define VBATT_PRESENT_THRESHOLD 10 #define VBATT_LPF_FREQ 0.4f // Battery monitoring stuff @@ -90,7 +89,7 @@ void updateBattery(void) updateBatteryVoltage(); /* battery has just been connected*/ - if (batteryState == BATTERY_NOT_PRESENT && vbat > MAX(VBATT_PRESENT_THRESHOLD, batteryConfig->batterynotpresentlevel)) + if (batteryState == BATTERY_NOT_PRESENT && vbat > batteryConfig->batterynotpresentlevel ) { /* Actual battery state is calculated below, this is really BATTERY_PRESENT */ batteryState = BATTERY_OK; @@ -111,7 +110,7 @@ void updateBattery(void) batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; } /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */ - else if (batteryState != BATTERY_NOT_PRESENT && vbat <= MAX(VBATT_PRESENT_THRESHOLD, batteryConfig->batterynotpresentlevel)) + else if (batteryState != BATTERY_NOT_PRESENT && vbat <= batteryConfig->batterynotpresentlevel) { batteryState = BATTERY_NOT_PRESENT; batteryCellCount = 0; From a257210b56f9f7a9684e99352398110557f7b466 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 11 Aug 2016 19:29:55 +0100 Subject: [PATCH 412/456] Split features into a separate module --- Makefile | 1 + src/main/blackbox/blackbox.c | 1 + src/main/config/config.c | 61 ++-------------- src/main/config/config.h | 7 -- src/main/config/feature.c | 124 ++++++++++++++++++++++++++++++++ src/main/config/feature.h | 26 +++++++ src/main/fc/mw.c | 1 + src/main/fc/rc_controls.c | 1 + src/main/fc/rc_curves.c | 1 + src/main/flight/mixer.c | 1 + src/main/io/beeper.c | 1 + src/main/io/display.c | 1 + src/main/io/gps.c | 1 + src/main/io/ledstrip.c | 3 +- src/main/io/serial_cli.c | 1 + src/main/io/serial_msp.c | 1 + src/main/main.c | 1 + src/main/rx/pwm.c | 1 + src/main/rx/rx.c | 1 + src/main/sensors/acceleration.c | 1 + src/main/sensors/battery.c | 1 + src/main/sensors/sonar.c | 1 + src/main/telemetry/frsky.c | 1 + src/main/telemetry/smartport.c | 4 +- 24 files changed, 177 insertions(+), 66 deletions(-) create mode 100644 src/main/config/feature.c create mode 100644 src/main/config/feature.h diff --git a/Makefile b/Makefile index 8cb92e667d..9d291017b0 100644 --- a/Makefile +++ b/Makefile @@ -385,6 +385,7 @@ COMMON_SRC = \ common/typeconversion.c \ config/config.c \ config/config_eeprom.c \ + config/feature.c \ fc/runtime_config.c \ drivers/adc.c \ drivers/buf_writer.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 102ff6adcb..5adfa0022a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -81,6 +81,7 @@ #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" #include "blackbox.h" #include "blackbox_io.h" diff --git a/src/main/config/config.c b/src/main/config/config.c index 3c3a5f3de8..981b5b4e50 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -80,6 +80,7 @@ #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" #ifndef DEFAULT_RX_FEATURE #define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM @@ -97,11 +98,15 @@ void targetConfiguration(master_t *config); master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; -static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; + +void intFeatureClearAll(master_t *config); +void intFeatureSet(uint32_t mask, master_t *config); +void intFeatureClear(uint32_t mask, master_t *config); + static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { accelerometerTrims->values.pitch = 0; @@ -372,10 +377,6 @@ uint16_t getCurrentMinthrottle(void) return masterConfig.escAndServoConfig.minthrottle; } -static void intFeatureClearAll(master_t *config); -static void intFeatureSet(uint32_t mask, master_t *config); -static void intFeatureClear(uint32_t mask, master_t *config); - // Default settings void createDefaultConfig(master_t *config) { @@ -872,56 +873,6 @@ void changeControlRateProfile(uint8_t profileIndex) activateControlRateConfig(); } -void latchActiveFeatures() -{ - activeFeaturesLatch = masterConfig.enabledFeatures; -} - -bool featureConfigured(uint32_t mask) -{ - return masterConfig.enabledFeatures & mask; -} - -bool feature(uint32_t mask) -{ - return activeFeaturesLatch & mask; -} - -void featureSet(uint32_t mask) -{ - intFeatureSet(mask, &masterConfig); -} - -static void intFeatureSet(uint32_t mask, master_t *config) -{ - config->enabledFeatures |= mask; -} - -void featureClear(uint32_t mask) -{ - intFeatureClear(mask, &masterConfig); -} - -static void intFeatureClear(uint32_t mask, master_t *config) -{ - config->enabledFeatures &= ~(mask); -} - -void featureClearAll() -{ - intFeatureClearAll(&masterConfig); -} - -static void intFeatureClearAll(master_t *config) -{ - config->enabledFeatures = 0; -} - -uint32_t featureMask(void) -{ - return masterConfig.enabledFeatures; -} - void beeperOffSet(uint32_t mask) { masterConfig.beeper_off_flags |= mask; diff --git a/src/main/config/config.h b/src/main/config/config.h index 9878adc876..292fb13af8 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -55,13 +55,6 @@ typedef enum { FEATURE_SOFTSPI = 1 << 26, } features_e; -void latchActiveFeatures(void); -bool featureConfigured(uint32_t mask); -bool feature(uint32_t mask); -void featureSet(uint32_t mask); -void featureClear(uint32_t mask); -void featureClearAll(void); -uint32_t featureMask(void); void beeperOffSet(uint32_t mask); void beeperOffSetAll(uint8_t beeperCount); void beeperOffClear(uint32_t mask); diff --git a/src/main/config/feature.c b/src/main/config/feature.c new file mode 100644 index 0000000000..77b9069f35 --- /dev/null +++ b/src/main/config/feature.c @@ -0,0 +1,124 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" + +#include "common/color.h" +#include "common/axis.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/system.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.h" +#include "drivers/serial.h" + +#include "sensors/sensors.h" +#include "sensors/gyro.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/beeper.h" +#include "io/serial.h" +#include "io/gimbal.h" +#include "io/escservo.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" +#include "io/ledstrip.h" +#include "io/gps.h" +#include "io/osd.h" +#include "io/vtx.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" +#include "flight/altitudehold.h" +#include "flight/navigation.h" + +#include "config/config.h" +#include "config/config_profile.h" +#include "config/config_master.h" +#include "config/feature.h" + +static uint32_t activeFeaturesLatch = 0; + +void intFeatureSet(uint32_t mask, master_t *config) +{ + config->enabledFeatures |= mask; +} + +void intFeatureClear(uint32_t mask, master_t *config) +{ + config->enabledFeatures &= ~(mask); +} + +void intFeatureClearAll(master_t *config) +{ + config->enabledFeatures = 0; +} + +void latchActiveFeatures() +{ + activeFeaturesLatch = masterConfig.enabledFeatures; +} + +bool featureConfigured(uint32_t mask) +{ + return masterConfig.enabledFeatures & mask; +} + +bool feature(uint32_t mask) +{ + return activeFeaturesLatch & mask; +} + +void featureSet(uint32_t mask) +{ + intFeatureSet(mask, &masterConfig); +} + +void featureClear(uint32_t mask) +{ + intFeatureClear(mask, &masterConfig); +} + +void featureClearAll() +{ + intFeatureClearAll(&masterConfig); +} + +uint32_t featureMask(void) +{ + return masterConfig.enabledFeatures; +} + + diff --git a/src/main/config/feature.h b/src/main/config/feature.h new file mode 100644 index 0000000000..526d73c839 --- /dev/null +++ b/src/main/config/feature.h @@ -0,0 +1,26 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +void latchActiveFeatures(void); +bool featureConfigured(uint32_t mask); +bool feature(uint32_t mask); +void featureSet(uint32_t mask); +void featureClear(uint32_t mask); +void featureClearAll(void); +uint32_t featureMask(void); diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 5dc03e2fb5..864a6eedd8 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -87,6 +87,7 @@ #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" #include "scheduler/scheduler.h" #include "scheduler/scheduler_tasks.h" diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index dd2903a6ce..dcbeedf8b1 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -29,6 +29,7 @@ #include "common/maths.h" #include "config/config.h" +#include "config/feature.h" #include "fc/runtime_config.h" diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c index 55cdce5247..ee69070d46 100644 --- a/src/main/fc/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -21,6 +21,7 @@ #include "platform.h" #include "config/config.h" +#include "config/feature.h" #include "io/escservo.h" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 2943290e51..17d9066ac1 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -55,6 +55,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/feature.h" uint8_t motorCount; diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index e015172be7..8ee23cd3b7 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -41,6 +41,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/feature.h" #include "io/beeper.h" diff --git a/src/main/io/display.c b/src/main/io/display.c index ce18ee3704..3002ea7d3f 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -58,6 +58,7 @@ #endif #include "config/config.h" +#include "config/feature.h" #include "fc/runtime_config.h" #include "config/config_profile.h" diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 11a9f0873e..ac3cfefea8 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -50,6 +50,7 @@ #include "flight/navigation.h" #include "config/config.h" +#include "config/feature.h" #include "fc/runtime_config.h" diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 4a361303fc..f8292c553a 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -40,7 +40,7 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" -#include +#include "common/printf.h" #include "common/axis.h" #include "common/utils.h" @@ -77,6 +77,7 @@ #include "config/config.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" /* PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 010fa34d2d..3f8770f663 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -89,6 +89,7 @@ #include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" #include "common/printf.h" diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 50ee576d64..922a556690 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -94,6 +94,7 @@ #include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" diff --git a/src/main/main.c b/src/main/main.c index 98d06a167e..4d1023d587 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -108,6 +108,7 @@ #include "config/config_eeprom.h" #include "config/config_profile.h" #include "config/config_master.h" +#include "config/feature.h" #define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times diff --git a/src/main/rx/pwm.c b/src/main/rx/pwm.c index a85fd94759..e01653e2f5 100644 --- a/src/main/rx/pwm.c +++ b/src/main/rx/pwm.c @@ -32,6 +32,7 @@ #include "drivers/pwm_rx.h" #include "config/config.h" +#include "config/feature.h" #include "rx/rx.h" #include "rx/pwm.h" diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index d5629bfae3..8667f38f92 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -29,6 +29,7 @@ #include "common/maths.h" #include "config/config.h" +#include "config/feature.h" #include "drivers/serial.h" #include "drivers/adc.h" diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 07cd90b204..97b75326c1 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -34,6 +34,7 @@ #include "io/beeper.h" #include "sensors/boardalignment.h" #include "config/config.h" +#include "config/feature.h" #include "sensors/acceleration.h" diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 532f74ce79..d43e05ab1e 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -31,6 +31,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/feature.h" #include "sensors/battery.h" diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 88e2f2c47d..2e11e64d97 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -33,6 +33,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/feature.h" #include "sensors/sensors.h" #include "sensors/battery.h" diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 20552b01a4..aa704142a5 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -59,6 +59,7 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/feature.h" #include "telemetry/telemetry.h" #include "telemetry/frsky.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index fcf26b010f..3d585d2f74 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -61,9 +61,7 @@ #include "fc/runtime_config.h" #include "config/config.h" -#include "config/config_profile.h" -extern profile_t *currentProfile; -extern controlRateConfig_t *currentControlRateProfile; +#include "config/feature.h" enum { From a65ac0fab1538d89e52f0d396ee66059e9d43214 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 4 Sep 2016 18:55:04 +0100 Subject: [PATCH 413/456] Fixup after rebase --- src/main/telemetry/smartport.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 3d585d2f74..402bf6a4f4 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -61,8 +61,12 @@ #include "fc/runtime_config.h" #include "config/config.h" +#include "config/config_profile.h" #include "config/feature.h" +extern profile_t *currentProfile; +extern controlRateConfig_t *currentControlRateProfile; + enum { SPSTATE_UNINITIALIZED, From 9a4d975077661f3e4e1c5c0a91fc0dff250eb69d Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 17 Aug 2016 09:14:08 +0100 Subject: [PATCH 414/456] Moved #defines out of mixer function definitions --- src/main/config/config.c | 8 ++--- src/main/flight/mixer.c | 63 ++++++++++++++++++---------------------- src/main/flight/mixer.h | 12 ++++---- src/main/main.c | 5 ++-- 4 files changed, 40 insertions(+), 48 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 3c3a5f3de8..6e74f19f9b 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -678,10 +678,6 @@ void activateConfig(void) setAccelerationFilter(masterConfig.acc_lpf_hz); mixerUseConfigs( -#ifdef USE_SERVOS - masterConfig.servoConf, - &masterConfig.gimbalConfig, -#endif &masterConfig.flight3DConfig, &masterConfig.escAndServoConfig, &masterConfig.mixerConfig, @@ -689,6 +685,10 @@ void activateConfig(void) &masterConfig.rxConfig ); +#ifdef USE_SERVOS + servoUseConfigs(masterConfig.servoConf, &masterConfig.gimbalConfig); +#endif + imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f; imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f; imuRuntimeConfig.acc_unarmedcal = masterConfig.acc_unarmedcal; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 2943290e51..11620b9d11 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -335,20 +335,12 @@ static servoMixer_t *customServoMixers; static motorMixer_t *customMixers; void mixerUseConfigs( -#ifdef USE_SERVOS - servoParam_t *servoConfToUse, - gimbalConfig_t *gimbalConfigToUse, -#endif flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse) { -#ifdef USE_SERVOS - servoConf = servoConfToUse; - gimbalConfig = gimbalConfigToUse; -#endif flight3DConfig = flight3DConfigToUse; escAndServoConfig = escAndServoConfigToUse; mixerConfig = mixerConfigToUse; @@ -357,6 +349,12 @@ void mixerUseConfigs( } #ifdef USE_SERVOS +void servoUseConfigs(servoParam_t *servoConfToUse, gimbalConfig_t *gimbalConfigToUse) +{ + servoConf = servoConfToUse; + gimbalConfig = gimbalConfigToUse; +} + int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex) { uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel; @@ -377,8 +375,31 @@ int servoDirection(int servoIndex, int inputSource) else return 1; } + +void servoInit(servoMixer_t *initialCustomServoMixers) +{ + customServoMixers = initialCustomServoMixers; + + // enable servos for mixes that require them. note, this shifts motor counts. + useServo = mixers[currentMixerMode].useServo; + // if we want camstab/trig, that also enables servos, even if mixer doesn't + if (feature(FEATURE_SERVO_TILT)) + useServo = 1; + + // give all servos a default command + for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servo[i] = DEFAULT_SERVO_MIDDLE; + } +} #endif +void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) +{ + currentMixerMode = mixerMode; + + customMixers = initialCustomMixers; +} + #ifndef USE_QUAD_MIXER_ONLY void loadCustomServoMixer(void) @@ -400,25 +421,6 @@ void loadCustomServoMixer(void) } } -void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, servoMixer_t *initialCustomServoMixers) -{ - currentMixerMode = mixerMode; - - customMixers = initialCustomMotorMixers; - customServoMixers = initialCustomServoMixers; - - // enable servos for mixes that require them. note, this shifts motor counts. - useServo = mixers[currentMixerMode].useServo; - // if we want camstab/trig, that also enables servos, even if mixer doesn't - if (feature(FEATURE_SERVO_TILT)) - useServo = 1; - - // give all servos a default command - for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo[i] = DEFAULT_SERVO_MIDDLE; - } -} - void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration, bool use_unsyncedPwm) { int i; @@ -520,13 +522,6 @@ void mixerLoadMix(int index, motorMixer_t *customMixers) #else -void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) -{ - currentMixerMode = mixerMode; - - customMixers = initialCustomMixers; -} - void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration, bool use_unsyncedPwm) { UNUSED(pwmOutputConfiguration); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index bf1293e732..980fabd031 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -193,10 +193,6 @@ struct escAndServoConfig_s; struct rxConfig_s; void mixerUseConfigs( -#ifdef USE_SERVOS - servoParam_t *servoConfToUse, - struct gimbalConfig_s *gimbalConfigToUse, -#endif flight3DConfig_t *flight3DConfigToUse, struct escAndServoConfig_s *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, @@ -206,13 +202,15 @@ void mixerUseConfigs( void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); #ifdef USE_SERVOS -void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixer_t *customServoMixers); +void servoInit(servoMixer_t *customServoMixers); +struct servoParam_s; +struct gimbalConfig_s; +void servoUseConfigs(struct servoParam_s *servoConfToUse, struct gimbalConfig_s *gimbalConfigToUse); void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); void loadCustomServoMixer(void); int servoDirection(int servoIndex, int fromChannel); -#else -void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); #endif +void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); struct pwmOutputConfiguration_s; void mixerUsePWMOutputConfiguration(struct pwmOutputConfiguration_s *pwmOutputConfiguration, bool use_unsyncedPwm); void mixerResetDisarmedMotors(void); diff --git a/src/main/main.c b/src/main/main.c index 98d06a167e..3d42395a38 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -239,10 +239,9 @@ void init(void) serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif -#ifdef USE_SERVOS - mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer, masterConfig.customServoMixer); -#else mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); +#ifdef USE_SERVOS + servoInit(masterConfig.customServoMixer); #endif drv_pwm_config_t pwm_params; From a36ad01c3a8101f18c0bf71f067d8a3e4febb5dd Mon Sep 17 00:00:00 2001 From: nathan Date: Sun, 4 Sep 2016 13:41:39 -0700 Subject: [PATCH 415/456] absolute path to gcc in .travis.yml --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 61f556c440..06f9ea073f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -71,7 +71,7 @@ compiler: clang install: - make arm_sdk_install -before_script: tools/gcc-arm-none-*/bin/arm-none-eabi-gcc --version +before_script: tools/gcc-arm-none-eabi-5_4-2016q2/bin/arm-none-eabi-gcc --version script: ./.travis.sh cache: apt From bca2a074410c141fa2feb46830cb0b80d943647b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anton=20St=C3=A5lheim?= Date: Mon, 5 Sep 2016 17:19:32 +0200 Subject: [PATCH 416/456] Added ARMED guard to BATTERY_NOT_PRESENT --- src/main/sensors/battery.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 9426c3898b..33eec1689b 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -110,7 +110,7 @@ void updateBattery(void) batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; } /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of VBATT_PRESENT_THRESHOLD */ - else if (batteryState != BATTERY_NOT_PRESENT && vbat <= batteryConfig->batterynotpresentlevel) + else if (batteryState != BATTERY_NOT_PRESENT && vbat <= batteryConfig->batterynotpresentlevel && !ARMING_FLAG(ARMED)) { batteryState = BATTERY_NOT_PRESENT; batteryCellCount = 0; From c4ace2c8df3fcf9199bbe2f6156fe93e51385ed7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anton=20St=C3=A5lheim?= Date: Mon, 5 Sep 2016 18:01:29 +0200 Subject: [PATCH 417/456] Feedback FPVANGLEMIX mode --- src/main/io/serial_msp.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 1e1c9bed4c..21f68bebac 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -599,7 +599,8 @@ static uint32_t packFlightModeFlags(void) IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | - IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE; + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX; for (i = 0; i < activeBoxIdCount; i++) { int flag = (tmp & (1 << activeBoxIds[i])); From 2987c7d8c8db4f8fb9d71840c110153662a7a08b Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 6 Sep 2016 00:46:52 +0200 Subject: [PATCH 418/456] Fix expo curve --- src/main/mw.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/mw.c b/src/main/mw.c index 05f20cb918..d846085543 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -193,7 +193,7 @@ float calculateSetpointRate(int axis, int16_t rc) { if (rcExpo) { float expof = rcExpo / 100.0f; - rcCommandf = rcCommandf * (expof * (powerf(rcInput[axis], currentControlRateProfile->rcExpoPwr)) + rcInput[axis]*(1-expof)); + rcCommandf = rcCommandf * powerf(rcInput[axis], currentControlRateProfile->rcExpoPwr) * expof + rcCommandf * (1-expof); } angleRate = 200.0f * rcRate * rcCommandf; From efd43059b56ab23ecb99bdc9e3640305908cb0f2 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 6 Sep 2016 01:28:26 +0200 Subject: [PATCH 419/456] Remove unnecessary complicated expo power --- src/main/config/config.c | 3 +-- src/main/io/rc_controls.h | 1 - src/main/io/serial_cli.c | 1 - src/main/io/serial_msp.c | 6 +----- src/main/mw.c | 3 ++- 5 files changed, 4 insertions(+), 10 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 8917f2c4df..f5aabf3629 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 145; +static const uint8_t EEPROM_CONF_VERSION = 144; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -191,7 +191,6 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) controlRateConfig->dynThrPID = 10; controlRateConfig->rcYawExpo8 = 0; controlRateConfig->tpa_breakpoint = 1650; - controlRateConfig->rcExpoPwr = 3; for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { controlRateConfig->rates[axis] = 70; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 55e3cecbaf..38c093f537 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -152,7 +152,6 @@ typedef struct controlRateConfig_s { uint8_t dynThrPID; uint8_t rcYawExpo8; uint16_t tpa_breakpoint; // Breakpoint where TPA is activated - uint8_t rcExpoPwr; } controlRateConfig_t; extern int16_t rcCommand[4]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5095ef0fe7..9509d0caa2 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -800,7 +800,6 @@ const clivalue_t valueTable[] = { { "rc_rate_yaw", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawRate8, .config.minmax = { 0, 255 } }, { "rc_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpo8, .config.minmax = { 0, 100 } }, { "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } }, - { "rc_expo_power", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcExpoPwr, .config.minmax = { 2, 4 } }, { "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } }, { "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } }, { "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 21f68bebac..33bd4a74c9 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -821,7 +821,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16((uint16_t)gyro.targetLooptime); break; case MSP_RC_TUNING: - headSerialReply(13); + headSerialReply(12); serialize8(currentControlRateProfile->rcRate8); serialize8(currentControlRateProfile->rcExpo8); for (i = 0 ; i < 3; i++) { @@ -833,7 +833,6 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentControlRateProfile->tpa_breakpoint); serialize8(currentControlRateProfile->rcYawExpo8); serialize8(currentControlRateProfile->rcYawRate8); - serialize8(currentControlRateProfile->rcExpoPwr); break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); @@ -1426,9 +1425,6 @@ static bool processInCommand(void) if (currentPort->dataSize >= 12) { currentControlRateProfile->rcYawRate8 = read8(); } - if (currentPort->dataSize >=13) { - currentControlRateProfile->rcExpoPwr = read8(); - } } else { headSerialError(0); } diff --git a/src/main/mw.c b/src/main/mw.c index d846085543..faed2088b0 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -174,6 +174,7 @@ bool isCalibrating() } #define RC_RATE_INCREMENTAL 14.54f +#define RC_EXPO_POWER 3 float calculateSetpointRate(int axis, int16_t rc) { float angleRate, rcRate, rcSuperfactor, rcCommandf; @@ -193,7 +194,7 @@ float calculateSetpointRate(int axis, int16_t rc) { if (rcExpo) { float expof = rcExpo / 100.0f; - rcCommandf = rcCommandf * powerf(rcInput[axis], currentControlRateProfile->rcExpoPwr) * expof + rcCommandf * (1-expof); + rcCommandf = rcCommandf * powerf(rcInput[axis], RC_EXPO_POWER) * expof + rcCommandf * (1-expof); } angleRate = 200.0f * rcRate * rcCommandf; From d0ceb1167aecd85d44d60b6596367373af4cbd9f Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 6 Sep 2016 01:48:49 +0200 Subject: [PATCH 420/456] Increase Pterm setpoint weight range --- src/main/io/serial_cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 9509d0caa2..2c10cea9b7 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -853,7 +853,7 @@ const clivalue_t valueTable[] = { { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, - { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } }, + { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {40, 255 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, From 2baa1e1d39d5e52ebbc0f282bc706c5e78118d9a Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 6 Sep 2016 22:44:19 +1200 Subject: [PATCH 421/456] Fixed broken LEDSTRIP 'west' indicator. --- src/main/io/ledstrip.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index c41c3a884b..090f56c946 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -257,9 +257,9 @@ STATIC_UNIT_TESTED void determineLedStripDimensions(void) STATIC_UNIT_TESTED void determineOrientationLimits(void) { - highestYValueForNorth = MIN((ledGridHeight / 2) - 1, 0); + highestYValueForNorth = MAX((ledGridHeight / 2) - 1, 0); lowestYValueForSouth = (ledGridHeight + 1) / 2; - highestXValueForWest = MIN((ledGridWidth / 2) - 1, 0); + highestXValueForWest = MAX((ledGridWidth / 2) - 1, 0); lowestXValueForEast = (ledGridWidth + 1) / 2; } From 9dd135ec802de71b57469468b2c1318dc50264a5 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 6 Sep 2016 22:02:55 +0200 Subject: [PATCH 422/456] Add back ESC jump limit // revert p weight limit --- src/main/config/config.c | 5 +++-- src/main/flight/mixer.c | 13 +++++++++++++ src/main/io/escservo.h | 1 + src/main/io/serial_cli.c | 3 ++- 4 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index f5aabf3629..078a3f5c27 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 144; +static const uint8_t EEPROM_CONF_VERSION = 145; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->ptermSetpointWeight = 80; + pidProfile->ptermSetpointWeight = 85; pidProfile->dtermSetpointWeight = 150; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; @@ -320,6 +320,7 @@ void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) escAndServoConfig->maxthrottle = 2000; escAndServoConfig->mincommand = 1000; escAndServoConfig->servoCenterPulse = 1500; + escAndServoConfig->maxEscThrottleJumpMs = 0; } void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6f6dbb3ae2..6111a9883c 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -862,6 +862,19 @@ void mixTable(void *pidProfilePtr) } } + // Anti Desync feature for ESC's. Limit rapid throttle changes + if (escAndServoConfig->maxEscThrottleJumpMs) { + const int16_t maxThrottleStep = constrain(escAndServoConfig->maxEscThrottleJumpMs / (1000 / targetPidLooptime), 5, 10000); + + // Only makes sense when it's within the range + if (maxThrottleStep < throttleRange) { + static int16_t motorPrevious[MAX_SUPPORTED_MOTORS]; + + motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep); + motorPrevious[i] = motor[i]; + } + } + // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { diff --git a/src/main/io/escservo.h b/src/main/io/escservo.h index 66cd7db59e..fe37d26445 100644 --- a/src/main/io/escservo.h +++ b/src/main/io/escservo.h @@ -24,4 +24,5 @@ typedef struct escAndServoConfig_s { uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500. + uint16_t maxEscThrottleJumpMs; } escAndServoConfig_t; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2c10cea9b7..0b64d0526f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -670,6 +670,7 @@ const clivalue_t valueTable[] = { { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, + { "max_esc_throttle_jump", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxEscThrottleJumpMs, .config.minmax = { 0, 1000 } }, { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_high, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME lower limit should match code in the mixer, 1500 currently, @@ -853,7 +854,7 @@ const clivalue_t valueTable[] = { { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, - { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {40, 255 } }, + { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {40, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, From b0e182a3233f5522e014c281c1275a304fef6319 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 6 Sep 2016 22:06:39 +0200 Subject: [PATCH 423/456] Change Defaults --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 078a3f5c27..b818805a75 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -211,7 +211,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[PITCH] = 60; pidProfile->I8[PITCH] = 65; pidProfile->D8[PITCH] = 22; - pidProfile->P8[YAW] = 80; + pidProfile->P8[YAW] = 70; pidProfile->I8[YAW] = 45; pidProfile->D8[YAW] = 20; pidProfile->P8[PIDALT] = 50; From bf7b093c930c7ca0eec5662fe95f928f0ca7e299 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 6 Sep 2016 10:19:57 +0200 Subject: [PATCH 424/456] Add BetaflightF3 --- src/main/target/BETAFLIGHTF3/readme.txt | 13 ++ src/main/target/BETAFLIGHTF3/target.c | 84 +++++++++++++ src/main/target/BETAFLIGHTF3/target.h | 151 ++++++++++++++++++++++++ src/main/target/BETAFLIGHTF3/target.mk | 17 +++ 4 files changed, 265 insertions(+) create mode 100755 src/main/target/BETAFLIGHTF3/readme.txt create mode 100755 src/main/target/BETAFLIGHTF3/target.c create mode 100755 src/main/target/BETAFLIGHTF3/target.h create mode 100755 src/main/target/BETAFLIGHTF3/target.mk diff --git a/src/main/target/BETAFLIGHTF3/readme.txt b/src/main/target/BETAFLIGHTF3/readme.txt new file mode 100755 index 0000000000..b6c5f279b0 --- /dev/null +++ b/src/main/target/BETAFLIGHTF3/readme.txt @@ -0,0 +1,13 @@ +==BETAFLIGHTF3== + +Owner: BorisB + +Board information: + +- CPU - STM32F303CCT6 +- MPU-6000 +- SD Card Slot +- Build in 5V BEC + LC filter - board can be powered from main battery +- Built in current meter, PDB + +More info to come \ No newline at end of file diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c new file mode 100755 index 0000000000..9c177c4baa --- /dev/null +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -0,0 +1,84 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + //Target code By Hector "Hectech FPV" Hind + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM1 | (MAP_TO_PWM_INPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM3, IO_TAG(PB7), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // RC_CH1 - PA0 - *TIM2_CH1 + + { TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 + { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 + { TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM3 - PB8 + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - PB9 + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM5 - PA1 + { TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA0 + { TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM7 - PA2 + { TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9 }, // PWM8 - PA3 + + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO_TIMER / LED_STRIP +}; + diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h new file mode 100755 index 0000000000..05b0807b6d --- /dev/null +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -0,0 +1,151 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + //Target code By Hector "Hectech FPV" Hind + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "BETAFC" + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + + +//#define LED0 PC14 +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USABLE_TIMER_CHANNEL_COUNT 17 + +#define USE_MAG_DATA_READY_SIGNAL +#define ENSURE_MAG_DATA_READY_IS_HIGH + +#define MPU6000_CS_PIN PA15 +#define MPU6000_SPI_INSTANCE SPI1 + + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW180_DEG + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW180_DEG + +// MPU6000 interrupts +#define USE_MPU_DATA_READY_SIGNAL +#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) +#define MPU_INT_EXTI PC13 +#define USE_EXTI + +#define USB_IO + +//#define USE_FLASHFS +//#define USE_FLASH_M25P16 + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_SOFTSERIAL2 +#define SERIAL_PORT_COUNT 5 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 // PA14 / SWCLK +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 // PB10 (AF7) +#define UART3_RX_PIN PB11 // PB11 (AF7) + +#define SOFTSERIAL_2_TIMER TIM3 +#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 5 +#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 6 + + +#undef USE_I2C + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 +//GPIO_AF_1 + +#define SPI1_NSS_PIN PA15 +#define SPI1_SCK_PIN PB3 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PB5 + +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + + + + +#define USE_SDCARD +#define USE_SDCARD_SPI2 +#define SDCARD_DETECT_INVERTED + +#define SDCARD_DETECT_PIN PC14 +#define SDCARD_SPI_INSTANCE SPI2 +//#define SDCARD_SPI_CS_GPIO SPI2_GPIO +#define SDCARD_SPI_CS_PIN SPI2_NSS_PIN + +#define SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER 128 +#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 2 + +#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5 +#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA1_FLAG_TC5 +//#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 +//#define SDCARD_DMA_CHANNEL DMA_Channel_0 + + +#define BOARD_HAS_VOLTAGE_DIVIDER +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define VBAT_ADC_PIN PA4 +#define CURRENT_METER_ADC_PIN PA5 +#define RSSI_ADC_PIN PB2 + +#define LED_STRIP + +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_PIN PA8 +#define WS2811_TIMER TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_CURRENT_METER) + +#define SPEKTRUM_BIND +// USART3, +#define BIND_PIN PB11 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// IO - stm32f303cc in 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) + +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) \ No newline at end of file diff --git a/src/main/target/BETAFLIGHTF3/target.mk b/src/main/target/BETAFLIGHTF3/target.mk new file mode 100755 index 0000000000..022c21a259 --- /dev/null +++ b/src/main/target/BETAFLIGHTF3/target.mk @@ -0,0 +1,17 @@ + +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD +TARGET_FLAGS = -DSPRACINGF3 + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/display_ug2864hsweg01.h \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c \ + drivers/serial_softserial.c \ + drivers/sonar_hcsr04.c + From 7df4fbe35716afe2bb98c24fded2d20553c6b5e5 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 7 Sep 2016 15:21:52 +0200 Subject: [PATCH 425/456] Disable forced Enabling of Telemetry Inversion --- src/main/config/config.c | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b818805a75..3e058a6fec 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -891,12 +891,6 @@ void validateAndFixConfig(void) } #endif -#ifdef STM32F303xC - // hardware supports serial port inversion, make users life easier for those that want to connect SBus RX's - masterConfig.telemetryConfig.telemetry_inversion = 1; -#endif - - /*#if defined(LED_STRIP) && defined(TRANSPONDER) // TODO - Add transponder feature if ((WS2811_DMA_TC_FLAG == TRANSPONDER_DMA_TC_FLAG) && featureConfigured(FEATURE_TRANSPONDER) && featureConfigured(FEATURE_LED_STRIP)) { featureClear(FEATURE_LED_STRIP); From 35886233834a1b5480fef6fbcf4ad11ac7d98383 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 7 Sep 2016 15:34:15 +0200 Subject: [PATCH 426/456] Throttle Jump Limit only applied to accelerating situation --- src/main/flight/mixer.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6111a9883c..07418470a3 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -864,13 +864,13 @@ void mixTable(void *pidProfilePtr) // Anti Desync feature for ESC's. Limit rapid throttle changes if (escAndServoConfig->maxEscThrottleJumpMs) { - const int16_t maxThrottleStep = constrain(escAndServoConfig->maxEscThrottleJumpMs / (1000 / targetPidLooptime), 5, 10000); + const int16_t maxThrottleStep = constrain(escAndServoConfig->maxEscThrottleJumpMs / (1000 / targetPidLooptime), 2, 10000); // Only makes sense when it's within the range if (maxThrottleStep < throttleRange) { static int16_t motorPrevious[MAX_SUPPORTED_MOTORS]; - motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep); + motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, motorPrevious[i] + maxThrottleStep); // Only limit accelerating situation motorPrevious[i] = motor[i]; } } From 5096ef3aa196e278bccc4b1f5b0d6db001c0d4d6 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 25 Aug 2016 21:05:36 +0200 Subject: [PATCH 427/456] AlienFlight Target updates Add AlienFlight documentation --- src/main/target/ALIENFLIGHTF1/AlienFlight.md | 52 ++++++++++++++++++++ src/main/target/ALIENFLIGHTF1/target.h | 12 ++--- src/main/target/ALIENFLIGHTF3/AlienFlight.md | 52 ++++++++++++++++++++ src/main/target/ALIENFLIGHTF3/target.h | 5 +- src/main/target/ALIENFLIGHTF4/AlienFlight.md | 52 ++++++++++++++++++++ src/main/target/ALIENFLIGHTF4/target.h | 1 + 6 files changed, 164 insertions(+), 10 deletions(-) create mode 100644 src/main/target/ALIENFLIGHTF1/AlienFlight.md create mode 100644 src/main/target/ALIENFLIGHTF3/AlienFlight.md create mode 100644 src/main/target/ALIENFLIGHTF4/AlienFlight.md diff --git a/src/main/target/ALIENFLIGHTF1/AlienFlight.md b/src/main/target/ALIENFLIGHTF1/AlienFlight.md new file mode 100644 index 0000000000..cb6e154bdb --- /dev/null +++ b/src/main/target/ALIENFLIGHTF1/AlienFlight.md @@ -0,0 +1,52 @@ +# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3 and ALIENFLIGHTF4 target) + +AlienWii is now AlienFlight. This targets supports various variants of brushed and brushless flight controllers. The designs for them are released partially for public use at: + +http://www.alienflight.com + +All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users or RC vendors to build this designs. + +Some variants of the AlienFlight controllers will be available for purchase from: + +http://www.microfpv.eu + +Here are the general hardware specifications for this boards: + +- STM32F103CBT6 MCU (ALIENFLIGHTF1) +- STM32F303CCT6 MCU (ALIENFLIGHTF3) +- STM32F405RGT6 MCU (ALIENFLIGHTF4) +- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit +- The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware +- 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) +- extra-wide traces on the PCB, for maximum power throughput (brushed variants) +- some new F4 boards using a 4-layer PCB for better power distribution +- USB port, integrated +- (*) serial connection for external DSM2/DSMX sat receiver (e.g. Spektrum SAT, OrangeRx R100, Lemon RX or Deltang Rx31) and SBUS +- CPPM input +- ground and 3.3V for the receiver, some boards have also the option to power an 5V receiver +- hardware bind plug for easy binding +- motor connections are at the corners for a clean look with reduced wiring +- small footprint +- direct operation from a single cell Lipoly battery for brushed versions +- 3.3V LDO power regulator (older prototypes) +- 3.3V buck-boost power converter (all new versions) +- 5V buck-boost power converter for FPV (some versions) +- brushless versions are designed for 3S operation and also have an 5V power output +- battery monitoring with an LED for buzzer functionality (for some ALIENFLIGHTF3 and F4 variants only) + +(*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator. + + set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit) + set spektrum_sat_bind = 5 + +For more detail of the different bind modes please refer the CleanFlight Spektrum Bind document. + +Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. + +The pin layout for the ALIENFLIGHTF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENFLIGHTF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight/AlienWii F3 layouts running the same firmware which takes care on the differences with a hardware detection. + +The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3 or ALIENFLIGHTF4. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. + +## Flashing the firmware + +The firmware can be updated with the BetaFlight configurator as for any other target. All AlienFlight boards have a boot jumper which need to be closed for initial flashing or for recovery from a broken firmware. diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index 6d945008f7..b80430b769 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -18,6 +18,7 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "AFF1" // AlienFlight F1. +#define TARGET_CONFIG #define LED0 PB3 #define LED1 PB4 @@ -25,7 +26,6 @@ #define BEEPER PA12 #define USE_EXTI -#define MAG_INT_EXTI PC14 #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL @@ -70,8 +70,9 @@ // USART2, PA3 #define BIND_PIN PA3 -// alternative defaults for AlienFlight F1 target -#define TARGET_CONFIG +#define HARDWARE_BIND_PLUG +// Hardware bind plug at PB5 (Pin 41) +#define BINDPLUG_PIN PB5 #define BRUSHED_MOTORS #define DEFAULT_FEATURES FEATURE_MOTOR_STOP @@ -80,10 +81,7 @@ #define SERIALRX_UART SERIAL_PORT_USART2 #define RX_CHANNELS_TAER -#define HARDWARE_BIND_PLUG -// Hardware bind plug at PB5 (Pin 41) -#define BINDPLUG_PIN PB5 - +#define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - assuming all IOs on 48pin package #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/ALIENFLIGHTF3/AlienFlight.md b/src/main/target/ALIENFLIGHTF3/AlienFlight.md new file mode 100644 index 0000000000..cb6e154bdb --- /dev/null +++ b/src/main/target/ALIENFLIGHTF3/AlienFlight.md @@ -0,0 +1,52 @@ +# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3 and ALIENFLIGHTF4 target) + +AlienWii is now AlienFlight. This targets supports various variants of brushed and brushless flight controllers. The designs for them are released partially for public use at: + +http://www.alienflight.com + +All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users or RC vendors to build this designs. + +Some variants of the AlienFlight controllers will be available for purchase from: + +http://www.microfpv.eu + +Here are the general hardware specifications for this boards: + +- STM32F103CBT6 MCU (ALIENFLIGHTF1) +- STM32F303CCT6 MCU (ALIENFLIGHTF3) +- STM32F405RGT6 MCU (ALIENFLIGHTF4) +- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit +- The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware +- 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) +- extra-wide traces on the PCB, for maximum power throughput (brushed variants) +- some new F4 boards using a 4-layer PCB for better power distribution +- USB port, integrated +- (*) serial connection for external DSM2/DSMX sat receiver (e.g. Spektrum SAT, OrangeRx R100, Lemon RX or Deltang Rx31) and SBUS +- CPPM input +- ground and 3.3V for the receiver, some boards have also the option to power an 5V receiver +- hardware bind plug for easy binding +- motor connections are at the corners for a clean look with reduced wiring +- small footprint +- direct operation from a single cell Lipoly battery for brushed versions +- 3.3V LDO power regulator (older prototypes) +- 3.3V buck-boost power converter (all new versions) +- 5V buck-boost power converter for FPV (some versions) +- brushless versions are designed for 3S operation and also have an 5V power output +- battery monitoring with an LED for buzzer functionality (for some ALIENFLIGHTF3 and F4 variants only) + +(*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator. + + set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit) + set spektrum_sat_bind = 5 + +For more detail of the different bind modes please refer the CleanFlight Spektrum Bind document. + +Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. + +The pin layout for the ALIENFLIGHTF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENFLIGHTF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight/AlienWii F3 layouts running the same firmware which takes care on the differences with a hardware detection. + +The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3 or ALIENFLIGHTF4. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. + +## Flashing the firmware + +The firmware can be updated with the BetaFlight configurator as for any other target. All AlienFlight boards have a boot jumper which need to be closed for initial flashing or for recovery from a broken firmware. diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 28a896ff01..b92a100aaa 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -124,11 +124,10 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -// IO - assuming 303 in 64pin package, TODO +// IO - stm32f303cc in 48pin package #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(15) | TIM_N(17) ) diff --git a/src/main/target/ALIENFLIGHTF4/AlienFlight.md b/src/main/target/ALIENFLIGHTF4/AlienFlight.md new file mode 100644 index 0000000000..cb6e154bdb --- /dev/null +++ b/src/main/target/ALIENFLIGHTF4/AlienFlight.md @@ -0,0 +1,52 @@ +# AlienFlight (ALIENFLIGHTF1, ALIENFLIGHTF3 and ALIENFLIGHTF4 target) + +AlienWii is now AlienFlight. This targets supports various variants of brushed and brushless flight controllers. The designs for them are released partially for public use at: + +http://www.alienflight.com + +All published designs are flight tested by various people. The intention here is to make these flight controllers available and enable skilled users or RC vendors to build this designs. + +Some variants of the AlienFlight controllers will be available for purchase from: + +http://www.microfpv.eu + +Here are the general hardware specifications for this boards: + +- STM32F103CBT6 MCU (ALIENFLIGHTF1) +- STM32F303CCT6 MCU (ALIENFLIGHTF3) +- STM32F405RGT6 MCU (ALIENFLIGHTF4) +- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit +- The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware +- 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants) +- extra-wide traces on the PCB, for maximum power throughput (brushed variants) +- some new F4 boards using a 4-layer PCB for better power distribution +- USB port, integrated +- (*) serial connection for external DSM2/DSMX sat receiver (e.g. Spektrum SAT, OrangeRx R100, Lemon RX or Deltang Rx31) and SBUS +- CPPM input +- ground and 3.3V for the receiver, some boards have also the option to power an 5V receiver +- hardware bind plug for easy binding +- motor connections are at the corners for a clean look with reduced wiring +- small footprint +- direct operation from a single cell Lipoly battery for brushed versions +- 3.3V LDO power regulator (older prototypes) +- 3.3V buck-boost power converter (all new versions) +- 5V buck-boost power converter for FPV (some versions) +- brushless versions are designed for 3S operation and also have an 5V power output +- battery monitoring with an LED for buzzer functionality (for some ALIENFLIGHTF3 and F4 variants only) + +(*) Spektrum Compatible DSM2 satellites are supported out of the box. DSMX sat will work with DSM2 protocol with default settings (DSM2, 11bit, 11ms is preset). This is chosen for maximum compatibility. For optimal connection it is recommended to adjust settings to match the capabilities of your transmitter and satellite receiver. If possible it is recommended to use the DSMX protocol since it is known as more reliable. Also to make use of additional channels you should adjust the following two parameters with the Cleanflight Configurator. + + set serialrx_provider = 1 (0 for 1024bit, 1 for 2048bit) + set spektrum_sat_bind = 5 + +For more detail of the different bind modes please refer the CleanFlight Spektrum Bind document. + +Deltang receivers in serial mode will work like any other Spektrum satellite receiver (10bit, 22ms) only the bind process will be different. + +The pin layout for the ALIENFLIGHTF1 is very similar to NAZE32 or the related clones (MW32, Flip32, etc.). The hardware bind pin is connected to pin 41 (PB5). The pin layout for the ALIENFLIGHTF3 is similar to Sparky. The hardware bind pin is connected to pin 25 (PB12). The new AlienFlightF3 V2 design have the sensor connected via SPI and some slightly different pin layout. All AlienFlight/AlienWii F3 layouts running the same firmware which takes care on the differences with a hardware detection. + +The AlienFlight firmware will be built as target ALIENFLIGHTF1, ALIENFLIGHTF3 or ALIENFLIGHTF4. The firmware image will come with alternative default settings which will give the user a plug and play experience. There is no computer needed to get this into the air with a small Quadcopter. A preconfigured custom mixer for an Octocopter is part of the default settings to allow clean straight wiring with the AlienFlight. The mixer can be activated with "mixer custom" in the CLI. To use the AlienFlight controller in a Hexa- or Octocopter or to do some more tuning additional configuration changes can be done as usual in the CLI or the BetaFlight configurator. + +## Flashing the firmware + +The firmware can be updated with the BetaFlight configurator as for any other target. All AlienFlight boards have a boot jumper which need to be closed for initial flashing or for recovery from a broken firmware. diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index a6fc694e6a..ef01181237 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -27,6 +27,7 @@ #define LED1 PD2 #define BEEPER PC13 +#define BEEPER_INVERTED #define INVERTER PC15 #define INVERTER_USART USART2 From 57c954e6088b90048a0223a853a6e8a91c4e2f76 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 7 Sep 2016 23:04:51 +0200 Subject: [PATCH 428/456] Slight notch default shift --- src/main/config/config.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 3e058a6fec..417a8b4969 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -240,8 +240,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yawItermIgnoreRate = 32; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default - pidProfile->dterm_notch_hz = 260; - pidProfile->dterm_notch_cutoff = 160; + pidProfile->dterm_notch_hz = 250; + pidProfile->dterm_notch_cutoff = 150; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; From bbd0b67134b30360038ed1d69f5490ab0a426268 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 8 Sep 2016 11:41:54 +0200 Subject: [PATCH 429/456] Revert "Slight notch default shift" This reverts commit 57c954e6088b90048a0223a853a6e8a91c4e2f76. --- src/main/config/config.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 417a8b4969..3e058a6fec 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -240,8 +240,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yawItermIgnoreRate = 32; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default - pidProfile->dterm_notch_hz = 250; - pidProfile->dterm_notch_cutoff = 150; + pidProfile->dterm_notch_hz = 260; + pidProfile->dterm_notch_cutoff = 160; pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->vbatPidCompensation = 0; pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; From 5980c087b4a030d70ab4df40631191725bb84709 Mon Sep 17 00:00:00 2001 From: hrrr Date: Thu, 8 Sep 2016 19:06:45 +0200 Subject: [PATCH 430/456] Sparky 2 beeper correction --- src/main/target/SPARKY2/target.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index febebfa1fb..1b9a66974a 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -30,6 +30,8 @@ #define LED2 PB6 #define BEEPER PC9 +#define BEEPER_INVERTED + #define INVERTER PC6 #define INVERTER_USART USART6 From 0d1cc8f4484e9c5149b4471fdcf16a9e2c34e1bb Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 8 Sep 2016 16:55:34 +0200 Subject: [PATCH 431/456] Reimplementation of P setpoint weight. Applied to S rates now for nicer feel --- src/main/blackbox/blackbox.c | 2 +- src/main/config/config.c | 10 +++++----- src/main/flight/pid.c | 11 +++-------- src/main/flight/pid.h | 2 +- src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 4 ++-- src/main/mw.c | 24 ++++++++++++++++++------ 7 files changed, 31 insertions(+), 24 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b9d60e1c10..5c0ca9f89a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1283,7 +1283,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("toleranceBandReduction:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.toleranceBandReduction); BLACKBOX_PRINT_HEADER_LINE("zeroCrossAllowanceCount:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.zeroCrossAllowanceCount); BLACKBOX_PRINT_HEADER_LINE("itermThrottleGain:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.itermThrottleGain); - BLACKBOX_PRINT_HEADER_LINE("ptermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSetpointWeight); + BLACKBOX_PRINT_HEADER_LINE("ptermSRateWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSRateWeight); BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dtermSetpointWeight); BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawRateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rateAccelLimit); diff --git a/src/main/config/config.c b/src/main/config/config.c index 3e058a6fec..ffb4367c95 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 145; +static const uint8_t EEPROM_CONF_VERSION = 146; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -235,7 +235,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 75; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; - pidProfile->yaw_lpf_hz = 80; + pidProfile->yaw_lpf_hz = 0; pidProfile->rollPitchItermIgnoreRate = 130; pidProfile->yawItermIgnoreRate = 32; pidProfile->dterm_filter_type = FILTER_BIQUAD; @@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->ptermSetpointWeight = 85; + pidProfile->ptermSRateWeight = 50; pidProfile->dtermSetpointWeight = 150; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; @@ -491,8 +491,8 @@ void createDefaultConfig(master_t *config) #endif config->gyro_soft_type = FILTER_PT1; config->gyro_soft_lpf_hz = 90; - config->gyro_soft_notch_hz = 0; - config->gyro_soft_notch_cutoff = 150; + config->gyro_soft_notch_hz = 210; + config->gyro_soft_notch_cutoff = 110; config->debug_mode = DEBUG_NONE; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8950db6ab1..234b98462b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -49,7 +49,7 @@ extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float setpointRate[3]; +extern float setpointRate[3], ptermSetpointRate[3]; extern float rcInput[3]; static bool pidStabilisationEnabled; @@ -135,7 +135,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; + static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; float delta; int axis; float horizonLevelStrength = 1; @@ -187,7 +187,6 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; - b[axis] = pidProfile->ptermSetpointWeight / 100.0f; c[axis] = pidProfile->dtermSetpointWeight / 100.0f; yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT(); rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT(); @@ -234,11 +233,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // ----- calculate error / angle rates ---------- errorRate = setpointRate[axis] - PVRate; // r - y - rP = b[axis] * setpointRate[axis] - PVRate; // br - y - - // Slowly restore original setpoint with more stick input - float diffRate = errorRate - rP; - rP += diffRate * rcInput[axis]; + rP = ptermSetpointRate[axis] - PVRate; // br - y // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount float dynReduction = tpaFactor; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index fd1fb08f5a..56082360eb 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -98,7 +98,7 @@ typedef struct pidProfile_s { uint8_t toleranceBandReduction; // Lowest possible P and D reduction in percentage uint8_t zeroCrossAllowanceCount; // Amount of bouncebacks within tolerance band allowed before reduction kicks in uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm - uint8_t ptermSetpointWeight; // Setpoint weight for Pterm (lower means more PV tracking) + uint8_t ptermSRateWeight; // Setpoint super expo ratio for Pterm (lower means that pretty much only P has super expo rates) uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 0b64d0526f..6099732163 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -854,7 +854,7 @@ const clivalue_t valueTable[] = { { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, - { "pterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {40, 100 } }, + { "pterm_srate_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSRateWeight, .config.minmax = {0, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 33bd4a74c9..5d1367f999 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1268,7 +1268,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentProfile->pidProfile.yaw_p_limit); serialize8(currentProfile->pidProfile.deltaMethod); serialize8(currentProfile->pidProfile.vbatPidCompensation); - serialize8(currentProfile->pidProfile.ptermSetpointWeight); + serialize8(currentProfile->pidProfile.ptermSRateWeight); serialize8(currentProfile->pidProfile.dtermSetpointWeight); serialize8(currentProfile->pidProfile.toleranceBand); serialize8(currentProfile->pidProfile.toleranceBandReduction); @@ -1866,7 +1866,7 @@ static bool processInCommand(void) currentProfile->pidProfile.yaw_p_limit = read16(); currentProfile->pidProfile.deltaMethod = read8(); currentProfile->pidProfile.vbatPidCompensation = read8(); - currentProfile->pidProfile.ptermSetpointWeight = read8(); + currentProfile->pidProfile.ptermSRateWeight = read8(); currentProfile->pidProfile.dtermSetpointWeight = read8(); currentProfile->pidProfile.toleranceBand = read8(); currentProfile->pidProfile.toleranceBandReduction = read8(); diff --git a/src/main/mw.c b/src/main/mw.c index faed2088b0..de92425d56 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -123,7 +123,7 @@ extern uint8_t PIDweight[3]; uint16_t filteredCycleTime; static bool isRXDataNew; static bool armingCalibrationWasInitialised; -float setpointRate[3]; +float setpointRate[3], ptermSetpointRate[3]; float rcInput[3]; extern pidControllerFuncPtr pid_controller; @@ -176,7 +176,7 @@ bool isCalibrating() #define RC_RATE_INCREMENTAL 14.54f #define RC_EXPO_POWER 3 -float calculateSetpointRate(int axis, int16_t rc) { +void calculateSetpointRate(int axis, int16_t rc) { float angleRate, rcRate, rcSuperfactor, rcCommandf; uint8_t rcExpo; @@ -201,7 +201,19 @@ float calculateSetpointRate(int axis, int16_t rc) { if (currentControlRateProfile->rates[axis]) { rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); - angleRate *= rcSuperfactor; + if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) { + ptermSetpointRate[axis] = angleRate * rcSuperfactor; + if (currentProfile->pidProfile.ptermSRateWeight < 100) { + const float pWeight = currentProfile->pidProfile.ptermSRateWeight / 100.0f; + angleRate = angleRate + (pWeight * ptermSetpointRate[axis] - angleRate); + } else { + angleRate = ptermSetpointRate[axis]; + } + } else { + angleRate *= rcSuperfactor; + } + } else { + if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) ptermSetpointRate[axis] = angleRate; } if (debugMode == DEBUG_ANGLERATE) { @@ -209,9 +221,9 @@ float calculateSetpointRate(int axis, int16_t rc) { } if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) - return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection + setpointRate[axis] = constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection else - return constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) + setpointRate[axis] = constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) } void scaleRcCommandToFpvCamAngle(void) { @@ -290,7 +302,7 @@ void processRcCommand(void) if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) scaleRcCommandToFpvCamAngle(); - for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); + for (int axis = 0; axis < 3; axis++) calculateSetpointRate(axis, rcCommand[axis]); isRXDataNew = false; } From 13b189b4ffbd877012632ab0ed6fe64c26abaab7 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 8 Sep 2016 22:37:35 +0200 Subject: [PATCH 432/456] Cleanup --- src/main/blackbox/blackbox.c | 3 --- src/main/config/config.c | 7 ++----- src/main/flight/pid.c | 34 +++------------------------------- src/main/flight/pid.h | 8 -------- src/main/io/serial_cli.c | 3 --- src/main/io/serial_msp.c | 8 ++++---- 6 files changed, 9 insertions(+), 54 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5c0ca9f89a..78d5ffc821 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1279,9 +1279,6 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidAtMinThrottle); // Betaflight PID controller parameters - BLACKBOX_PRINT_HEADER_LINE("toleranceBand:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.toleranceBand); - BLACKBOX_PRINT_HEADER_LINE("toleranceBandReduction:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.toleranceBandReduction); - BLACKBOX_PRINT_HEADER_LINE("zeroCrossAllowanceCount:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.zeroCrossAllowanceCount); BLACKBOX_PRINT_HEADER_LINE("itermThrottleGain:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.itermThrottleGain); BLACKBOX_PRINT_HEADER_LINE("ptermSRateWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSRateWeight); BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dtermSetpointWeight); diff --git a/src/main/config/config.c b/src/main/config/config.c index ffb4367c95..22cd6b94f1 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -247,13 +247,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->ptermSRateWeight = 50; + pidProfile->ptermSRateWeight = 100; pidProfile->dtermSetpointWeight = 150; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; - pidProfile->toleranceBand = 0; - pidProfile->toleranceBandReduction = 40; - pidProfile->zeroCrossAllowanceCount = 2; pidProfile->itermThrottleGain = 0; #ifdef GTUNE @@ -491,7 +488,7 @@ void createDefaultConfig(master_t *config) #endif config->gyro_soft_type = FILTER_PT1; config->gyro_soft_lpf_hz = 90; - config->gyro_soft_notch_hz = 210; + config->gyro_soft_notch_hz = 220; config->gyro_soft_notch_cutoff = 110; config->debug_mode = DEBUG_NONE; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 234b98462b..523a50fc0c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -235,36 +235,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc errorRate = setpointRate[axis] - PVRate; // r - y rP = ptermSetpointRate[axis] - PVRate; // br - y - // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount - float dynReduction = tpaFactor; - if (pidProfile->toleranceBand) { - const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; - static uint8_t zeroCrossCount[3]; - static uint8_t currentErrorPolarity[3]; - if (ABS(errorRate) < pidProfile->toleranceBand) { - if (zeroCrossCount[axis]) { - if (currentErrorPolarity[axis] == POSITIVE_ERROR) { - if (errorRate < 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = NEGATIVE_ERROR; - } - } else { - if (errorRate > 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = POSITIVE_ERROR; - } - } - } else { - dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); - } - } else { - zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; - currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; - } - } - // -----calculate P component - PTerm = Kp[axis] * rP * dynReduction; + PTerm = Kp[axis] * rP * tpaFactor; // -----calculate I component. // Reduce strong Iterm accumulation during higher stick inputs @@ -289,7 +261,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // Divide delta by targetLooptime to get differential (ie dr/dt) delta *= (1.0f / getdT()); - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor; // Filter delta if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); @@ -302,7 +274,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc } } - DTerm = Kd[axis] * delta * dynReduction; + DTerm = Kd[axis] * delta * tpaFactor; // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 56082360eb..df266e3273 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -63,11 +63,6 @@ typedef enum { SUPEREXPO_YAW_ALWAYS } pidSuperExpoYaw_e; -typedef enum { - NEGATIVE_ERROR = 0, - POSITIVE_ERROR -} pidErrorPolarity_e; - typedef enum { PID_STABILISATION_OFF = 0, PID_STABILISATION_ON @@ -94,9 +89,6 @@ typedef struct pidProfile_s { uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. // Betaflight PID controller parameters - uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances - uint8_t toleranceBandReduction; // Lowest possible P and D reduction in percentage - uint8_t zeroCrossAllowanceCount; // Amount of bouncebacks within tolerance band allowed before reduction kicks in uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm uint8_t ptermSRateWeight; // Setpoint super expo ratio for Pterm (lower means that pretty much only P has super expo rates) uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 6099732163..6cbb66ee5c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -850,9 +850,6 @@ const clivalue_t valueTable[] = { { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 500 } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, - { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, - { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, - { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, { "pterm_srate_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSRateWeight, .config.minmax = {0, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 5d1367f999..9b40111dbd 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1270,8 +1270,8 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentProfile->pidProfile.vbatPidCompensation); serialize8(currentProfile->pidProfile.ptermSRateWeight); serialize8(currentProfile->pidProfile.dtermSetpointWeight); - serialize8(currentProfile->pidProfile.toleranceBand); - serialize8(currentProfile->pidProfile.toleranceBandReduction); + serialize8(0); // reserved + serialize8(0); // reserved serialize8(currentProfile->pidProfile.itermThrottleGain); serialize16(currentProfile->pidProfile.rateAccelLimit); serialize16(currentProfile->pidProfile.yawRateAccelLimit); @@ -1868,8 +1868,8 @@ static bool processInCommand(void) currentProfile->pidProfile.vbatPidCompensation = read8(); currentProfile->pidProfile.ptermSRateWeight = read8(); currentProfile->pidProfile.dtermSetpointWeight = read8(); - currentProfile->pidProfile.toleranceBand = read8(); - currentProfile->pidProfile.toleranceBandReduction = read8(); + read8(); // reserved + read8(); // reserved currentProfile->pidProfile.itermThrottleGain = read8(); currentProfile->pidProfile.rateAccelLimit = read16(); currentProfile->pidProfile.yawRateAccelLimit = read16(); From 389a27e3f156b6cb501bb28bfdfe6184d6d75bf9 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 9 Sep 2016 09:21:49 +0200 Subject: [PATCH 433/456] Remove gyro notch as default // Remove yaw srate ratio --- src/main/config/config.c | 6 +++--- src/main/mw.c | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 22cd6b94f1..28b4dbc0e9 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->ptermSRateWeight = 100; + pidProfile->ptermSRateWeight = 85; pidProfile->dtermSetpointWeight = 150; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; @@ -488,8 +488,8 @@ void createDefaultConfig(master_t *config) #endif config->gyro_soft_type = FILTER_PT1; config->gyro_soft_lpf_hz = 90; - config->gyro_soft_notch_hz = 220; - config->gyro_soft_notch_cutoff = 110; + config->gyro_soft_notch_hz = 0; + config->gyro_soft_notch_cutoff = 130; config->debug_mode = DEBUG_NONE; diff --git a/src/main/mw.c b/src/main/mw.c index de92425d56..9c7bf171e7 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -203,7 +203,7 @@ void calculateSetpointRate(int axis, int16_t rc) { rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) { ptermSetpointRate[axis] = angleRate * rcSuperfactor; - if (currentProfile->pidProfile.ptermSRateWeight < 100) { + if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW) { const float pWeight = currentProfile->pidProfile.ptermSRateWeight / 100.0f; angleRate = angleRate + (pWeight * ptermSetpointRate[axis] - angleRate); } else { From cab7b562d8dec756d925380162f47a93f1e5e0eb Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 9 Sep 2016 09:55:46 +0200 Subject: [PATCH 434/456] Change gyro filter / debug order --- src/main/sensors/gyro.c | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a17b61df20..7c39168568 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -158,7 +158,6 @@ void gyroUpdate(void) } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis]; gyroADC[axis] = gyroADCRaw[axis]; } @@ -172,21 +171,21 @@ void gyroUpdate(void) if (gyroSoftLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - float sample = (float) gyroADC[axis]; - if (gyroSoftNotchHz) { - sample = biquadFilterApply(&gyroFilterNotch[axis], sample); - } - if (debugMode == DEBUG_NOTCH && axis < 2){ - debug[axis*2 + 0] = gyroADC[axis]; - debug[axis*2 + 1] = lrintf(sample); - } + if (debugMode == DEBUG_GYRO) + debug[axis] = gyroADC[axis]; + + if (gyroSoftLpfType == FILTER_BIQUAD) + gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]); + else + gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt); + + if (debugMode == DEBUG_NOTCH) + debug[axis] = lrintf(gyroADCf[axis]); + + if (gyroSoftNotchHz) + gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch[axis], gyroADCf[axis]); - if (gyroSoftLpfType == FILTER_BIQUAD) { - gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample); - } else { - gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], sample, gyroSoftLpfHz, gyroDt); - } gyroADC[axis] = lrintf(gyroADCf[axis]); } } else { From c595a789fcdf4347d6d1a3fbcbeeb75df4bac05f Mon Sep 17 00:00:00 2001 From: TheAngularity Date: Fri, 9 Sep 2016 10:25:48 +0200 Subject: [PATCH 435/456] removed MAP_TO_SERVO_OUTPUT on PWM8/9 probably it fixed #1129 --- src/main/target/SPRACINGF3EVO/target.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 488e9a7a9c..843ad3d9aa 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -30,8 +30,8 @@ const uint16_t multiPPM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF @@ -44,8 +44,8 @@ const uint16_t multiPWM[] = { PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF From cfa0d686790358cf6df1557b06a15dc346da689c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 9 Sep 2016 11:38:46 +0200 Subject: [PATCH 436/456] Match rate limit for ptermSetpointRate --- src/main/mw.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/mw.c b/src/main/mw.c index 9c7bf171e7..54b605a120 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -202,7 +202,7 @@ void calculateSetpointRate(int axis, int16_t rc) { if (currentControlRateProfile->rates[axis]) { rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) { - ptermSetpointRate[axis] = angleRate * rcSuperfactor; + ptermSetpointRate[axis] = constrainf(angleRate * rcSuperfactor, -1998.0f, 1998.0f); if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW) { const float pWeight = currentProfile->pidProfile.ptermSRateWeight / 100.0f; angleRate = angleRate + (pWeight * ptermSetpointRate[axis] - angleRate); From 0720fcf463f1671259fcecbaa8f14c9ad938cf3e Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 9 Sep 2016 12:04:31 +0200 Subject: [PATCH 437/456] Add Back Some Dirty target code See #1131 #1127 --- src/main/drivers/pwm_mapping.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 2a484352e5..08e0ec0bcc 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -242,6 +242,12 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } #endif +#if defined(SPRACINGF3EVO) + // remap PWM6+7 as servos + if ((timerIndex == PWM8 || timerIndex == PWM9) && timerHardwarePtr->tim == TIM3) + type = MAP_TO_SERVO_OUTPUT; +#endif + #if (defined(STM32F3DISCOVERY) && !defined(CHEBUZZF3)) // remap PWM 5+6 or 9+10 as servos - softserial pin pairs require timer ports that use the same timer if (init->useSoftSerial) { From a60cb8597f60b78e60fd8694075af7a5f156d500 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 10 Sep 2016 16:22:44 +0100 Subject: [PATCH 438/456] Rename serial MSP functions in prep for converting to using streambuf --- src/main/blackbox/blackbox_io.c | 2 +- src/main/fc/mw.c | 6 +++--- src/main/io/serial.c | 2 +- src/main/io/serial_msp.c | 10 +++++----- src/main/io/serial_msp.h | 8 ++++---- src/main/main.c | 2 +- 6 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 8f7d81ac24..44bd59cbd5 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -650,7 +650,7 @@ void blackboxDeviceClose(void) * of time to shut down asynchronously, we're the only ones that know when to call it. */ if (blackboxPortSharing == PORTSHARING_SHARED) { - mspAllocateSerialPorts(&masterConfig.serialConfig); + mspSerialAllocatePorts(&masterConfig.serialConfig); } break; default: diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 5dc03e2fb5..6f2a48e76f 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -410,7 +410,7 @@ void mwDisarm(void) void releaseSharedTelemetryPorts(void) { serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); while (sharedPort) { - mspReleasePortIfAllocated(sharedPort); + mspSerialReleasePortIfAllocated(sharedPort); sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); } } @@ -443,7 +443,7 @@ void mwArm(void) if (feature(FEATURE_BLACKBOX)) { serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); if (sharedBlackboxAndMspPort) { - mspReleasePortIfAllocated(sharedBlackboxAndMspPort); + mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort); } startBlackbox(); } @@ -702,7 +702,7 @@ void processRx(void) } else { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); - mspAllocateSerialPorts(&masterConfig.serialConfig); + mspSerialAllocatePorts(&masterConfig.serialConfig); } } #endif diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 83eb4260ed..e5ef5da9f2 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -433,7 +433,7 @@ void handleSerial(void) } #endif - mspProcess(); + mspSerialProcess(); } void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort) diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 50ee576d64..d00d5d8ffe 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -428,7 +428,7 @@ static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort) mspPortToReset->port = serialPort; } -void mspAllocateSerialPorts(serialConfig_t *serialConfig) +void mspSerialAllocatePorts(serialConfig_t *serialConfig) { UNUSED(serialConfig); @@ -455,7 +455,7 @@ void mspAllocateSerialPorts(serialConfig_t *serialConfig) } } -void mspReleasePortIfAllocated(serialPort_t *serialPort) +void mspSerialReleasePortIfAllocated(serialPort_t *serialPort) { uint8_t portIndex; for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { @@ -467,7 +467,7 @@ void mspReleasePortIfAllocated(serialPort_t *serialPort) } } -void mspInit(serialConfig_t *serialConfig) +void mspSerialInit(serialConfig_t *serialConfig) { // calculate used boxes based on features and fill availableBoxes[] array memset(activeBoxIds, 0xFF, sizeof(activeBoxIds)); @@ -566,7 +566,7 @@ void mspInit(serialConfig_t *serialConfig) #endif memset(mspPorts, 0x00, sizeof(mspPorts)); - mspAllocateSerialPorts(serialConfig); + mspSerialAllocatePorts(serialConfig); } #define IS_ENABLED(mask) (mask == 0 ? 0 : 1) @@ -1959,7 +1959,7 @@ STATIC_UNIT_TESTED void setCurrentPort(mspPort_t *port) mspSerialPort = currentPort->port; } -void mspProcess(void) +void mspSerialProcess(void) { uint8_t portIndex; mspPort_t *candidatePort; diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 114e94ab84..a1c7cdbdf9 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -44,8 +44,8 @@ typedef struct mspPort_s { } mspPort_t; struct serialConfig_s; -void mspInit(struct serialConfig_s *serialConfig); -void mspProcess(void); -void mspAllocateSerialPorts(struct serialConfig_s *serialConfig); +void mspSerialInit(struct serialConfig_s *serialConfig); +void mspSerialProcess(void); +void mspSerialAllocatePorts(struct serialConfig_s *serialConfig); struct serialPort_s; -void mspReleasePortIfAllocated(struct serialPort_s *serialPort); +void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); diff --git a/src/main/main.c b/src/main/main.c index 98d06a167e..a5a3facb3b 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -504,7 +504,7 @@ void init(void) imuInit(); - mspInit(&masterConfig.serialConfig); + mspSerialInit(&masterConfig.serialConfig); #ifdef USE_CLI cliInit(&masterConfig.serialConfig); From e0b9a152b760beec97cd4010f934da9663ad83b1 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 10 Sep 2016 16:34:22 +0100 Subject: [PATCH 439/456] Add streambuf code from Cleanflight --- src/main/common/streambuf.c | 106 ++++++++++++++++++++++++++++++++++++ src/main/common/streambuf.h | 45 +++++++++++++++ 2 files changed, 151 insertions(+) create mode 100644 src/main/common/streambuf.c create mode 100644 src/main/common/streambuf.h diff --git a/src/main/common/streambuf.c b/src/main/common/streambuf.c new file mode 100644 index 0000000000..0f47511e03 --- /dev/null +++ b/src/main/common/streambuf.c @@ -0,0 +1,106 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "streambuf.h" + +void sbufWriteU8(sbuf_t *dst, uint8_t val) +{ + *dst->ptr++ = val; +} + +void sbufWriteU16(sbuf_t *dst, uint16_t val) +{ + sbufWriteU8(dst, val >> 0); + sbufWriteU8(dst, val >> 8); +} + +void sbufWriteU32(sbuf_t *dst, uint32_t val) +{ + sbufWriteU8(dst, val >> 0); + sbufWriteU8(dst, val >> 8); + sbufWriteU8(dst, val >> 16); + sbufWriteU8(dst, val >> 24); +} + +void sbufWriteData(sbuf_t *dst, const void *data, int len) +{ + memcpy(dst->ptr, data, len); + dst->ptr += len; +} + +void sbufWriteString(sbuf_t *dst, const char *string) +{ + sbufWriteData(dst, string, strlen(string)); +} + +uint8_t sbufReadU8(sbuf_t *src) +{ + return *src->ptr++; +} + +uint16_t sbufReadU16(sbuf_t *src) +{ + uint16_t ret; + ret = sbufReadU8(src); + ret |= sbufReadU8(src) << 8; + return ret; +} + +uint32_t sbufReadU32(sbuf_t *src) +{ + uint32_t ret; + ret = sbufReadU8(src); + ret |= sbufReadU8(src) << 8; + ret |= sbufReadU8(src) << 16; + ret |= sbufReadU8(src) << 24; + return ret; +} + +void sbufReadData(sbuf_t *src, void *data, int len) +{ + memcpy(data, src->ptr, len); +} + +// reader - return bytes remaining in buffer +// writer - return available space +int sbufBytesRemaining(sbuf_t *buf) +{ + return buf->end - buf->ptr; +} + +uint8_t* sbufPtr(sbuf_t *buf) +{ + return buf->ptr; +} + +// advance buffer pointer +// reader - skip data +// writer - commit written data +void sbufAdvance(sbuf_t *buf, int size) +{ + buf->ptr += size; +} + +// modifies streambuf so that written data are prepared for reading +void sbufSwitchToReader(sbuf_t *buf, uint8_t *base) +{ + buf->end = buf->ptr; + buf->ptr = base; +} diff --git a/src/main/common/streambuf.h b/src/main/common/streambuf.h new file mode 100644 index 0000000000..7de771ef02 --- /dev/null +++ b/src/main/common/streambuf.h @@ -0,0 +1,45 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include + +// simple buffer-based serializer/deserializer without implicit size check +// little-endian encoding implemneted now + +typedef struct sbuf_s { + uint8_t *ptr; // data pointer must be first (sbuff_t* is equivalent to uint8_t **) + uint8_t *end; +} sbuf_t; + +void sbufWriteU8(sbuf_t *dst, uint8_t val); +void sbufWriteU16(sbuf_t *dst, uint16_t val); +void sbufWriteU32(sbuf_t *dst, uint32_t val); +void sbufWriteData(sbuf_t *dst, const void *data, int len); +void sbufWriteString(sbuf_t *dst, const char *string); + +uint8_t sbufReadU8(sbuf_t *src); +uint16_t sbufReadU16(sbuf_t *src); +uint32_t sbufReadU32(sbuf_t *src); +void sbufReadData(sbuf_t *dst, void *data, int len); + +int sbufBytesRemaining(sbuf_t *buf); +uint8_t* sbufPtr(sbuf_t *buf); +void sbufAdvance(sbuf_t *buf, int size); + +void sbufSwitchToReader(sbuf_t *buf, uint8_t * base); From 977a64d4a5090ec93ae68938d6fb47092c1b6669 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 10 Sep 2016 16:37:05 +0100 Subject: [PATCH 440/456] Added streambuf to makefile --- Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/Makefile b/Makefile index 8cb92e667d..e1f49347ac 100644 --- a/Makefile +++ b/Makefile @@ -382,6 +382,7 @@ COMMON_SRC = \ common/filter.c \ common/maths.c \ common/printf.c \ + common/streambuf.c \ common/typeconversion.c \ config/config.c \ config/config_eeprom.c \ From cb16d65532bdb582e51c8fa9364a6bff1524af27 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sat, 10 Sep 2016 23:29:48 +0200 Subject: [PATCH 441/456] Fix level mode in betaflight PID controller --- src/main/flight/pid.c | 4 ++-- src/main/mw.c | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 523a50fc0c..39fa4b8064 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -218,11 +218,11 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc #endif if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed - setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; + ptermSetpointRate[axis] = setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; } else { // HORIZON mode - direct sticks control is applied to rate PID // mix up angle error to desired AngleRate to add a little auto-level feel - setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); + ptermSetpointRate[axis] = setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); } } diff --git a/src/main/mw.c b/src/main/mw.c index 54b605a120..b112aa6678 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -203,7 +203,7 @@ void calculateSetpointRate(int axis, int16_t rc) { rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) { ptermSetpointRate[axis] = constrainf(angleRate * rcSuperfactor, -1998.0f, 1998.0f); - if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW) { + if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW && !flightModeFlags) { const float pWeight = currentProfile->pidProfile.ptermSRateWeight / 100.0f; angleRate = angleRate + (pWeight * ptermSetpointRate[axis] - angleRate); } else { From a21694b065e6305082925e6fee81d242535110b1 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 11 Sep 2016 00:22:05 +0200 Subject: [PATCH 442/456] Enable Telemetry Inversion by Default --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 28b4dbc0e9..90effae403 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -331,7 +331,7 @@ void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) #ifdef TELEMETRY void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) { - telemetryConfig->telemetry_inversion = 0; + telemetryConfig->telemetry_inversion = 1; telemetryConfig->telemetry_switch = 0; telemetryConfig->gpsNoFixLatitude = 0; telemetryConfig->gpsNoFixLongitude = 0; From 1a34f63056cc078b7b1ae2f2e1991bbace129469 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Sun, 11 Sep 2016 15:34:27 +0200 Subject: [PATCH 443/456] Added missing timer.h include. --- src/main/target/BETAFLIGHTF3/target.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/BETAFLIGHTF3/target.c b/src/main/target/BETAFLIGHTF3/target.c index 9c177c4baa..b96615c6a6 100755 --- a/src/main/target/BETAFLIGHTF3/target.c +++ b/src/main/target/BETAFLIGHTF3/target.c @@ -20,6 +20,7 @@ #include #include "drivers/io.h" +#include "drivers/timer.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { From f04b59a11b325d4c4f943df5031394feca6e623e Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Sun, 11 Sep 2016 15:55:21 +0200 Subject: [PATCH 444/456] Added BETAFLIGHTF3 to the Travis test builds. Removed STM32F3DISCOVERY. --- .travis.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 80b896b6a6..77c4b01891 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,6 +11,7 @@ env: - TARGET=BLUEJAYF4 - TARGET=CC3D - TARGET=CC3D_OPBL + - TARGET=BETAFLIGHTF3 # - TARGET=CHEBUZZF3 # - TARGET=CJMCU # - TARGET=COLIBRI @@ -43,7 +44,7 @@ env: - TARGET=SPRACINGF3 - TARGET=SPRACINGF3EVO # - TARGET=SPRACINGF3MINI - - TARGET=STM32F3DISCOVERY +# - TARGET=STM32F3DISCOVERY # - TARGET=VRRACE # - TARGET=X_RACERSPI # - TARGET=ZCOREF3 From bb70ae35d68d151808c9448e54d23d9f54def3c5 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Sun, 11 Sep 2016 16:06:55 +0200 Subject: [PATCH 445/456] Target order. --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 77c4b01891..7915db371e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -8,10 +8,10 @@ env: # - TARGET=ALIENFLIGHTF1 - TARGET=ALIENFLIGHTF3 - TARGET=ALIENFLIGHTF4 + - TARGET=BETAFLIGHTF3 - TARGET=BLUEJAYF4 - TARGET=CC3D - TARGET=CC3D_OPBL - - TARGET=BETAFLIGHTF3 # - TARGET=CHEBUZZF3 # - TARGET=CJMCU # - TARGET=COLIBRI From bb73fb816c492f85a329e0287fc1e69e192f5b26 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Sun, 11 Sep 2016 17:11:06 +0200 Subject: [PATCH 446/456] Fixed RACEBASE includes. --- src/main/target/RACEBASE/config.c | 9 +++++---- src/main/target/RACEBASE/target.c | 1 + 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/target/RACEBASE/config.c b/src/main/target/RACEBASE/config.c index 0ad304995c..65147d3db2 100755 --- a/src/main/target/RACEBASE/config.c +++ b/src/main/target/RACEBASE/config.c @@ -19,7 +19,7 @@ #include -#include "build_config.h" +#include "build/build_config.h" #include "blackbox/blackbox_io.h" @@ -39,6 +39,10 @@ #include "drivers/io.h" #include "drivers/pwm_mapping.h" +#include "fc/rc_controls.h" +#include "fc/rc_curves.h" +#include "fc/runtime_config.h" + #include "sensors/sensors.h" #include "sensors/gyro.h" #include "sensors/compass.h" @@ -51,8 +55,6 @@ #include "io/serial.h" #include "io/gimbal.h" #include "io/escservo.h" -#include "io/rc_controls.h" -#include "io/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" #include "io/osd.h" @@ -69,7 +71,6 @@ #include "flight/altitudehold.h" #include "flight/navigation.h" -#include "config/runtime_config.h" #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/target/RACEBASE/target.c b/src/main/target/RACEBASE/target.c index 0b6a8f1695..e1386f9617 100755 --- a/src/main/target/RACEBASE/target.c +++ b/src/main/target/RACEBASE/target.c @@ -19,6 +19,7 @@ #include #include "drivers/io.h" +#include "drivers/timer.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { From 65ca99af117216c834f95127c69ccf98a892ce05 Mon Sep 17 00:00:00 2001 From: Sean Blakemore Date: Mon, 12 Sep 2016 15:15:34 +1000 Subject: [PATCH 447/456] IMPULSERCF3 target added for Helix flight controller --- src/main/target/IMPULSERCF3/target.c | 77 ++++++++++++++++++ src/main/target/IMPULSERCF3/target.h | 113 ++++++++++++++++++++++++++ src/main/target/IMPULSERCF3/target.mk | 10 +++ 3 files changed, 200 insertions(+) create mode 100644 src/main/target/IMPULSERCF3/target.c create mode 100644 src/main/target/IMPULSERCF3/target.h create mode 100644 src/main/target/IMPULSERCF3/target.mk diff --git a/src/main/target/IMPULSERCF3/target.c b/src/main/target/IMPULSERCF3/target.c new file mode 100644 index 0000000000..6c98e152c7 --- /dev/null +++ b/src/main/target/IMPULSERCF3/target.c @@ -0,0 +1,77 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" + + +const uint16_t multiPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t multiPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPPM[] = { + PWM1 | (MAP_TO_PPM_INPUT << 8), + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const uint16_t airPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + 0xFFFF +}; + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1}, // PPM/SERIAL RX + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM1 + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM2 + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM3 + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2}, // PWM4 + { TIM16, IO_TAG(PB8), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM5 + { TIM17, IO_TAG(PB9), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1}, // PWM6 + { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6}, // LED_STRIP +}; + diff --git a/src/main/target/IMPULSERCF3/target.h b/src/main/target/IMPULSERCF3/target.h new file mode 100644 index 0000000000..f293c070b3 --- /dev/null +++ b/src/main/target/IMPULSERCF3/target.h @@ -0,0 +1,113 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "IMF3" + +#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE + +#define LED0 PB7 + +#define BEEPER PC15 + +#define USABLE_TIMER_CHANNEL_COUNT 8 + +#define USB_IO + +#define USE_EXTI +#define MPU_INT_EXTI PC13 +#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW + +#define GYRO +#define USE_GYRO_SPI_MPU6000 +#define GYRO_MPU6000_ALIGN CW0_DEG + +#define ACC +#define USE_ACC_SPI_MPU6000 +#define ACC_MPU6000_ALIGN CW0_DEG + +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define SERIAL_PORT_COUNT 4 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA14 +#define UART2_RX_PIN PA15 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_SPI +#define USE_SPI_DEVICE_1 // PA4, 5, 6, 7 +#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 + +#define M25P16_CS_GPIO GPIOB +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_INSTANCE SPI1 + +#define BOARD_HAS_VOLTAGE_DIVIDER +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define CURRENT_METER_ADC_PIN PA0 +#define RSSI_ADC_PIN PA1 +#define VBAT_ADC_PIN PA2 + +#define LED_STRIP + +#define USE_LED_STRIP_ON_DMA1_CHANNEL2 +#define WS2811_PIN PA8 +#define WS2811_TIMER TIM1 +#define WS2811_DMA_CHANNEL DMA1_Channel2 +#define WS2811_IRQ DMA1_Channel2_IRQn +#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART3 + +#define SPEKTRUM_BIND +// USART2, PA15 +#define BIND_PIN PA15 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +// IO - stm32f303cc in 48pin package +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) +//#define TARGET_IO_PORTF (BIT(0)|BIT(1)) +// !!TODO - check the following line is correct +#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) + +#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(16) |TIM_N(17)) + diff --git a/src/main/target/IMPULSERCF3/target.mk b/src/main/target/IMPULSERCF3/target.mk new file mode 100644 index 0000000000..ec4b5802f2 --- /dev/null +++ b/src/main/target/IMPULSERCF3/target.mk @@ -0,0 +1,10 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/flash_m25p16.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f30x.c + From a4f7144d65025016e3ac44ddccaf759f52598c89 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Mon, 12 Sep 2016 10:58:42 +0200 Subject: [PATCH 448/456] Travis. Re-enabled STM32F3DISCOVERY. Added a few missing targets, all disabled though. --- .travis.yml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 7915db371e..35333fbe8f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,6 +5,7 @@ env: # - TARGET=AFROMINI # - TARGET=AIORACERF3 # - TARGET=AIR32 +# - TARGET=AIRHEROF3 # - TARGET=ALIENFLIGHTF1 - TARGET=ALIENFLIGHTF3 - TARGET=ALIENFLIGHTF4 @@ -15,6 +16,7 @@ env: # - TARGET=CHEBUZZF3 # - TARGET=CJMCU # - TARGET=COLIBRI +# - TARGET=COLIBRI_OPBL # - TARGET=COLIBRI_RACE # - TARGET=DOGE # - TARGET=EUSTM32F103RC @@ -22,6 +24,7 @@ env: # - TARGET=FURYF3 - TARGET=FURYF4 # - TARGET=IRCFUSIONF3 +# - TARGET=ISHAPEDF3 # - TARGET=KISSFC # - TARGET=LUX_RACE # - TARGET=MICROSCISKY @@ -32,6 +35,7 @@ env: # - TARGET=OMNIBUSF4 # - TARGET=PIKOBLX # - TARGET=PORT103R +# - TARGET=RACEBASE - TARGET=REVO # - TARGET=REVONANO # - TARGET=REVO_OPBL @@ -44,7 +48,7 @@ env: - TARGET=SPRACINGF3 - TARGET=SPRACINGF3EVO # - TARGET=SPRACINGF3MINI -# - TARGET=STM32F3DISCOVERY + - TARGET=STM32F3DISCOVERY # - TARGET=VRRACE # - TARGET=X_RACERSPI # - TARGET=ZCOREF3 From 4d03791cbc2ad730914f8f27daf24d4cb6da7a05 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 21 Aug 2016 08:15:18 +0100 Subject: [PATCH 449/456] Removed unused EXTI_CALLBACK_HANDLER_COUNT --- src/main/drivers/system.c | 19 ----- src/main/drivers/system.h | 17 +---- src/main/drivers/system_stm32f10x.c | 2 - src/main/drivers/system_stm32f30x.c | 2 - src/main/drivers/system_stm32f4xx.c | 2 - src/main/target/AIRHEROF3/target.h | 1 - src/main/target/ALIENFLIGHTF1/target.h | 4 +- src/main/target/ALIENFLIGHTF3/target.h | 2 - src/main/target/ALIENFLIGHTF4/target.h | 1 - src/main/target/BLUEJAYF4/target.h | 8 +-- src/main/target/COLIBRI/target.h | 98 +++++++++++++------------- src/main/target/COLIBRI_RACE/target.h | 1 - src/main/target/DOGE/target.h | 4 +- src/main/target/F4BY/target.h | 5 +- src/main/target/FURYF3/target.h | 1 - src/main/target/FURYF4/target.h | 1 - src/main/target/IRCFUSIONF3/target.h | 5 +- src/main/target/LUX_RACE/target.h | 1 - src/main/target/MICROSCISKY/target.h | 1 - src/main/target/NAZE/target.h | 3 +- src/main/target/OMNIBUS/target.h | 1 - src/main/target/OMNIBUSF4/target.h | 3 +- src/main/target/REVO/target.h | 1 - src/main/target/REVONANO/target.h | 1 - src/main/target/RMDO/target.h | 1 - src/main/target/SPARKY2/target.h | 1 - src/main/target/SPRACINGF3/target.h | 3 +- src/main/target/VRRACE/target.h | 5 +- src/main/target/X_RACERSPI/target.h | 5 +- 29 files changed, 68 insertions(+), 131 deletions(-) diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index 962b637349..37b870bfd8 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -27,25 +27,6 @@ #include "system.h" -#ifndef EXTI_CALLBACK_HANDLER_COUNT -#define EXTI_CALLBACK_HANDLER_COUNT 1 -#endif - -extiCallbackHandlerConfig_t extiHandlerConfigs[EXTI_CALLBACK_HANDLER_COUNT]; - -void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn) -{ - for (int index = 0; index < EXTI_CALLBACK_HANDLER_COUNT; index++) { - extiCallbackHandlerConfig_t *candidate = &extiHandlerConfigs[index]; - if (!candidate->fn) { - candidate->fn = fn; - candidate->irqn = irqn; - return; - } - } - failureMode(FAILURE_DEVELOPER); // EXTI_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required. -} - // cycles per microsecond static uint32_t usTicks = 0; // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care. diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 105a584c44..084bab2256 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -48,22 +48,9 @@ void enableGPIOPowerUsageAndNoiseReductions(void); // current crystal frequency - 8 or 12MHz extern uint32_t hse_value; +extern uint32_t cachedRccCsrValue; typedef void extiCallbackHandlerFunc(void); -typedef struct extiCallbackHandlerConfig_s { - IRQn_Type irqn; - extiCallbackHandlerFunc* fn; -} extiCallbackHandlerConfig_t; - -#ifndef EXTI_CALLBACK_HANDLER_COUNT -#define EXTI_CALLBACK_HANDLER_COUNT 1 -#endif - -extern extiCallbackHandlerConfig_t extiHandlerConfigs[EXTI_CALLBACK_HANDLER_COUNT]; - -void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn); -void unregisterExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn); - -extern uint32_t cachedRccCsrValue; +void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn);void unregisterExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn); diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index d77f9fec55..d7f0d136e5 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -17,7 +17,6 @@ #include #include -#include #include "platform.h" @@ -107,7 +106,6 @@ void systemInit(void) // Init cycle counter cycleCounterInit(); - memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); // SysTick SysTick_Config(SystemCoreClock / 1000); } diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index 507e0811c5..b62d8bb3c7 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -17,7 +17,6 @@ #include #include -#include #include "platform.h" @@ -100,7 +99,6 @@ void systemInit(void) // Init cycle counter cycleCounterInit(); - memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); // SysTick SysTick_Config(SystemCoreClock / 1000); } diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 89d80c4cd3..e9b2c7e241 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -17,7 +17,6 @@ #include #include -#include #include "platform.h" @@ -183,7 +182,6 @@ void systemInit(void) // Init cycle counter cycleCounterInit(); - memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs)); // SysTick SysTick_Config(SystemCoreClock / 1000); } diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h index a578be44ec..27c3ea7f6d 100755 --- a/src/main/target/AIRHEROF3/target.h +++ b/src/main/target/AIRHEROF3/target.h @@ -30,7 +30,6 @@ #define USE_EXTI #define MPU_INT_EXTI PC13 -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU INT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/ALIENFLIGHTF1/target.h b/src/main/target/ALIENFLIGHTF1/target.h index a28bbf6f96..69ac283e52 100644 --- a/src/main/target/ALIENFLIGHTF1/target.h +++ b/src/main/target/ALIENFLIGHTF1/target.h @@ -26,9 +26,9 @@ #define BEEPER PA12 #define USE_EXTI -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready -//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define MAG_INT_EXTI PC14 #define USE_MPU_DATA_READY_SIGNAL +//#define DEBUG_MPU_DATA_READY_INTERRUPT #define GYRO #define USE_GYRO_MPU6050 diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 94317e7768..590e0b7ee5 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -35,8 +35,6 @@ #define BEEPER PA5 -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready - #define USE_EXTI //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index ef01181237..a8b0638f19 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -35,7 +35,6 @@ // MPU interrupt #define USE_EXTI #define MPU_INT_EXTI PC14 -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index b9d24c95d1..4ab4f96334 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -27,7 +27,6 @@ #define HW_PIN PB2 #define BOARD_HAS_VOLTAGE_DIVIDER -#define USE_EXTI #define LED0 PB6 #define LED1 PB5 @@ -41,10 +40,11 @@ #define INVERTER_USART USART6 // MPU6500 interrupt -//#define DEBUG_MPU_DATA_READY_INTERRUPT +#define USE_EXTI +#define MPU_INT_EXTI PC5 #define USE_MPU_DATA_READY_SIGNAL -//#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready -#define MPU_INT_EXTI PC5 +//#define DEBUG_MPU_DATA_READY_INTERRUPT + #define MPU6500_CS_PIN PC4 #define MPU6500_SPI_INSTANCE SPI1 diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index 5fbed88248..4aa6b2046c 100644 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -17,9 +17,6 @@ #pragma once -#define PLL_M 16 -#define PLL_N 336 - #define TARGET_BOARD_IDENTIFIER "COLI" #define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8) @@ -29,96 +26,99 @@ #define USBD_SERIALNUMBER_STRING "0x8020000" #endif -#define LED0 PC14 -#define LED1 PC13 -#define BEEPER PC5 -#define INVERTER PB2 // PB2 used as inverter select GPIO -#define INVERTER_USART USART2 +#define PLL_M 16 +#define PLL_N 336 -#define MPU6000_CS_PIN PC4 -#define MPU6000_SPI_INSTANCE SPI1 +#define LED0 PC14 +#define LED1 PC13 + +#define BEEPER PC5 + +#define INVERTER PB2 // PB2 used as inverter select GPIO +#define INVERTER_USART USART2 + +#define MPU6000_CS_PIN PC4 +#define MPU6000_SPI_INSTANCE SPI1 #define ACC #define USE_ACC_SPI_MPU6000 -#define ACC_MPU6000_ALIGN CW270_DEG_FLIP +#define ACC_MPU6000_ALIGN CW270_DEG_FLIP #define GYRO #define USE_GYRO_SPI_MPU6000 -#define GYRO_MPU6000_ALIGN CW270_DEG_FLIP +#define GYRO_MPU6000_ALIGN CW270_DEG_FLIP // MPU6000 interrupts #define USE_EXTI -#define MPU_INT_EXTI PC0 +#define MPU_INT_EXTI PC0 #define USE_MPU_DATA_READY_SIGNAL -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) #define MAG #define USE_MAG_HMC5883 -#define MAG_HMC5883_ALIGN CW270_DEG_FLIP +#define MAG_HMC5883_ALIGN CW270_DEG_FLIP +#define MAG_INT_EXTI PC1 #define USE_MAG_DATA_READY_SIGNAL #define ENSURE_MAG_DATA_READY_IS_HIGH -#define MAG_INT_EXTI PC1 #define BARO #define USE_BARO_MS5611 -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_INSTANCE SPI2 +#define M25P16_CS_PIN PB12 +#define M25P16_SPI_INSTANCE SPI2 #define USE_FLASHFS #define USE_FLASH_M25P16 #define USE_VCP -#define VBUS_SENSING_PIN PA9 +#define VBUS_SENSING_PIN PA9 #define USE_UART1 -#define UART1_RX_PIN PB7 -#define UART1_TX_PIN PB6 -#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN PB6 +#define UART1_AHB1_PERIPHERALS RCC_AHB1Periph_DMA2 #define USE_UART2 -#define UART2_RX_PIN PA3 -#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 #define USE_UART3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 //#define USE_UART4 #define UART4_RX_PIN PC11 #define UART4_TX_PIN PC10 //#define USE_UART5 -#define UART5_RX_PIN PD2 -#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 -#define SERIAL_PORT_COUNT 4 //VCP, UART1, UART2, UART3 +#define SERIAL_PORT_COUNT 4 //VCP, UART1, UART2, UART3 #define USE_SPI #define USE_SPI_DEVICE_1 -#define SPI1_NSS_PIN PC4 -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 +#define SPI1_NSS_PIN PC4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 #define USE_SPI_DEVICE_2 -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PC2 -#define SPI2_MOSI_PIN PC3 +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 #define USE_I2C -#define I2C_DEVICE (I2CDEV_3) -#define I2C3_SCL PA8 -#define I2C3_SDA PC9 +#define I2C_DEVICE (I2CDEV_3) +#define I2C3_SCL PA8 +#define I2C3_SDA PC9 -#define SENSORS_SET (SENSOR_ACC) +#define SENSORS_SET (SENSOR_ACC) #define LED_STRIP - #define WS2811_PIN PB7 // Shared UART1 #define WS2811_TIMER TIM4 #define WS2811_TIMER_CHANNEL TIM_Channel_2 @@ -134,15 +134,15 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES FEATURE_BLACKBOX +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define DEFAULT_FEATURES FEATURE_BLACKBOX #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff #define USABLE_TIMER_CHANNEL_COUNT 16 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(10) | TIM_N(11)) +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(10) | TIM_N(11)) diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index a68d7a71d9..5e8ac5d79e 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -34,7 +34,6 @@ // MPU6500 interrupt #define USE_EXTI #define MPU_INT_EXTI PA5 -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW //#define DEBUG_MPU_DATA_READY_INTERRUPT diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 3f40c4ab43..a5084a93e5 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -110,13 +110,11 @@ // mpu_int definition in sensors/initialisation.c #define USE_EXTI -#define MPU_INT_EXTI PC13 +#define MPU_INT_EXTI PC13 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready - #define LED_STRIP // tqfp48 pin 16 diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index d8f8154031..c366b5f23d 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -34,10 +34,9 @@ // MPU6000 interrupts -#define USE_MPU_DATA_READY_SIGNAL -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PB0 #define USE_EXTI +#define MPU_INT_EXTI PB0 +#define USE_MPU_DATA_READY_SIGNAL #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 1cc93b6f21..4ae2ceacaa 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -29,7 +29,6 @@ #define USE_EXTI #define MPU_INT_EXTI PA3 -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h index 2ccc49f739..74de219de1 100644 --- a/src/main/target/FURYF4/target.h +++ b/src/main/target/FURYF4/target.h @@ -34,7 +34,6 @@ // MPU6000 interrupts #define USE_EXTI #define MPU_INT_EXTI PC4 -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready (mag disabled) #define USE_MPU_DATA_READY_SIGNAL #define MPU6000_CS_PIN PA4 diff --git a/src/main/target/IRCFUSIONF3/target.h b/src/main/target/IRCFUSIONF3/target.h index b629d48df1..f3f2ee8119 100644 --- a/src/main/target/IRCFUSIONF3/target.h +++ b/src/main/target/IRCFUSIONF3/target.h @@ -24,10 +24,7 @@ #define LED0 PB3 #define USE_EXTI -#define MPU_INT_EXTI PC13 - -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready, no MAG - +#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index b0a2bf7520..c92c3e97e3 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -32,7 +32,6 @@ // MPU6500 interrupt #define USE_EXTI #define MPU_INT_EXTI PA5 -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/MICROSCISKY/target.h b/src/main/target/MICROSCISKY/target.h index 1d9c3fa70e..740fb2fd32 100644 --- a/src/main/target/MICROSCISKY/target.h +++ b/src/main/target/MICROSCISKY/target.h @@ -32,7 +32,6 @@ #define USE_EXTI #define MAG_INT_EXTI PC14 -#define EXTI_CALLBACK_HANDLER_COUNT 3 // MPU data ready, MAG data ready, BMP085 EOC //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL //#define DEBUG_MAG_DATA_READY_INTERRUPT diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index b69f8e9129..4caeaf33f4 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -37,8 +37,7 @@ #define INVERTER_USART USART2 #define USE_EXTI -#define MAG_INT_EXTI PC14 -#define EXTI_CALLBACK_HANDLER_COUNT 3 // MPU data ready, MAG data ready, BMP085 EOC +#define MAG_INT_EXTI PC14 //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL //#define DEBUG_MAG_DATA_READY_INTERRUPT diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 5434391b3e..2550b27fb4 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -29,7 +29,6 @@ #define USE_EXTI #define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) #define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define MPU6000_SPI_INSTANCE SPI1 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 4bd45103a3..376a683c77 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -49,11 +49,10 @@ #define USE_EXTI #define MPU_INT_EXTI PC4 #define USE_MPU_DATA_READY_SIGNAL -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) #define MAG #define USE_MAG_HMC5883 -#define MAG_HMC5883_ALIGN CW90_DEG +#define MAG_HMC5883_ALIGN CW90_DEG //#define USE_MAG_NAZA //#define MAG_NAZA_ALIGN CW180_DEG_FLIP diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index b697c887f3..57ad576597 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -47,7 +47,6 @@ #define USE_EXTI #define MPU_INT_EXTI PC4 #define USE_MPU_DATA_READY_SIGNAL -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) #define MAG #define USE_MAG_HMC5883 diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 3a4f8d157a..fa38860682 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -57,7 +57,6 @@ #define MPU_INT_EXTI PA15 #define USE_MPU_DATA_READY_SIGNAL //#define ENSURE_MPU_DATA_READY_IS_LOW -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready (mag disabled) #define USE_VCP #define VBUS_SENSING_PIN PA9 diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 57155d209b..1b94d82f24 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -28,7 +28,6 @@ #define USE_EXTI #define MPU_INT_EXTI PC13 -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW //#define USE_MAG_DATA_READY_SIGNAL // XXX Do RMDO has onboard mag??? diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 1b9a66974a..9ef673ff5c 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -39,7 +39,6 @@ // MPU9250 interrupt #define USE_EXTI #define MPU_INT_EXTI PC5 -#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready //#define DEBUG_MPU_DATA_READY_INTERRUPT #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 4b2ac2e943..543a91d20b 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -27,8 +27,7 @@ #define BEEPER_INVERTED #define USE_EXTI -#define MPU_INT_EXTI PC13 -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready +#define MPU_INT_EXTI PC13 #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW diff --git a/src/main/target/VRRACE/target.h b/src/main/target/VRRACE/target.h index cda9b14463..def63803d7 100644 --- a/src/main/target/VRRACE/target.h +++ b/src/main/target/VRRACE/target.h @@ -42,10 +42,9 @@ #define GYRO_MPU6500_ALIGN CW270_DEG // MPU6500 interrupts -#define USE_MPU_DATA_READY_SIGNAL -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PD10 #define USE_EXTI +#define MPU_INT_EXTI PD10 +#define USE_MPU_DATA_READY_SIGNAL /* #define BARO diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 9cc822ebee..d8e8f1b179 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -42,10 +42,9 @@ #define ACC_MPU6000_ALIGN CW270_DEG // MPU6000 interrupts -#define USE_MPU_DATA_READY_SIGNAL -#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready (mag disabled) -#define MPU_INT_EXTI PC13 #define USE_EXTI +#define MPU_INT_EXTI PC13 +#define USE_MPU_DATA_READY_SIGNAL #define USE_FLASHFS From 18bf3e924ef86bb5bf90ae1d3c3ee61b5791edf7 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Wed, 14 Sep 2016 10:39:08 +1200 Subject: [PATCH 450/456] Implemented support for MSP jumbo frames, switched dataflash reading to be using jumbo frames. Set default MSP serial speed to 500000 kbps. --- src/main/config/config.c | 2 +- src/main/io/serial.c | 2 +- src/main/io/serial.h | 2 ++ src/main/io/serial_msp.c | 59 +++++++++++++++++++++++++++++++--------- 4 files changed, 50 insertions(+), 15 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index fac719dd48..32f5299300 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -308,7 +308,7 @@ void resetSerialConfig(serialConfig_t *serialConfig) for (index = 0; index < SERIAL_PORT_COUNT; index++) { serialConfig->portConfigs[index].identifier = serialPortIdentifiers[index]; - serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_115200; + serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_500000; serialConfig->portConfigs[index].gps_baudrateIndex = BAUD_57600; serialConfig->portConfigs[index].telemetry_baudrateIndex = BAUD_AUTO; serialConfig->portConfigs[index].blackbox_baudrateIndex = BAUD_115200; diff --git a/src/main/io/serial.c b/src/main/io/serial.c index e5ef5da9f2..8b5fcfc855 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -85,7 +85,7 @@ const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = { static uint8_t serialPortCount; -const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000}; // see baudRate_e +const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000, 500000, 1000000}; // see baudRate_e #define BAUD_RATE_COUNT (sizeof(baudRates) / sizeof(baudRates[0])) diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 924f582323..13845446a1 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -46,6 +46,8 @@ typedef enum { BAUD_115200, BAUD_230400, BAUD_250000, + BAUD_500000, + BAUD_1000000, } baudRate_e; extern const uint32_t baudRates[]; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 088eafbc8c..84cec6466e 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -195,6 +195,10 @@ STATIC_UNIT_TESTED bufWriter_t *writer; #define RATEPROFILE_MASK (1 << 7) +#define JUMBO_FRAME_SIZE_LIMIT 255 + +#define DATAFLASH_BUFFER_SIZE 4096 + static void serialize8(uint8_t a) { bufWriterAppend(writer, a); @@ -232,7 +236,7 @@ static uint32_t read32(void) return t; } -static void headSerialResponse(uint8_t err, uint8_t responseBodySize) +static void headSerialResponse(uint8_t err, uint16_t responseBodySize) { serialBeginWrite(mspSerialPort); @@ -240,11 +244,18 @@ static void headSerialResponse(uint8_t err, uint8_t responseBodySize) serialize8('M'); serialize8(err ? '!' : '>'); currentPort->checksum = 0; // start calculating a new checksum - serialize8(responseBodySize); + if (responseBodySize < JUMBO_FRAME_SIZE_LIMIT) { + serialize8(responseBodySize); + } else { + serialize8(JUMBO_FRAME_SIZE_LIMIT); + } serialize8(currentPort->cmdMSP); + if (responseBodySize >= JUMBO_FRAME_SIZE_LIMIT) { + serialize16(responseBodySize); + } } -static void headSerialReply(uint8_t responseBodySize) +static void headSerialReply(uint16_t responseBodySize) { headSerialResponse(0, responseBodySize); } @@ -400,25 +411,38 @@ static void serializeDataflashSummaryReply(void) } #ifdef USE_FLASHFS -static void serializeDataflashReadReply(uint32_t address, uint8_t size) +static void serializeDataflashReadReply(uint32_t address, uint16_t size, bool useLegacyFormat) { - uint8_t buffer[128]; - int bytesRead; + static uint8_t buffer[DATAFLASH_BUFFER_SIZE]; if (size > sizeof(buffer)) { size = sizeof(buffer); } - headSerialReply(4 + size); - - serialize32(address); - // bytesRead will be lower than that requested if we reach end of volume - bytesRead = flashfsReadAbs(address, buffer, size); + int bytesRead = flashfsReadAbs(address, buffer, size); - for (int i = 0; i < bytesRead; i++) { + if (useLegacyFormat) { + headSerialReply(sizeof(uint32_t) + size); + + serialize32(address); + } else { + headSerialReply(sizeof(uint32_t) + sizeof(uint16_t) + bytesRead); + + serialize32(address); + serialize16(bytesRead); + } + + int i; + for (i = 0; i < bytesRead; i++) { serialize8(buffer[i]); } + + if (useLegacyFormat) { + for (; i < size; i++) { + serialize8(0); + } + } } #endif @@ -1162,8 +1186,17 @@ static bool processOutCommand(uint8_t cmdMSP) case MSP_DATAFLASH_READ: { uint32_t readAddress = read32(); + uint16_t readLength; + bool useLegacyFormat; + if (currentPort->dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) { + readLength = read16(); + useLegacyFormat = false; + } else { + readLength = 128; + useLegacyFormat = true; + } - serializeDataflashReadReply(readAddress, 128); + serializeDataflashReadReply(readAddress, readLength, useLegacyFormat); } break; #endif From 351a921bfb842516c73eee71cd4007719e176cb2 Mon Sep 17 00:00:00 2001 From: nathan Date: Tue, 13 Sep 2016 20:14:55 -0700 Subject: [PATCH 451/456] use the toolchain on the path before asking to download a new one --- .gitignore | 2 ++ Makefile | 10 ++++++---- make/local.mk | 2 ++ make/tools.mk | 13 ++++++++----- 4 files changed, 18 insertions(+), 9 deletions(-) create mode 100644 make/local.mk diff --git a/.gitignore b/.gitignore index 054f2748e4..b984b1e5dc 100644 --- a/.gitignore +++ b/.gitignore @@ -22,3 +22,5 @@ README.pdf /downloads /tools /build +# local changes only +make/local.mk diff --git a/Makefile b/Makefile index add849a2ba..9f3ee3456a 100644 --- a/Makefile +++ b/Makefile @@ -70,6 +70,8 @@ LINKER_DIR = $(ROOT)/src/main/target ## Build tools, so we all share the same versions # import macros common to all supported build systems include $(ROOT)/make/system-id.mk +# developer preferences, edit these at will, they'll be gitignored +include $(ROOT)/make/local.mk # configure some directories that are relative to wherever ROOT_DIR is located TOOLS_DIR := $(ROOT)/tools @@ -591,10 +593,10 @@ CCACHE := endif # Tool names -CC := $(CCACHE) $(ARM_SDK_DIR)/bin/arm-none-eabi-gcc -CPP := $(CCACHE) $(ARM_SDK_DIR)/bin/arm-none-eabi-g++ -OBJCOPY := $(ARM_SDK_DIR)/bin/arm-none-eabi-objcopy -SIZE := $(ARM_SDK_DIR)/bin/arm-none-eabi-size +CC := $(CCACHE) $(ARM_SDK_PREFIX)gcc +CPP := $(CCACHE) $(ARM_SDK_PREFIX)g++ +OBJCOPY := $(ARM_SDK_PREFIX)objcopy +SIZE := $(ARM_SDK_PREFIX)size # # Tool options. diff --git a/make/local.mk b/make/local.mk new file mode 100644 index 0000000000..1b66457a50 --- /dev/null +++ b/make/local.mk @@ -0,0 +1,2 @@ +# override the toolchain version, should match the output from of your version of the toolchain: $(arm-none-eabi-gcc -dumpversion) +#GCC_REQUIRED_VERSION=5.4.1 diff --git a/make/tools.mk b/make/tools.mk index 77aa88a6cb..299086b1bb 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -15,6 +15,8 @@ # Set up ARM (STM32) SDK ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-5_4-2016q2 +# Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion) +GCC_REQUIRED_VERSION := 5.4.1 .PHONY: arm_sdk_install @@ -271,13 +273,14 @@ zip_clean: # ############################## +GCC_VERSION=$(shell arm-none-eabi-gcc -dumpversion) ifeq ($(shell [ -d "$(ARM_SDK_DIR)" ] && echo "exists"), exists) ARM_SDK_PREFIX := $(ARM_SDK_DIR)/bin/arm-none-eabi- -else - ifndef IGNORE_MISSING_TOOLCHAIN - ifeq (,$(findstring _install,$(MAKECMDGOALS))) - $(error **WARNING** ARM-SDK not in $(ARM_SDK_DIR) Please run 'make arm_sdk_install') - endif +else ifeq (,$(findstring _install,$(MAKECMDGOALS))) + ifeq ($(GCC_VERSION),) + $(error **ERROR** arm-none-eabi-gcc not in the PATH. Run 'make arm_sdk_install' to install automatically in the tools folder of this repo) + else ifneq ($(GCC_VERSION), $(GCC_REQUIRED_VERSION)) + $(error **WARNING** your arm-none-eabi-gcc is '$(GCC_VERSION)', but '$(GCC_REQUIRED_VERSION)' is expected. Run 'make arm_sdk_install' to install automatically in the tools folder of this repo) endif # not installed, hope it's in the path... ARM_SDK_PREFIX ?= arm-none-eabi- From 764c25a2d1fea53865fa6d85553e7303d8b423b4 Mon Sep 17 00:00:00 2001 From: nathan Date: Tue, 13 Sep 2016 20:15:08 -0700 Subject: [PATCH 452/456] use curl instead of wget (which isnt included on mac by default) --- make/tools.mk | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/make/tools.mk b/make/tools.mk index 299086b1bb..71438f49df 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -39,7 +39,7 @@ arm_sdk_install: | $(DL_DIR) $(TOOLS_DIR) arm_sdk_install: arm_sdk_clean ifneq ($(OSFAMILY), windows) # download the source only if it's newer than what we already have - $(V1) wget --no-check-certificate -N -P "$(DL_DIR)" "$(ARM_SDK_URL)" + $(V1) curl -L -k -o "$(DL_DIR)/$(ARM_SDK_FILE)" "$(ARM_SDK_URL)" # binary only release so just extract it $(V1) tar -C $(TOOLS_DIR) -xjf "$(DL_DIR)/$(ARM_SDK_FILE)" @@ -185,7 +185,7 @@ dfuutil_install: | $(DL_DIR) $(TOOLS_DIR) dfuutil_install: dfuutil_clean # download the source $(V0) @echo " DOWNLOAD $(DFUUTIL_URL)" - $(V1) wget -N -P "$(DL_DIR)" "$(DFUUTIL_URL)" + $(V1) curl -L -k -o "$(DL_DIR)/$(DFUUTIL_FILE)" "$(DFUUTIL_URL)" # extract the source $(V0) @echo " EXTRACT $(DFUUTIL_FILE)" @@ -220,8 +220,6 @@ uncrustify_install: UNCRUSTIFY_OPTIONS := prefix=$(UNCRUSTIFY_DIR) uncrustify_install: uncrustify_clean ifneq ($(OSFAMILY), windows) $(V0) @echo " DOWNLOAD $(UNCRUSTIFY_URL)" - $(V1) wget --no-check-certificate -N -P "$(DL_DIR)" "$(UNCRUSTIFY_URL)" -else $(V1) curl -L -k -o "$(DL_DIR)/$(UNCRUSTIFY_FILE)" "$(UNCRUSTIFY_URL)" endif # extract the src From 56097952ef6bfd836a0a37b818ffa39c4546df15 Mon Sep 17 00:00:00 2001 From: nathan Date: Tue, 13 Sep 2016 20:30:05 -0700 Subject: [PATCH 453/456] better error message for gcc version mismatch error (toolchain) --- make/tools.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/make/tools.mk b/make/tools.mk index 71438f49df..a12dc50a3c 100644 --- a/make/tools.mk +++ b/make/tools.mk @@ -278,7 +278,7 @@ else ifeq (,$(findstring _install,$(MAKECMDGOALS))) ifeq ($(GCC_VERSION),) $(error **ERROR** arm-none-eabi-gcc not in the PATH. Run 'make arm_sdk_install' to install automatically in the tools folder of this repo) else ifneq ($(GCC_VERSION), $(GCC_REQUIRED_VERSION)) - $(error **WARNING** your arm-none-eabi-gcc is '$(GCC_VERSION)', but '$(GCC_REQUIRED_VERSION)' is expected. Run 'make arm_sdk_install' to install automatically in the tools folder of this repo) + $(error **ERROR** your arm-none-eabi-gcc is '$(GCC_VERSION)', but '$(GCC_REQUIRED_VERSION)' is expected. Override with 'GCC_REQUIRED_VERSION' in make/local.mk or run 'make arm_sdk_install' to install the right version automatically in the tools folder of this repo) endif # not installed, hope it's in the path... ARM_SDK_PREFIX ?= arm-none-eabi- From efc14e60684f67a335020ee6dc353e082484d263 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Thu, 15 Sep 2016 08:01:14 +1200 Subject: [PATCH 454/456] Updated .gitignore --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 245d33be92..fd8ff825fc 100644 --- a/.gitignore +++ b/.gitignore @@ -17,4 +17,5 @@ startup_stm32f10x_md_gcc.s # script-generated files docs/Manual.pdf README.pdf - +tools +downloads From 62b0500f017ba99aadfaf66a55c63aa430f54ada Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 18 Sep 2016 01:52:59 +0200 Subject: [PATCH 455/456] Fix OMNIBUSF4 Timers --- src/main/target/OMNIBUSF4/target.c | 34 +++++++++++------------------- 1 file changed, 12 insertions(+), 22 deletions(-) diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 2ed3eab8fb..96daa89f8f 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -30,11 +30,6 @@ const uint16_t multiPPM[] = { PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; @@ -62,11 +57,6 @@ const uint16_t airPPM[] = { PWM10 | (MAP_TO_SERVO_OUTPUT << 8), PWM11 | (MAP_TO_SERVO_OUTPUT << 8), PWM12 | (MAP_TO_SERVO_OUTPUT << 8), - PWM2 | (MAP_TO_SERVO_OUTPUT << 8), - PWM3 | (MAP_TO_SERVO_OUTPUT << 8), - PWM4 | (MAP_TO_SERVO_OUTPUT << 8), - PWM5 | (MAP_TO_SERVO_OUTPUT << 8), - PWM1 | (MAP_TO_SERVO_OUTPUT << 8), 0xFFFF }; @@ -88,17 +78,17 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12 }, // PPM (5th pin on FlexiIO port) - { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM12 }, // S2_IN - GPIO_PartialRemap_TIM3 - { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S3_IN - { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S4_IN - { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S5_IN - { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, IOCFG_IPD, GPIO_Mode_AF, GPIO_AF_TIM8 }, // S6_IN + { TIM12, IO_TAG(PB14), TIM_Channel_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM12 },// PPM (5th pin on FlexiIO port) + { TIM12, IO_TAG(PB15), TIM_Channel_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM12 },// S2_IN - GPIO_PartialRemap_TIM3 + { TIM8, IO_TAG(PC6), TIM_Channel_1, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S3_IN + { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S4_IN + { TIM8, IO_TAG(PC8), TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S5_IN + { TIM8, IO_TAG(PC9), TIM_Channel_4, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF_TIM8 }, // S6_IN - { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S1_OUT - { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM3 }, // S2_OUT - { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM9 }, // S3_OUT - { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM2 }, // S4_OUT - { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 - { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, IOCFG_AF_PP_PD, GPIO_Mode_AF, GPIO_AF_TIM5 }, // S6_OUT + { TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S1_OUT + { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM3 }, // S2_OUT + { TIM9, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM9 }, // S4_OUT + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM2 }, // S4_OUT + { TIM5, IO_TAG(PA1), TIM_Channel_2, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5 }, // S5_OUT - GPIO_PartialRemap_TIM3 + { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF_TIM5 }, // S6_OUT }; From ff4d401918b307ca56f8eac0cc349c43c73bb682 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Sun, 18 Sep 2016 15:19:46 +0200 Subject: [PATCH 456/456] IMPULSERCF3 build failed. Timer.h missing. Fixed. --- src/main/target/IMPULSERCF3/target.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/IMPULSERCF3/target.c b/src/main/target/IMPULSERCF3/target.c index 6c98e152c7..d3b68be89c 100644 --- a/src/main/target/IMPULSERCF3/target.c +++ b/src/main/target/IMPULSERCF3/target.c @@ -19,6 +19,7 @@ #include #include "drivers/io.h" +#include "drivers/timer.h" #include "drivers/pwm_mapping.h"