From f16c6fc019837863c473ac7a20e36e85598d2b96 Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Fri, 30 Jan 2015 15:53:45 +1300 Subject: [PATCH 01/36] Blackbox: Log PID intermediates for pidLuxFloat Previously nothing was logged since the intermediate calculations were floats. Logging them converted to integers is better than nothing. --- src/main/flight/flight.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 26e18f618..2c4f090c9 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -201,8 +201,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000); - } +#ifdef BLACKBOX + axisPID_P[axis] = PTerm; + axisPID_I[axis] = ITerm; + axisPID_D[axis] = -DTerm; +#endif + } } static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, From 6a6a5b99086fe3dd8aee8d6d8459d41df928978b Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Fri, 30 Jan 2015 18:58:10 +1300 Subject: [PATCH 02/36] PID tuning docs: Fix Angle level reference for PID controller 1 --- docs/PID tuning.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index 4ae7fb154..a7788134b 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -85,7 +85,7 @@ nebbian in v1.6.0. The autotune feature does not work on this controller, so don It is the first PID Controller designed for 32-bit processors and not derived from MultiWii. The strength of the auto-leveling correction applied during Angle mode is set by the parameter "level_angle" which -is labeled "LEVEL Integral" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to +is labeled "LEVEL Proportional" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to Horizon mode. The default is 5.0. The strength of the auto-leveling correction applied during Horizon mode is set by the parameter "level_horizon" which From 80d304ed49917570603996d5ceab50d62a179783 Mon Sep 17 00:00:00 2001 From: Dave Pitman Date: Fri, 30 Jan 2015 09:22:38 -0800 Subject: [PATCH 03/36] Update PID tuning.md Add warning about the Level_P default value of 90 which will most likely result in loss of control in Angle Mode. --- docs/PID tuning.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/PID tuning.md b/docs/PID tuning.md index a7788134b..b206d751c 100644 --- a/docs/PID tuning.md +++ b/docs/PID tuning.md @@ -63,7 +63,7 @@ affecting yaw, roll or pitch rotation rates (which are tuned by the dedicated ro settings). In Angle mode, this controller uses the LEVEL "P" PID setting to decide how strong the auto-level correction should -be. +be. Note that the default value for P_Level is 90. This is more than likely too high of a value for most, and will cause the model to be very unstable in Angle Mode, and could result in loss of control. It is recommended to change this value to 20 before using PID Controller 2 in Angle Mode. In Horizon mode, this controller uses the LEVEL "I" PID setting to decide how much auto-level correction should be applied. The default Cleanflight setting for "I" will result in virtually no auto-leveling being applied, so that will From f77a762b48c10f69b252791f062ee77c17f047b8 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 20:54:34 +0100 Subject: [PATCH 04/36] Allow inflight adjustments for floating-point based PID controllers. --- src/main/config/config.c | 7 +- src/main/config/config_profile.h | 2 - src/main/flight/autotune.c | 4 +- src/main/flight/autotune.h | 2 +- src/main/flight/flight.h | 6 +- src/main/io/rc_controls.c | 75 ++++++--- src/main/io/rc_controls.h | 3 + src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 10 +- src/main/mw.c | 2 +- src/test/unit/platform.h | 1 + src/test/unit/rc_controls_unittest.cc | 215 ++++++++++++++++++++++++++ 12 files changed, 295 insertions(+), 34 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index d74f714af..0d8c47eeb 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -110,7 +110,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 90; +static const uint8_t EEPROM_CONF_VERSION = 91; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -121,6 +121,8 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) static void resetPidProfile(pidProfile_t *pidProfile) { + pidProfile->pidController = 0; + pidProfile->P8[ROLL] = 40; pidProfile->I8[ROLL] = 30; pidProfile->D8[ROLL] = 23; @@ -386,7 +388,6 @@ static void resetConf(void) masterConfig.looptime = 3500; masterConfig.emf_avoidance = 0; - currentProfile->pidController = 0; resetPidProfile(¤tProfile->pidProfile); resetControlRateConfig(&masterConfig.controlRateProfiles[0]); @@ -603,7 +604,7 @@ void activateConfig(void) useTelemetryConfig(&masterConfig.telemetryConfig); #endif - setPIDController(currentProfile->pidController); + setPIDController(currentProfile->pidProfile.pidController); #ifdef GPS gpsUseProfile(¤tProfile->gpsProfile); diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 1aafc14bd..7968b97c0 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -18,8 +18,6 @@ #pragma once typedef struct profile_s { - uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid - pidProfile_t pidProfile; uint8_t defaultRateProfileIndex; diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index 2770b6824..4926848d8 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -415,7 +415,7 @@ void restorePids(pidProfile_t *pidProfileToTune) memcpy(pidProfileToTune, &pidBackup, sizeof(pidBackup)); } -void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse) +void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune) { phase = nextPhase; @@ -439,7 +439,7 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle firstPeakAngle = secondPeakAngle = 0; pidProfile = pidProfileToTune; - pidController = pidControllerInUse; + pidController = pidProfile->pidController; updatePidIndex(); updateTargetAngle(); diff --git a/src/main/flight/autotune.h b/src/main/flight/autotune.h index 095309aaf..8d4296e3c 100644 --- a/src/main/flight/autotune.h +++ b/src/main/flight/autotune.h @@ -18,7 +18,7 @@ #pragma once void autotuneReset(); -void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse); +void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune); float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle); void autotuneEndPhase(); diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index 51fd74a0a..cad033b4f 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -32,12 +32,16 @@ typedef enum { PID_ITEM_COUNT } pidIndex_e; +#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2) + typedef struct pidProfile_s { + uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid + uint8_t P8[PID_ITEM_COUNT]; uint8_t I8[PID_ITEM_COUNT]; uint8_t D8[PID_ITEM_COUNT]; - float P_f[3]; // float p i and d factors for the new baseflight pid + float P_f[3]; // float p i and d factors for lux float pid controller float I_f[3]; float D_f[3]; float A_level; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 64a70ba40..5a821a5cd 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -21,6 +21,8 @@ #include +#include "platform.h" + #include "common/axis.h" #include "common/maths.h" @@ -360,6 +362,7 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) { int newValue; + float newFloatValue; if (delta > 0) { queueConfirmationBeep(2); @@ -391,34 +394,70 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c break; case ADJUSTMENT_PITCH_ROLL_P: - newValue = (int)pidProfile->P8[PIDPITCH] + delta; - pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c - newValue = (int)pidProfile->P8[PIDROLL] + delta; - pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->P_f[PIDPITCH] + delta; + pidProfile->P_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = (int)pidProfile->P_f[PIDROLL] + delta; + pidProfile->P_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->P8[PIDPITCH] + delta; + pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)pidProfile->P8[PIDROLL] + delta; + pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_PITCH_ROLL_I: - newValue = (int)pidProfile->I8[PIDPITCH] + delta; - pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c - newValue = (int)pidProfile->I8[PIDROLL] + delta; - pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->I_f[PIDPITCH] + delta; + pidProfile->I_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = (int)pidProfile->I_f[PIDROLL] + delta; + pidProfile->I_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->I8[PIDPITCH] + delta; + pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)pidProfile->I8[PIDROLL] + delta; + pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_PITCH_ROLL_D: - newValue = (int)pidProfile->D8[PIDPITCH] + delta; - pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c - newValue = (int)pidProfile->D8[PIDROLL] + delta; - pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->D_f[PIDPITCH] + delta; + pidProfile->D_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = (int)pidProfile->D_f[PIDROLL] + delta; + pidProfile->D_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->D8[PIDPITCH] + delta; + pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)pidProfile->D8[PIDROLL] + delta; + pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_YAW_P: - newValue = (int)pidProfile->P8[PIDYAW] + delta; - pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->P_f[PIDYAW] + delta; + pidProfile->P_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->P8[PIDYAW] + delta; + pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_YAW_I: - newValue = (int)pidProfile->I8[PIDYAW] + delta; - pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->I_f[PIDYAW] + delta; + pidProfile->I_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->I8[PIDYAW] + delta; + pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; case ADJUSTMENT_YAW_D: - newValue = (int)pidProfile->D8[PIDYAW] + delta; - pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { + newFloatValue = (int)pidProfile->D_f[PIDYAW] + delta; + pidProfile->D_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } else { + newValue = (int)pidProfile->D8[PIDYAW] + delta; + pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + } break; default: break; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index a1c2a10a5..dde0536f0 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -207,7 +207,10 @@ typedef struct adjustmentState_s { uint32_t timeoutAt; } adjustmentState_t; + +#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT #define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel +#endif #define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches. diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2ec7e39f4..a8eaf45e6 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -370,7 +370,7 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_NONE }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 }, - { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 5 }, + { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidController, 0, 5 }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 5622da838..97822698a 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -865,7 +865,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); - if (currentProfile->pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid + if (currentProfile->pidProfile.pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid for (i = 0; i < 3; i++) { serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250)); @@ -896,7 +896,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_PID_CONTROLLER: headSerialReply(1); - serialize8(currentProfile->pidController); + serialize8(currentProfile->pidProfile.pidController); break; case MSP_MODE_RANGES: headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT); @@ -1205,11 +1205,11 @@ static bool processInCommand(void) currentProfile->accelerometerTrims.values.roll = read16(); break; case MSP_SET_PID_CONTROLLER: - currentProfile->pidController = read8(); - setPIDController(currentProfile->pidController); + currentProfile->pidProfile.pidController = read8(); + setPIDController(currentProfile->pidProfile.pidController); break; case MSP_SET_PID: - if (currentProfile->pidController == 2) { + if (currentProfile->pidProfile.pidController == 2) { for (i = 0; i < 3; i++) { currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f; currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f; diff --git a/src/main/mw.c b/src/main/mw.c index 0d542311c..028177515 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -124,7 +124,7 @@ void updateAutotuneState(void) autotuneReset(); landedAfterAutoTuning = false; } - autotuneBeginNextPhase(¤tProfile->pidProfile, currentProfile->pidController); + autotuneBeginNextPhase(¤tProfile->pidProfile); ENABLE_FLIGHT_MODE(AUTOTUNE_MODE); autoTuneWasUsed = true; } else { diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 39c809316..f74b9be29 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -25,3 +25,4 @@ #define SERIAL_PORT_COUNT 4 +#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6 diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 54a830278..91ec4f515 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -17,7 +17,9 @@ #include #include + extern "C" { + #include "platform.h" #include "common/axis.h" #include "flight/flight.h" @@ -29,6 +31,8 @@ extern "C" { #include "gtest/gtest.h" extern "C" { +extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile); + int constrain(int amt, int low, int high) { if (amt < low) @@ -479,6 +483,215 @@ TEST(RcControlsTest, processRcRateProfileAdjustments) EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); } +static const adjustmentConfig_t pidPitchAndRollPAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidPitchAndRollIAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidPitchAndRollDAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidYawPAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_YAW_P, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidYawIAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_YAW_I, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +static const adjustmentConfig_t pidYawDAdjustmentConfig = { + .adjustmentFunction = ADJUSTMENT_YAW_D, + .mode = ADJUSTMENT_MODE_STEP, + .data = { { 1 } } +}; + +TEST(RcControlsTest, processPIDIncreasePidController0) +{ + // given + modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; + memset(&modeActivationConditions, 0, sizeof (modeActivationConditions)); + + escAndServoConfig_t escAndServoConfig; + memset(&escAndServoConfig, 0, sizeof (escAndServoConfig)); + + pidProfile_t pidProfile; + memset(&pidProfile, 0, sizeof (pidProfile)); + pidProfile.pidController = 0; + pidProfile.P8[PIDPITCH] = 0; + pidProfile.P8[PIDROLL] = 5; + pidProfile.P8[YAW] = 7; + pidProfile.I8[PIDPITCH] = 10; + pidProfile.I8[PIDROLL] = 15; + pidProfile.I8[YAW] = 17; + pidProfile.D8[PIDPITCH] = 20; + pidProfile.D8[PIDROLL] = 25; + pidProfile.D8[YAW] = 27; + + // and + controlRateConfig_t controlRateConfig; + memset(&controlRateConfig, 0, sizeof (controlRateConfig)); + + // and + memset(&rxConfig, 0, sizeof (rxConfig)); + rxConfig.mincheck = DEFAULT_MIN_CHECK; + rxConfig.maxcheck = DEFAULT_MAX_CHECK; + rxConfig.midrc = 1500; + + adjustmentStateMask = 0; + memset(&adjustmentStates, 0, sizeof(adjustmentStates)); + + configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig); + configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig); + configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig); + configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig); + configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig); + configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig); + + // and + uint8_t index; + for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + rcData[index] = PWM_RANGE_MIDDLE; + } + + // and + resetCallCounters(); + resetMillis(); + + // and + rcData[AUX1] = PWM_RANGE_MAX; + rcData[AUX2] = PWM_RANGE_MAX; + rcData[AUX3] = PWM_RANGE_MAX; + + // and + uint8_t expectedAdjustmentStateMask = + (1 << 0) | + (1 << 1) | + (1 << 2) | + (1 << 3) | + (1 << 4) | + (1 << 5); + + // when + useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile); + processRcAdjustments(&controlRateConfig, &rxConfig); + + // then + EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6); + EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); + + // and + EXPECT_EQ(pidProfile.P8[PIDPITCH], 1); + EXPECT_EQ(pidProfile.P8[PIDROLL], 6); + EXPECT_EQ(pidProfile.P8[YAW], 8); + EXPECT_EQ(pidProfile.I8[PIDPITCH], 11); + EXPECT_EQ(pidProfile.I8[PIDROLL], 16); + EXPECT_EQ(pidProfile.I8[YAW], 18); + EXPECT_EQ(pidProfile.D8[PIDPITCH], 21); + EXPECT_EQ(pidProfile.D8[PIDROLL], 26); + EXPECT_EQ(pidProfile.D8[YAW], 28); +} + +TEST(RcControlsTest, processPIDIncreasePidController2) +{ + // given + modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT]; + memset(&modeActivationConditions, 0, sizeof (modeActivationConditions)); + + escAndServoConfig_t escAndServoConfig; + memset(&escAndServoConfig, 0, sizeof (escAndServoConfig)); + + pidProfile_t pidProfile; + memset(&pidProfile, 0, sizeof (pidProfile)); + pidProfile.pidController = 2; + pidProfile.P_f[PIDPITCH] = 0.0f; + pidProfile.P_f[PIDROLL] = 5.0f; + pidProfile.P_f[PIDYAW] = 7.0f; + pidProfile.I_f[PIDPITCH] = 10.0f; + pidProfile.I_f[PIDROLL] = 15.0f; + pidProfile.I_f[PIDYAW] = 17.0f; + pidProfile.D_f[PIDPITCH] = 20.0f; + pidProfile.D_f[PIDROLL] = 25.0f; + pidProfile.D_f[PIDYAW] = 27.0f; + + // and + controlRateConfig_t controlRateConfig; + memset(&controlRateConfig, 0, sizeof (controlRateConfig)); + + // and + memset(&rxConfig, 0, sizeof (rxConfig)); + rxConfig.mincheck = DEFAULT_MIN_CHECK; + rxConfig.maxcheck = DEFAULT_MAX_CHECK; + rxConfig.midrc = 1500; + + adjustmentStateMask = 0; + memset(&adjustmentStates, 0, sizeof(adjustmentStates)); + + configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig); + configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig); + configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig); + configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig); + configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig); + configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig); + + // and + uint8_t index; + for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) { + rcData[index] = PWM_RANGE_MIDDLE; + } + + // and + resetCallCounters(); + resetMillis(); + + // and + rcData[AUX1] = PWM_RANGE_MAX; + rcData[AUX2] = PWM_RANGE_MAX; + rcData[AUX3] = PWM_RANGE_MAX; + + // and + uint8_t expectedAdjustmentStateMask = + (1 << 0) | + (1 << 1) | + (1 << 2) | + (1 << 3) | + (1 << 4) | + (1 << 5); + + // when + useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile); + processRcAdjustments(&controlRateConfig, &rxConfig); + + // then + EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6); + EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); + + // and + EXPECT_EQ(pidProfile.P_f[PIDPITCH], 1.0f); + EXPECT_EQ(pidProfile.P_f[PIDROLL], 6.0f); + EXPECT_EQ(pidProfile.P_f[PIDYAW], 8.0f); + EXPECT_EQ(pidProfile.I_f[PIDPITCH], 11.0f); + EXPECT_EQ(pidProfile.I_f[PIDROLL], 16.0f); + EXPECT_EQ(pidProfile.I_f[PIDYAW], 18.0f); + EXPECT_EQ(pidProfile.D_f[PIDPITCH], 21.0f); + EXPECT_EQ(pidProfile.D_f[PIDROLL], 26.0f); + EXPECT_EQ(pidProfile.D_f[PIDYAW], 28.0f); + +} + extern "C" { void saveConfigAndNotify(void) {} void generateThrottleCurve(controlRateConfig_t *, escAndServoConfig_t *) {} @@ -494,6 +707,8 @@ void mwDisarm(void) {} uint8_t getCurrentControlRateProfile(void) { return 0; } +void GPS_reset_home_position(void) {} +void baroSetCalibrationCycles(uint16_t) {} uint8_t armingFlags = 0; int16_t heading; From fa1894008745f47787a6dccacfd3d952ca9007f2 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 21:25:18 +0100 Subject: [PATCH 05/36] Minor cleanups to allow CJMCU to build again. --- Makefile | 2 -- src/main/config/config_master.h | 2 ++ src/main/io/serial_cli.c | 6 ++++-- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index a62cc1ab8..aadb9ab50 100644 --- a/Makefile +++ b/Makefile @@ -395,8 +395,6 @@ CJMCU_SRC = \ drivers/system_stm32f10x.c \ drivers/timer.c \ drivers/timer_stm32f10x.c \ - blackbox/blackbox.c \ - blackbox/blackbox_io.c \ hardware_revision.c \ $(COMMON_SRC) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index bacb7b5a7..b99770729 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -85,8 +85,10 @@ typedef struct master_t { uint8_t current_profile_index; controlRateConfig_t controlRateProfiles[MAX_CONTROL_RATE_PROFILE_COUNT]; +#ifdef BLACKBOX uint8_t blackbox_rate_num; uint8_t blackbox_rate_denom; +#endif uint8_t magic_ef; // magic number, should be 0xEF uint8_t chk; // XOR checksum diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a8eaf45e6..0735c87bd 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -163,7 +163,7 @@ const clicmd_t cmdTable[] = { { "color", "configure colors", cliColor }, #endif { "defaults", "reset to defaults and reboot", cliDefaults }, - { "dump", "print configurable settings in a pastable form", cliDump }, + { "dump", "dump configuration", cliDump }, { "exit", "", cliExit }, { "feature", "list or -val or val", cliFeature }, { "get", "get variable value", cliGet }, @@ -249,7 +249,7 @@ const clivalue_t valueTable[] = { { "serial_port_2_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[1], 0, SERIAL_PORT_SCENARIO_MAX }, #if (SERIAL_PORT_COUNT > 2) { "serial_port_3_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[2], 0, SERIAL_PORT_SCENARIO_MAX }, -#if !defined(CC3D) +#if (SERIAL_PORT_COUNT > 3) { "serial_port_4_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[3], 0, SERIAL_PORT_SCENARIO_MAX }, #if (SERIAL_PORT_COUNT > 4) { "serial_port_5_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[4], 0, SERIAL_PORT_SCENARIO_MAX }, @@ -408,8 +408,10 @@ const clivalue_t valueTable[] = { { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 }, +#ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 }, +#endif }; #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) From ea386e6da2f15e1a7839901fa51add710c39c10d Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 21:27:09 +0100 Subject: [PATCH 06/36] Remove magic number usage. Fix limits for FP based pid controller PID adjustments to match those in serial_cli.c. --- src/main/flight/autotune.c | 4 ++-- src/main/io/rc_controls.c | 18 +++++++++--------- src/main/io/serial_msp.c | 4 ++-- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index 4926848d8..33974931b 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -226,8 +226,8 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin return errorAngle; } - if (pidController == 2) { - // TODO support new baseflight pid controller + if (IS_PID_CONTROLLER_FP_BASED(pidController) { + // TODO support floating point based pid controllers return errorAngle; } diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 5a821a5cd..203ed2b92 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -396,9 +396,9 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_PITCH_ROLL_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = (int)pidProfile->P_f[PIDPITCH] + delta; - pidProfile->P_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c newFloatValue = (int)pidProfile->P_f[PIDROLL] + delta; - pidProfile->P_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->P8[PIDPITCH] + delta; pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -409,9 +409,9 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_PITCH_ROLL_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = (int)pidProfile->I_f[PIDPITCH] + delta; - pidProfile->I_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c newFloatValue = (int)pidProfile->I_f[PIDROLL] + delta; - pidProfile->I_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDPITCH] + delta; pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -422,9 +422,9 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_PITCH_ROLL_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = (int)pidProfile->D_f[PIDPITCH] + delta; - pidProfile->D_f[PIDPITCH] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c newFloatValue = (int)pidProfile->D_f[PIDROLL] + delta; - pidProfile->D_f[PIDROLL] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDPITCH] + delta; pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -435,7 +435,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_YAW_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = (int)pidProfile->P_f[PIDYAW] + delta; - pidProfile->P_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->P_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->P8[PIDYAW] + delta; pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -444,7 +444,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_YAW_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = (int)pidProfile->I_f[PIDYAW] + delta; - pidProfile->I_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->I_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDYAW] + delta; pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -453,7 +453,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm case ADJUSTMENT_YAW_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { newFloatValue = (int)pidProfile->D_f[PIDYAW] + delta; - pidProfile->D_f[PIDYAW] = constrain(newFloatValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + pidProfile->D_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDYAW] + delta; pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 97822698a..d0677566c 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -865,7 +865,7 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_PID: headSerialReply(3 * PID_ITEM_COUNT); - if (currentProfile->pidProfile.pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid + if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid for (i = 0; i < 3; i++) { serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250)); @@ -1209,7 +1209,7 @@ static bool processInCommand(void) setPIDController(currentProfile->pidProfile.pidController); break; case MSP_SET_PID: - if (currentProfile->pidProfile.pidController == 2) { + if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { for (i = 0; i < 3; i++) { currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f; currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f; From 2ac72823142ea294c6b1fc54e1115da4cf468884 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 21:46:23 +0100 Subject: [PATCH 07/36] Use different scale for inflight adjustments to PIDs of FP based PID controllers. --- src/main/io/rc_controls.c | 38 ++++++++++++---------- src/test/Makefile | 1 + src/test/unit/rc_controls_unittest.cc | 47 +++++++++++---------------- 3 files changed, 40 insertions(+), 46 deletions(-) diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 203ed2b92..b73a867f1 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -32,10 +32,12 @@ #include "drivers/system.h" #include "flight/flight.h" +#include "flight/navigation.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" +#include "sensors/barometer.h" #include "sensors/battery.h" #include "sensors/sensors.h" #include "sensors/gyro.h" @@ -395,10 +397,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->P_f[PIDPITCH] + delta; - pidProfile->P_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = (int)pidProfile->P_f[PIDROLL] + delta; - pidProfile->P_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f); + pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f); + pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->P8[PIDPITCH] + delta; pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -408,10 +410,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->I_f[PIDPITCH] + delta; - pidProfile->I_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = (int)pidProfile->I_f[PIDROLL] + delta; - pidProfile->I_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 10.0f); + pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 10.0f); + pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDPITCH] + delta; pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -421,10 +423,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->D_f[PIDPITCH] + delta; - pidProfile->D_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = (int)pidProfile->D_f[PIDROLL] + delta; - pidProfile->D_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 10.0f); + pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 10.0f); + pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDPITCH] + delta; pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -434,8 +436,8 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->P_f[PIDYAW] + delta; - pidProfile->P_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f); + pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->P8[PIDYAW] + delta; pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -443,8 +445,8 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->I_f[PIDYAW] + delta; - pidProfile->I_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 10.0f); + pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDYAW] + delta; pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -452,8 +454,8 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->D_f[PIDYAW] + delta; - pidProfile->D_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 10.0f); + pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDYAW] + delta; pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c diff --git a/src/test/Makefile b/src/test/Makefile index 6642d98b8..9f3c5db21 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -263,6 +263,7 @@ $(OBJECT_DIR)/rc_controls_unittest.o : \ $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@ rc_controls_unittest : \ + $(OBJECT_DIR)/common/maths.o \ $(OBJECT_DIR)/io/rc_controls.o \ $(OBJECT_DIR)/rc_controls_unittest.o \ $(OBJECT_DIR)/gtest_main.a diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 91ec4f515..3e611ce1e 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -20,6 +20,7 @@ extern "C" { #include "platform.h" + #include "common/maths.h" #include "common/axis.h" #include "flight/flight.h" @@ -32,16 +33,6 @@ extern "C" { extern "C" { extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile); - -int constrain(int amt, int low, int high) -{ - if (amt < low) - return low; - else if (amt > high) - return high; - else - return amt; -} } TEST(RcControlsTest, updateActivatedModesWithAllInputsAtMidde) @@ -594,15 +585,15 @@ TEST(RcControlsTest, processPIDIncreasePidController0) EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); // and - EXPECT_EQ(pidProfile.P8[PIDPITCH], 1); - EXPECT_EQ(pidProfile.P8[PIDROLL], 6); - EXPECT_EQ(pidProfile.P8[YAW], 8); - EXPECT_EQ(pidProfile.I8[PIDPITCH], 11); - EXPECT_EQ(pidProfile.I8[PIDROLL], 16); - EXPECT_EQ(pidProfile.I8[YAW], 18); - EXPECT_EQ(pidProfile.D8[PIDPITCH], 21); - EXPECT_EQ(pidProfile.D8[PIDROLL], 26); - EXPECT_EQ(pidProfile.D8[YAW], 28); + EXPECT_EQ(1, pidProfile.P8[PIDPITCH]); + EXPECT_EQ(6, pidProfile.P8[PIDROLL]); + EXPECT_EQ(8, pidProfile.P8[YAW]); + EXPECT_EQ(11, pidProfile.I8[PIDPITCH]); + EXPECT_EQ(16, pidProfile.I8[PIDROLL]); + EXPECT_EQ(18, pidProfile.I8[YAW]); + EXPECT_EQ(21, pidProfile.D8[PIDPITCH]); + EXPECT_EQ(26, pidProfile.D8[PIDROLL]); + EXPECT_EQ(28, pidProfile.D8[YAW]); } TEST(RcControlsTest, processPIDIncreasePidController2) @@ -680,15 +671,15 @@ TEST(RcControlsTest, processPIDIncreasePidController2) EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); // and - EXPECT_EQ(pidProfile.P_f[PIDPITCH], 1.0f); - EXPECT_EQ(pidProfile.P_f[PIDROLL], 6.0f); - EXPECT_EQ(pidProfile.P_f[PIDYAW], 8.0f); - EXPECT_EQ(pidProfile.I_f[PIDPITCH], 11.0f); - EXPECT_EQ(pidProfile.I_f[PIDROLL], 16.0f); - EXPECT_EQ(pidProfile.I_f[PIDYAW], 18.0f); - EXPECT_EQ(pidProfile.D_f[PIDPITCH], 21.0f); - EXPECT_EQ(pidProfile.D_f[PIDROLL], 26.0f); - EXPECT_EQ(pidProfile.D_f[PIDYAW], 28.0f); + EXPECT_EQ(0.1f, pidProfile.P_f[PIDPITCH]); + EXPECT_EQ(5.1f, pidProfile.P_f[PIDROLL]); + EXPECT_EQ(7.1f, pidProfile.P_f[PIDYAW]); + EXPECT_EQ(10.1f, pidProfile.I_f[PIDPITCH]); + EXPECT_EQ(15.1f, pidProfile.I_f[PIDROLL]); + EXPECT_EQ(17.1f, pidProfile.I_f[PIDYAW]); + EXPECT_EQ(20.1f, pidProfile.D_f[PIDPITCH]); + EXPECT_EQ(25.1f, pidProfile.D_f[PIDROLL]); + EXPECT_EQ(27.1f, pidProfile.D_f[PIDYAW]); } From de9a89ca85a2e2cf4bd7cb0cc05c26bd444adaf2 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 21:47:29 +0100 Subject: [PATCH 08/36] Fix missing ) from ea386e6da2f15e1a7839901fa51add710c39c10d --- src/main/flight/autotune.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index 33974931b..189d9da24 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -226,7 +226,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin return errorAngle; } - if (IS_PID_CONTROLLER_FP_BASED(pidController) { + if (IS_PID_CONTROLLER_FP_BASED(pidController)) { // TODO support floating point based pid controllers return errorAngle; } From 1f3c9ab0dbf859b8fc4e05bc254ed3ac90f987d1 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 22:02:25 +0100 Subject: [PATCH 09/36] Fix failing targets. --- src/main/config/config.c | 2 +- src/main/target/OLIMEXINO/target.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 0d8c47eeb..eb5465c83 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -472,7 +472,7 @@ static void resetConf(void) masterConfig.escAndServoConfig.maxthrottle = 2000; masterConfig.motor_pwm_rate = 32000; masterConfig.looptime = 2000; - currentProfile->pidController = 3; + currentProfile->pidProfile.pidController = 3; currentProfile->pidProfile.P8[ROLL] = 36; currentProfile->pidProfile.P8[PITCH] = 36; currentProfile->failsafeConfig.failsafe_delay = 2; diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index 34ec32a7d..87dd9aadd 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -113,4 +113,4 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE - +#define BLACKBOX From d21a009d405262e8d8fc3fcee30dec8c46956fa0 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 22:09:36 +0100 Subject: [PATCH 10/36] Change scale for I and D for inflight adjustments to PIDs of FP based PID. --- src/main/io/rc_controls.c | 12 ++++++------ src/test/unit/rc_controls_unittest.cc | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index b73a867f1..9c79231c1 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -410,9 +410,9 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 10.0f); + newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f); pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 10.0f); + newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f); pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDPITCH] + delta; @@ -423,9 +423,9 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 10.0f); + newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f); pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 10.0f); + newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f); pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDPITCH] + delta; @@ -445,7 +445,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 10.0f); + newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f); pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDYAW] + delta; @@ -454,7 +454,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 10.0f); + newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f); pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDYAW] + delta; diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 3e611ce1e..c97348bff 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -674,12 +674,12 @@ TEST(RcControlsTest, processPIDIncreasePidController2) EXPECT_EQ(0.1f, pidProfile.P_f[PIDPITCH]); EXPECT_EQ(5.1f, pidProfile.P_f[PIDROLL]); EXPECT_EQ(7.1f, pidProfile.P_f[PIDYAW]); - EXPECT_EQ(10.1f, pidProfile.I_f[PIDPITCH]); - EXPECT_EQ(15.1f, pidProfile.I_f[PIDROLL]); - EXPECT_EQ(17.1f, pidProfile.I_f[PIDYAW]); - EXPECT_EQ(20.1f, pidProfile.D_f[PIDPITCH]); - EXPECT_EQ(25.1f, pidProfile.D_f[PIDROLL]); - EXPECT_EQ(27.1f, pidProfile.D_f[PIDYAW]); + EXPECT_EQ(10.01f, pidProfile.I_f[PIDPITCH]); + EXPECT_EQ(15.01f, pidProfile.I_f[PIDROLL]); + EXPECT_EQ(17.01f, pidProfile.I_f[PIDYAW]); + EXPECT_EQ(20.001f, pidProfile.D_f[PIDPITCH]); + EXPECT_EQ(25.001f, pidProfile.D_f[PIDROLL]); + EXPECT_EQ(27.001f, pidProfile.D_f[PIDYAW]); } From c85919325cc98b5fafab1204dade047835380218 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 22:43:04 +0100 Subject: [PATCH 11/36] Exclude a bit more code when USE_ADC is not defined for a target. --- src/main/drivers/adc.c | 10 ++++++++++ src/main/rx/rx.c | 8 +++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 4215df21d..eae243340 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -18,11 +18,14 @@ #include #include +#include "build_config.h" + #include "platform.h" #include "system.h" #include "adc.h" +#ifdef USE_ADC adc_config_t adcConfig[ADC_CHANNEL_COUNT]; volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; @@ -47,3 +50,10 @@ uint16_t adcGetChannel(uint8_t channel) return adcValues[adcConfig[channel].dmaIndex]; } +#else +uint16_t adcGetChannel(uint8_t channel) +{ + UNUSED(channel); + return 0; +} +#endif diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 17ce1f7a1..07ea188d6 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -21,6 +21,8 @@ #include +#include "build_config.h" + #include "platform.h" #include "common/maths.h" @@ -59,7 +61,7 @@ bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadR const char rcChannelLetters[] = "AERT12345678abcdefgh"; -uint16_t rssi; // range: [0;1023] +uint16_t rssi = 0; // range: [0;1023] int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] @@ -377,6 +379,9 @@ void updateRSSIPWM(void) void updateRSSIADC(uint32_t currentTime) { +#ifndef USE_ADC + UNUSED(currentTime); +#else static uint8_t adcRssiSamples[RSSI_ADC_SAMPLE_COUNT]; static uint8_t adcRssiSampleIndex = 0; static uint32_t rssiUpdateAt = 0; @@ -403,6 +408,7 @@ void updateRSSIADC(uint32_t currentTime) adcRssiMean = adcRssiMean / RSSI_ADC_SAMPLE_COUNT; rssi = (uint16_t)((constrain(adcRssiMean, 0, 100) / 100.0f) * 1023.0f); +#endif } void updateRSSI(uint32_t currentTime) From 27f0216a1976128585cc75a5e4fbb2f4aac8d57f Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 31 Jan 2015 21:56:01 +0100 Subject: [PATCH 12/36] Ensure that current meter feature is not disabled since it requires no hardware io. --- src/main/config/config.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index eb5465c83..8c49b2d79 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -676,7 +676,9 @@ void validateAndFixConfig(void) // rssi adc needs the same ports featureClear(FEATURE_RSSI_ADC); // current meter needs the same ports - featureClear(FEATURE_CURRENT_METER); + if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { + featureClear(FEATURE_CURRENT_METER); + } #endif #if defined(STM32F10X) || defined(CHEBUZZ) || defined(STM32F3DISCOVERY) @@ -709,13 +711,13 @@ void validateAndFixConfig(void) #endif #if defined(NAZE) && defined(SONAR) - if (feature(FEATURE_RX_PARALLEL_PWM) && feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER)) { + if (feature(FEATURE_RX_PARALLEL_PWM) && feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { featureClear(FEATURE_CURRENT_METER); } #endif #if defined(OLIMEXINO) && defined(SONAR) - if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER)) { + if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) { featureClear(FEATURE_CURRENT_METER); } #endif From a0cca0c889e425daa135a9cafd42fd0e3cf8ab67 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 31 Jan 2015 21:59:13 +0100 Subject: [PATCH 13/36] Updating sparky state in TODO list. --- docs/Board - Sparky.md | 1 - 1 file changed, 1 deletion(-) diff --git a/docs/Board - Sparky.md b/docs/Board - Sparky.md index 9501b5912..0169e6d55 100644 --- a/docs/Board - Sparky.md +++ b/docs/Board - Sparky.md @@ -18,7 +18,6 @@ Tested with revision 1 board. ## TODO * Baro - detection works but sending bad readings, disabled for now. -* LED Strip * ADC * Sonar * Display (via Flex port) From 7c2c5eb1528c692619f1e1d8e142c7848668b276 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 31 Jan 2015 22:08:03 +0100 Subject: [PATCH 14/36] Removing TODOs. If it needs to be done it will be done. --- src/main/flight/imu.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index afea1727e..ce3e18278 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -82,7 +82,6 @@ void configureIMU( pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband, float accz_lpf_cutoff, - //TODO: Move throttle angle correction code into flight/throttle_angle_correction.c uint16_t throttle_correction_angle ) { @@ -90,7 +89,6 @@ void configureIMU( pidProfile = initialPidProfile; accDeadband = initialAccDeadband; fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff); - //TODO: Move throttle angle correction code into flight/throttle_angle_correction.c throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); } @@ -101,7 +99,6 @@ void initIMU() gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; } -//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c float calculateThrottleAngleScale(uint16_t throttle_correction_angle) { return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); @@ -333,7 +330,6 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) } } -//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) { float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z); From 8b0a982931b3a81be8380dad81cf04bab51d5fd5 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 31 Jan 2015 22:12:11 +0100 Subject: [PATCH 15/36] Telemetry naming cleanup. --- src/main/main.c | 4 ++-- src/main/telemetry/telemetry.c | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index 3b1a33e8c..cc8d66960 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -93,7 +93,7 @@ failsafe_t *failsafe; void initPrintfSupport(void); void timerInit(void); -void initTelemetry(void); +void telemetryInit(void); void serialInit(serialConfig_t *initialSerialConfig); failsafe_t* failsafeInit(rxConfig_t *intialRxConfig); pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init); @@ -346,7 +346,7 @@ void init(void) #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) - initTelemetry(); + telemetryInit(); #endif #ifdef BLACKBOX diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index ad3e45cf1..8d840aca2 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -95,7 +95,7 @@ bool canUseTelemetryWithCurrentConfiguration(void) return true; } -void initTelemetry() +void telemetryInit() { if (isTelemetryProviderSmartPort()) { telemetryPortIsShared = isSerialPortFunctionShared(FUNCTION_SMARTPORT_TELEMETRY, FUNCTION_MSP); From 01b2ce0b36f19d5e7208ae7f7f9581a38cae4310 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 31 Jan 2015 22:23:38 +0100 Subject: [PATCH 16/36] IMU naming cleanup. --- src/main/config/config.c | 2 +- src/main/flight/altitudehold.c | 2 +- src/main/flight/imu.c | 24 ++++++++++++------------ src/main/flight/imu.h | 8 ++++---- src/main/main.c | 4 ++-- src/main/mw.c | 2 +- src/main/sensors/gyro.c | 2 +- src/main/sensors/gyro.h | 2 +- 8 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 8c49b2d79..f38c4499b 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -630,7 +630,7 @@ void activateConfig(void) imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;; imuRuntimeConfig.small_angle = masterConfig.small_angle; - configureIMU( + imuConfigure( &imuRuntimeConfig, ¤tProfile->pidProfile, ¤tProfile->accDeadband, diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index bfd7be91d..86e6ab700 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -294,7 +294,7 @@ void calculateEstimatedAltitude(uint32_t currentTime) debug[3] = accAlt; // height #endif - accSum_reset(); + imuResetAccelerationSum(); #ifdef BARO if (!isBaroCalibrationComplete()) { diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index ce3e18278..3f106a895 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -77,7 +77,7 @@ imuRuntimeConfig_t *imuRuntimeConfig; pidProfile_t *pidProfile; accDeadband_t *accDeadband; -void configureIMU( +void imuConfigure( imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband, @@ -92,7 +92,7 @@ void configureIMU( throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); } -void initIMU() +void imuInit() { smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle))); accVelScale = 9.80665f / acc_1G / 10000.0f; @@ -129,7 +129,7 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) t_fp_vector EstG; -void accSum_reset(void) +void imuResetAccelerationSum(void) { accSum[0] = 0; accSum[1] = 0; @@ -139,7 +139,7 @@ void accSum_reset(void) } // rotate acc into Earth frame and calculate acceleration in it -void acc_calc(uint32_t deltaT) +void imuCalculateAcceleration(uint32_t deltaT) { static int32_t accZoffset = 0; static float accz_smooth = 0; @@ -212,7 +212,7 @@ void acc_calc(uint32_t deltaT) * * //TODO: Add explanation for how it uses the Z dimension. */ -int16_t calculateHeading(t_fp_vector *vec) +int16_t imuCalculateHeading(t_fp_vector *vec) { int16_t head; @@ -234,7 +234,7 @@ int16_t calculateHeading(t_fp_vector *vec) return head; } -static void getEstimatedAttitude(void) +static void imuCalculateEstimatedAttitude(void) { int32_t axis; int32_t accMag = 0; @@ -295,24 +295,24 @@ static void getEstimatedAttitude(void) for (axis = 0; axis < 3; axis++) { EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor; } - heading = calculateHeading(&EstM); + heading = imuCalculateHeading(&EstM); } else { rotateV(&EstN.V, &deltaGyroAngle); normalizeV(&EstN.V, &EstN.V); - heading = calculateHeading(&EstN); + heading = imuCalculateHeading(&EstN); } - acc_calc(deltaT); // rotate acc vector into earth frame + imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame } -void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) +void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) { static int16_t gyroYawSmooth = 0; - gyroGetADC(); + gyroUpdate(); if (sensors(SENSOR_ACC)) { updateAccelerationReadings(accelerometerTrims); - getEstimatedAttitude(); + imuCalculateEstimatedAttitude(); } else { accADC[X] = 0; accADC[Y] = 0; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 45949c4e1..a8d76e35d 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -30,7 +30,7 @@ typedef struct imuRuntimeConfig_s { int8_t small_angle; } imuRuntimeConfig_t; -void configureIMU( +void imuConfigure( imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband, @@ -39,11 +39,11 @@ void configureIMU( ); void calculateEstimatedAltitude(uint32_t currentTime); -void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode); +void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode); float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); -int16_t calculateHeading(t_fp_vector *vec); +int16_t imuCalculateHeading(t_fp_vector *vec); -void accSum_reset(void); +void imuResetAccelerationSum(void); diff --git a/src/main/main.c b/src/main/main.c index cc8d66960..38b0ded3c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -104,7 +104,7 @@ void beepcodeInit(failsafe_t *initialFailsafe); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig); -void initIMU(void); +void imuInit(void); void displayInit(rxConfig_t *intialRxConfig); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse); void loop(void); @@ -267,7 +267,7 @@ void init(void) LED0_OFF; LED1_OFF; - initIMU(); + imuInit(); mixerInit(masterConfig.mixerMode, masterConfig.customMixer); #ifdef MAG diff --git a/src/main/mw.c b/src/main/mw.c index 028177515..62e7ca3e8 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -663,7 +663,7 @@ void loop(void) if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { loopTime = currentTime + masterConfig.looptime; - computeIMU(¤tProfile->accelerometerTrims, masterConfig.mixerMode); + imuUpdate(¤tProfile->accelerometerTrims, masterConfig.mixerMode); // Measure loop rate just after reading the sensors currentTime = micros(); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index d490a91f5..99b3a29ed 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -108,7 +108,7 @@ static void applyGyroZero(void) } } -void gyroGetADC(void) +void gyroUpdate(void) { // FIXME When gyro.read() fails due to i2c or other error gyroZero is continually re-applied to gyroADC resulting in a old reading that gets worse over time. diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 7acd2eb2a..a064a80dd 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -26,6 +26,6 @@ typedef struct gyroConfig_s { void useGyroConfig(gyroConfig_t *gyroConfigToUse); void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); -void gyroGetADC(void); +void gyroUpdate(void); bool isGyroCalibrationComplete(void); From 2a37e2671586491b01003c48501deb5754f8f324 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 31 Jan 2015 22:27:45 +0100 Subject: [PATCH 17/36] Use correct flag for GPIOC IDR check. --- .../CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c b/lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c index 1b0209447..299e03745 100755 --- a/lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c +++ b/lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c @@ -145,7 +145,7 @@ void SetSysClock(bool overclock) // On CJMCU new revision boards (Late 2014) bit 15 of GPIOC->IDR is '1'. RCC_CFGR_PLLMUL = RCC_CFGR_PLLMULL9; #else - RCC_CFGR_PLLMUL = GPIOC->IDR & CAN_MCR_RESET ? hse_value = 12000000, RCC_CFGR_PLLMULL6 : RCC_CFGR_PLLMULL9; + RCC_CFGR_PLLMUL = GPIOC->IDR & GPIO_IDR_IDR15 ? hse_value = 12000000, RCC_CFGR_PLLMULL6 : RCC_CFGR_PLLMULL9; #endif switch (clocksrc) { case SRC_HSE: From a9b2c39872e698edd71ab9e4bf397606215d9488 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 31 Jan 2015 22:29:17 +0100 Subject: [PATCH 18/36] printf naming cleanup --- src/main/common/printf.c | 4 ++-- src/main/main.c | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/common/printf.c b/src/main/common/printf.c index 46e5b3902..50d986bc1 100644 --- a/src/main/common/printf.c +++ b/src/main/common/printf.c @@ -185,7 +185,7 @@ static void _putc(void *p, char c) serialWrite(printfSerialPort, c); } -void initPrintfSupport(void) +void printfSupportInit(void) { init_printf(NULL, _putc); } @@ -201,7 +201,7 @@ int fputc(int c, FILE *f) return c; } -void initPrintfSupport(serialPort_t *serialPort) +void printfSupportInit(void) { // Nothing to do } diff --git a/src/main/main.c b/src/main/main.c index 38b0ded3c..b0d882eb4 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -91,7 +91,7 @@ serialPort_t *loopbackPort; failsafe_t *failsafe; -void initPrintfSupport(void); +void printfSupportInit(void); void timerInit(void); void telemetryInit(void); void serialInit(serialConfig_t *initialSerialConfig); @@ -125,7 +125,7 @@ void init(void) drv_pwm_config_t pwm_params; bool sensorsOK = false; - initPrintfSupport(); + printfSupportInit(); initEEPROM(); From 53406a7ac7871fd316076490f5226062ec84b8c4 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 31 Jan 2015 23:47:51 +0100 Subject: [PATCH 19/36] Relocate some structures and code to the right places. This cleans up the include file order somewhat and fixes a couple of dependencies. The goal of this is to rename flight.c/flight.h to pid.c/pid.h. --- src/main/blackbox/blackbox.c | 18 ++++++++------- src/main/blackbox/blackbox_io.c | 15 +++++++------ src/main/config/config.c | 22 +++++++++--------- src/main/drivers/serial_uart.c | 4 ++++ src/main/flight/altitudehold.c | 3 +-- src/main/flight/autotune.c | 6 +++++ src/main/flight/flight.c | 24 +++++++++----------- src/main/flight/flight.h | 36 ----------------------------- src/main/flight/imu.c | 13 +++++------ src/main/flight/imu.h | 30 ++++++++++++++++++++----- src/main/flight/mixer.c | 7 ++++++ src/main/io/rc_controls.c | 10 +++++---- src/main/io/serial_cli.c | 17 +++++++++----- src/main/io/serial_msp.c | 15 ++++++++----- src/main/main.c | 22 +++++++++++------- src/main/mw.c | 21 ++++++++++------- src/main/sensors/acceleration.c | 40 +++++++++++++++++++-------------- src/main/sensors/acceleration.h | 16 +++++++++---- src/main/sensors/sensors.h | 12 ++++++++++ src/main/telemetry/smartport.c | 12 ++++++---- 20 files changed, 198 insertions(+), 145 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2440af0a5..18589d73d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -36,7 +36,6 @@ #include "drivers/light_led.h" #include "drivers/sound_beeper.h" -#include "flight/flight.h" #include "sensors/sensors.h" #include "sensors/boardalignment.h" #include "sensors/sonar.h" @@ -45,17 +44,11 @@ #include "sensors/barometer.h" #include "sensors/gyro.h" #include "sensors/battery.h" + #include "io/beeper.h" #include "io/display.h" #include "io/escservo.h" -#include "rx/rx.h" #include "io/rc_controls.h" -#include "flight/mixer.h" -#include "flight/altitudehold.h" -#include "flight/failsafe.h" -#include "flight/imu.h" -#include "flight/autotune.h" -#include "flight/navigation.h" #include "io/gimbal.h" #include "io/gps.h" #include "io/ledstrip.h" @@ -63,9 +56,18 @@ #include "io/serial_cli.h" #include "io/serial_msp.h" #include "io/statusindicator.h" + +#include "rx/rx.h" #include "rx/msp.h" + #include "telemetry/telemetry.h" +#include "flight/mixer.h" +#include "flight/altitudehold.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/navigation.h" + #include "config/runtime_config.h" #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 194c03c40..50226ff20 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -21,24 +21,19 @@ #include "drivers/light_led.h" #include "drivers/sound_beeper.h" -#include "flight/flight.h" #include "sensors/sensors.h" #include "sensors/boardalignment.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/gyro.h" #include "sensors/battery.h" + #include "io/beeper.h" #include "io/display.h" #include "io/escservo.h" #include "rx/rx.h" #include "io/rc_controls.h" -#include "flight/mixer.h" -#include "flight/altitudehold.h" -#include "flight/failsafe.h" -#include "flight/imu.h" -#include "flight/autotune.h" -#include "flight/navigation.h" + #include "io/gimbal.h" #include "io/gps.h" #include "io/ledstrip.h" @@ -50,6 +45,12 @@ #include "telemetry/telemetry.h" #include "common/printf.h" +#include "flight/mixer.h" +#include "flight/altitudehold.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/navigation.h" + #include "config/runtime_config.h" #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/config/config.c b/src/main/config/config.c index f38c4499b..45d8a7ab6 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -27,38 +27,38 @@ #include "common/axis.h" #include "common/maths.h" -#include "flight/flight.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 "sensors/sensors.h" #include "sensors/gyro.h" #include "sensors/compass.h" - -#include "io/statusindicator.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" -#include "drivers/serial.h" -#include "io/serial.h" -#include "telemetry/telemetry.h" - -#include "flight/mixer.h" #include "sensors/boardalignment.h" #include "sensors/battery.h" + +#include "io/statusindicator.h" +#include "io/serial.h" #include "io/gimbal.h" #include "io/escservo.h" -#include "rx/rx.h" #include "io/rc_controls.h" #include "io/rc_curves.h" #include "io/ledstrip.h" #include "io/gps.h" + +#include "rx/rx.h" + +#include "telemetry/telemetry.h" + +#include "flight/mixer.h" +#include "flight/flight.h" #include "flight/failsafe.h" #include "flight/altitudehold.h" #include "flight/imu.h" diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 7a6e83b1f..0057b7ba9 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -40,6 +40,10 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode); uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode); static void usartConfigurePinInversion(uartPort_t *uartPort) { +#if !defined(INVERTER) && !defined(STM32F303xC) + UNUSED(uartPort); +#endif + #ifdef INVERTER if (uartPort->port.inversion == SERIAL_INVERTED && uartPort->USARTx == INVERTER_USART) { // Enable hardware inverter if available. diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 86e6ab700..1950a6688 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -30,14 +30,13 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" -#include "flight/flight.h" - #include "sensors/sensors.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/sonar.h" #include "flight/mixer.h" +#include "flight/flight.h" #include "flight/imu.h" #include "rx/rx.h" diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index 189d9da24..91bef60c3 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -28,8 +28,14 @@ #include "common/maths.h" #include "drivers/system.h" +#include "drivers/sensor.h" +#include "drivers/accgyro.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" #include "flight/flight.h" +#include "flight/imu.h" #include "config/config.h" #include "blackbox/blackbox.h" diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 2c4f090c9..053542d06 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -26,20 +26,24 @@ #include "common/axis.h" #include "common/maths.h" -#include "config/runtime_config.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 "drivers/sensor.h" -#include "drivers/accgyro.h" -#include "sensors/sensors.h" -#include "sensors/gyro.h" - #include "io/rc_controls.h" +#include "io/gps.h" + #include "flight/flight.h" +#include "flight/imu.h" #include "flight/navigation.h" #include "flight/autotune.h" -#include "io/gps.h" + +#include "config/runtime_config.h" extern uint16_t cycleTime; extern uint8_t motorCount; @@ -65,12 +69,6 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii -void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims) -{ - rollAndPitchTrims->values.roll = 0; - rollAndPitchTrims->values.pitch = 0; -} - void resetErrorAngle(void) { errorAngleI[ROLL] = 0; diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index cad033b4f..6a6c27b08 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -65,44 +65,9 @@ typedef enum { #define FLIGHT_DYNAMICS_INDEX_COUNT 3 -typedef struct int16_flightDynamicsTrims_s { - int16_t roll; - int16_t pitch; - int16_t yaw; -} flightDynamicsTrims_def_t; - -typedef union { - int16_t raw[3]; - flightDynamicsTrims_def_t values; -} flightDynamicsTrims_t; - -typedef struct rollAndPitchTrims_s { - int16_t roll; - int16_t pitch; -} rollAndPitchTrims_t_def; - -typedef union { - int16_t raw[2]; - rollAndPitchTrims_t_def values; -} rollAndPitchTrims_t; - -typedef struct rollAndPitchInclination_s { - // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 - int16_t rollDeciDegrees; - int16_t pitchDeciDegrees; -} rollAndPitchInclination_t_def; - -typedef union { - int16_t raw[ANGLE_INDEX_COUNT]; - rollAndPitchInclination_t_def values; -} rollAndPitchInclination_t; - - #define DEGREES_TO_DECIDEGREES(angle) (angle * 10) #define DECIDEGREES_TO_DEGREES(angle) (angle / 10.0f) -extern rollAndPitchInclination_t inclination; - extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT]; extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT]; @@ -118,7 +83,6 @@ extern int32_t AltHold; extern int32_t vario; void setPIDController(int type); -void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims); void resetErrorAngle(void); void resetErrorGyro(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 3f106a895..320eb8390 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -26,11 +26,8 @@ #include #include "common/axis.h" -#include "flight/flight.h" #include "drivers/system.h" - - #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/compass.h" @@ -42,14 +39,16 @@ #include "sensors/barometer.h" #include "sensors/sonar.h" +#include "flight/mixer.h" +#include "flight/flight.h" +#include "flight/imu.h" + #include "config/runtime_config.h" -#include "flight/mixer.h" -#include "flight/imu.h" extern int16_t debug[4]; -int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT]; +int16_t gyroADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT]; int32_t accSum[XYZ_AXIS_COUNT]; uint32_t accTimeSum = 0; // keep track for integration of acc @@ -311,7 +310,7 @@ void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) gyroUpdate(); if (sensors(SENSOR_ACC)) { - updateAccelerationReadings(accelerometerTrims); + updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer' imuCalculateEstimatedAttitude(); } else { accADC[X] = 0; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index a8d76e35d..f7a8f9188 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -22,6 +22,24 @@ extern uint32_t accTimeSum; extern int accSumCount; extern float accVelScale; +typedef struct rollAndPitchInclination_s { + // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 + int16_t rollDeciDegrees; + int16_t pitchDeciDegrees; +} rollAndPitchInclination_t_def; + +typedef union { + int16_t raw[ANGLE_INDEX_COUNT]; + rollAndPitchInclination_t_def values; +} rollAndPitchInclination_t; + +extern rollAndPitchInclination_t inclination; + +typedef struct accDeadband_s { + uint8_t xy; // set the acc deadband for xy-Axis + uint8_t z; // set the acc deadband for z-Axis, this ignores small accelerations +} accDeadband_t; + typedef struct imuRuntimeConfig_s { uint8_t acc_lpf_factor; uint8_t acc_unarmedcal; @@ -31,11 +49,11 @@ typedef struct imuRuntimeConfig_s { } imuRuntimeConfig_t; void imuConfigure( - imuRuntimeConfig_t *initialImuRuntimeConfig, - pidProfile_t *initialPidProfile, - accDeadband_t *initialAccDeadband, - float accz_lpf_cutoff, - uint16_t throttle_correction_angle + imuRuntimeConfig_t *initialImuRuntimeConfig, + pidProfile_t *initialPidProfile, + accDeadband_t *initialAccDeadband, + float accz_lpf_cutoff, + uint16_t throttle_correction_angle ); void calculateEstimatedAltitude(uint32_t currentTime); @@ -47,3 +65,5 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); int16_t imuCalculateHeading(t_fp_vector *vec); void imuResetAccelerationSum(void); + + diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f32237474..cfe9cac0d 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -30,14 +30,21 @@ #include "drivers/timer.h" #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" +#include "drivers/sensor.h" +#include "drivers/accgyro.h" #include "rx/rx.h" + #include "io/gimbal.h" #include "io/escservo.h" #include "io/rc_controls.h" +#include "sensors/sensors.h" +#include "sensors/acceleration.h" + #include "flight/mixer.h" #include "flight/flight.h" +#include "flight/imu.h" #include "config/runtime_config.h" #include "config/config.h" diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 9c79231c1..6b74c323e 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -31,9 +31,6 @@ #include "drivers/system.h" -#include "flight/flight.h" -#include "flight/navigation.h" - #include "drivers/sensor.h" #include "drivers/accgyro.h" @@ -45,13 +42,18 @@ #include "io/gps.h" #include "io/beeper.h" -#include "mw.h" #include "rx/rx.h" #include "io/escservo.h" #include "io/rc_controls.h" #include "io/rc_curves.h" +#include "flight/flight.h" +#include "flight/navigation.h" + +#include "mw.h" + + static escAndServoConfig_t *escAndServoConfig; static pidProfile_t *pidProfile; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 0735c87bd..bb950a69b 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -44,18 +44,18 @@ #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_rx.h" -#include "flight/flight.h" -#include "flight/mixer.h" -#include "flight/navigation.h" -#include "flight/failsafe.h" -#include "rx/rx.h" + + #include "io/escservo.h" #include "io/gps.h" #include "io/gimbal.h" #include "io/rc_controls.h" #include "io/serial.h" #include "io/ledstrip.h" + +#include "rx/rx.h" #include "rx/spektrum.h" + #include "sensors/battery.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h" @@ -63,6 +63,13 @@ #include "sensors/gyro.h" #include "sensors/compass.h" #include "sensors/barometer.h" + +#include "flight/flight.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/navigation.h" +#include "flight/failsafe.h" + #include "telemetry/telemetry.h" #include "config/runtime_config.h" diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index d0677566c..0494b99e6 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -41,20 +41,16 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" -#include "flight/flight.h" -#include "flight/mixer.h" -#include "flight/failsafe.h" -#include "flight/navigation.h" -#include "flight/altitudehold.h" - #include "rx/rx.h" #include "rx/msp.h" + #include "io/escservo.h" #include "io/rc_controls.h" #include "io/gps.h" #include "io/gimbal.h" #include "io/serial.h" #include "io/ledstrip.h" + #include "telemetry/telemetry.h" #include "sensors/boardalignment.h" @@ -66,6 +62,13 @@ #include "sensors/compass.h" #include "sensors/gyro.h" +#include "flight/flight.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/failsafe.h" +#include "flight/navigation.h" +#include "flight/altitudehold.h" + #include "config/runtime_config.h" #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/main.c b/src/main/main.c index b0d882eb4..27093e9e5 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -25,6 +25,8 @@ #include "common/axis.h" #include "common/color.h" #include "common/atomic.h" +#include "common/maths.h" + #include "drivers/nvic.h" #include "drivers/sensor.h" @@ -45,30 +47,34 @@ #include "drivers/bus_spi.h" #include "drivers/inverter.h" -#include "flight/flight.h" -#include "flight/mixer.h" +#include "rx/rx.h" #include "io/serial.h" -#include "flight/failsafe.h" -#include "flight/navigation.h" - -#include "rx/rx.h" #include "io/gps.h" #include "io/escservo.h" #include "io/rc_controls.h" #include "io/gimbal.h" #include "io/ledstrip.h" #include "io/display.h" + #include "sensors/sensors.h" #include "sensors/sonar.h" #include "sensors/barometer.h" #include "sensors/compass.h" #include "sensors/acceleration.h" #include "sensors/gyro.h" -#include "telemetry/telemetry.h" -#include "blackbox/blackbox.h" #include "sensors/battery.h" #include "sensors/boardalignment.h" + +#include "telemetry/telemetry.h" +#include "blackbox/blackbox.h" + +#include "flight/flight.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/failsafe.h" +#include "flight/navigation.h" + #include "config/runtime_config.h" #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/mw.c b/src/main/mw.c index 62e7ca3e8..34c9a6e39 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -37,7 +37,6 @@ #include "drivers/timer.h" #include "drivers/pwm_rx.h" -#include "flight/flight.h" #include "sensors/sensors.h" #include "sensors/boardalignment.h" #include "sensors/sonar.h" @@ -46,18 +45,12 @@ #include "sensors/barometer.h" #include "sensors/gyro.h" #include "sensors/battery.h" + #include "io/beeper.h" #include "io/display.h" #include "io/escservo.h" -#include "rx/rx.h" #include "io/rc_controls.h" #include "io/rc_curves.h" -#include "flight/mixer.h" -#include "flight/altitudehold.h" -#include "flight/failsafe.h" -#include "flight/imu.h" -#include "flight/autotune.h" -#include "flight/navigation.h" #include "io/gimbal.h" #include "io/gps.h" #include "io/ledstrip.h" @@ -65,10 +58,22 @@ #include "io/serial_cli.h" #include "io/serial_msp.h" #include "io/statusindicator.h" + +#include "rx/rx.h" #include "rx/msp.h" + #include "telemetry/telemetry.h" #include "blackbox/blackbox.h" +#include "flight/mixer.h" +#include "flight/flight.h" +#include "flight/altitudehold.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/autotune.h" +#include "flight/navigation.h" + + #include "config/runtime_config.h" #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 303a2148a..f02e0e992 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -25,8 +25,6 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" -#include "flight/flight.h" - #include "sensors/battery.h" #include "sensors/sensors.h" #include "io/beeper.h" @@ -36,6 +34,8 @@ #include "sensors/acceleration.h" +int16_t accADC[XYZ_AXIS_COUNT]; + acc_t acc; // acc access functions uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected sensor_align_e accAlign = 0; @@ -71,6 +71,12 @@ bool isOnFirstAccelerationCalibrationCycle(void) return calibratingA == CALIBRATING_ACC_CYCLES; } +void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims) +{ + rollAndPitchTrims->values.roll = 0; + rollAndPitchTrims->values.pitch = 0; +} + void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims) { static int32_t a[3]; @@ -92,9 +98,9 @@ void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims) if (isOnFinalAccelerationCalibrationCycle()) { // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration - accelerationTrims->raw[FD_ROLL] = (a[FD_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; - accelerationTrims->raw[FD_PITCH] = (a[FD_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; - accelerationTrims->raw[FD_YAW] = (a[FD_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G; + accelerationTrims->raw[X] = (a[X] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; + accelerationTrims->raw[Y] = (a[Y] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES; + accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G; resetRollAndPitchTrims(rollAndPitchTrims); @@ -113,9 +119,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri // Saving old zeropoints before measurement if (InflightcalibratingA == 50) { - accZero_saved[FD_ROLL] = accelerationTrims->raw[FD_ROLL]; - accZero_saved[FD_PITCH] = accelerationTrims->raw[FD_PITCH]; - accZero_saved[FD_YAW] = accelerationTrims->raw[FD_YAW]; + accZero_saved[X] = accelerationTrims->raw[X]; + accZero_saved[Y] = accelerationTrims->raw[Y]; + accZero_saved[Z] = accelerationTrims->raw[Z]; angleTrim_saved.values.roll = rollAndPitchTrims->values.roll; angleTrim_saved.values.pitch = rollAndPitchTrims->values.pitch; } @@ -136,9 +142,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri AccInflightCalibrationMeasurementDone = true; queueConfirmationBeep(5); // beeper to indicating the end of calibration // recover saved values to maintain current flight behaviour until new values are transferred - accelerationTrims->raw[FD_ROLL] = accZero_saved[FD_ROLL]; - accelerationTrims->raw[FD_PITCH] = accZero_saved[FD_PITCH]; - accelerationTrims->raw[FD_YAW] = accZero_saved[FD_YAW]; + accelerationTrims->raw[X] = accZero_saved[X]; + accelerationTrims->raw[Y] = accZero_saved[Y]; + accelerationTrims->raw[Z] = accZero_saved[Z]; rollAndPitchTrims->values.roll = angleTrim_saved.values.roll; rollAndPitchTrims->values.pitch = angleTrim_saved.values.pitch; } @@ -147,9 +153,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration if (AccInflightCalibrationSavetoEEProm) { // the aircraft is landed, disarmed and the combo has been done again AccInflightCalibrationSavetoEEProm = false; - accelerationTrims->raw[FD_ROLL] = b[FD_ROLL] / 50; - accelerationTrims->raw[FD_PITCH] = b[FD_PITCH] / 50; - accelerationTrims->raw[FD_YAW] = b[FD_YAW] / 50 - acc_1G; // for nunchuck 200=1G + accelerationTrims->raw[X] = b[X] / 50; + accelerationTrims->raw[Y] = b[Y] / 50; + accelerationTrims->raw[Z] = b[Z] / 50 - acc_1G; // for nunchuck 200=1G resetRollAndPitchTrims(rollAndPitchTrims); @@ -159,9 +165,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims) { - accADC[FD_ROLL] -= accelerationTrims->raw[FD_ROLL]; - accADC[FD_PITCH] -= accelerationTrims->raw[FD_PITCH]; - accADC[FD_YAW] -= accelerationTrims->raw[FD_YAW]; + accADC[X] -= accelerationTrims->raw[X]; + accADC[Y] -= accelerationTrims->raw[Y]; + accADC[Z] -= accelerationTrims->raw[Z]; } void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index b1cd83d75..159393aa1 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -36,12 +36,20 @@ extern sensor_align_e accAlign; extern acc_t acc; extern uint16_t acc_1G; -typedef struct accDeadband_s { - uint8_t xy; // set the acc deadband for xy-Axis - uint8_t z; // set the acc deadband for z-Axis, this ignores small accelerations -} accDeadband_t; +extern int16_t accADC[XYZ_AXIS_COUNT]; + +typedef struct rollAndPitchTrims_s { + int16_t roll; + int16_t pitch; +} rollAndPitchTrims_t_def; + +typedef union { + int16_t raw[2]; + rollAndPitchTrims_t_def values; +} rollAndPitchTrims_t; bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); +void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims); void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims); void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse); diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 81d82a9f8..805043d5c 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -17,6 +17,18 @@ #pragma once + +typedef struct int16_flightDynamicsTrims_s { + int16_t roll; + int16_t pitch; + int16_t yaw; +} flightDynamicsTrims_def_t; + +typedef union { + int16_t raw[3]; + flightDynamicsTrims_def_t values; +} flightDynamicsTrims_t; + #define CALIBRATING_GYRO_CYCLES 1000 #define CALIBRATING_ACC_CYCLES 400 #define CALIBRATING_BARO_CYCLES 200 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 4451fd856..2add9afcc 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -27,18 +27,16 @@ #include "drivers/adc.h" #include "drivers/light_led.h" -#include "flight/flight.h" -#include "flight/mixer.h" -#include "flight/failsafe.h" -#include "flight/navigation.h" #include "rx/rx.h" #include "rx/msp.h" + #include "io/escservo.h" #include "io/rc_controls.h" #include "io/gps.h" #include "io/gimbal.h" #include "io/serial.h" #include "io/ledstrip.h" + #include "sensors/boardalignment.h" #include "sensors/sensors.h" #include "sensors/battery.h" @@ -47,6 +45,12 @@ #include "sensors/compass.h" #include "sensors/gyro.h" +#include "flight/flight.h" +#include "flight/imu.h" +#include "flight/mixer.h" +#include "flight/failsafe.h" +#include "flight/navigation.h" + #include "telemetry/telemetry.h" #include "telemetry/smartport.h" From 8cc9e8ca37e4d7130aff3cdb2c9b760c682045f8 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 1 Feb 2015 00:03:46 +0100 Subject: [PATCH 20/36] More relocation of variables. --- src/main/common/axis.h | 15 +++++++++++++++ src/main/flight/altitudehold.h | 3 +++ src/main/flight/flight.c | 2 +- src/main/flight/flight.h | 27 --------------------------- src/main/flight/imu.c | 8 ++------ src/main/flight/imu.h | 5 +++++ src/main/flight/navigation.c | 1 + src/main/flight/navigation.h | 2 ++ src/main/io/display.c | 28 ++++++++++++++++------------ src/main/sensors/gyro.c | 2 ++ src/main/sensors/gyro.h | 3 +++ src/main/telemetry/frsky.c | 20 ++++++++++++++------ src/main/telemetry/smartport.c | 1 + 13 files changed, 65 insertions(+), 52 deletions(-) diff --git a/src/main/common/axis.h b/src/main/common/axis.h index fffd76432..c7f1a96a8 100644 --- a/src/main/common/axis.h +++ b/src/main/common/axis.h @@ -25,3 +25,18 @@ typedef enum { #define XYZ_AXIS_COUNT 3 +// See http://en.wikipedia.org/wiki/Flight_dynamics +typedef enum { + FD_ROLL = 0, + FD_PITCH, + FD_YAW +} flight_dynamics_index_t; + +#define FLIGHT_DYNAMICS_INDEX_COUNT 3 + +typedef enum { + AI_ROLL = 0, + AI_PITCH, +} angle_index_t; + +#define ANGLE_INDEX_COUNT 2 diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index 1e42c65ef..8f45ce3c7 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -22,6 +22,9 @@ #include "sensors/barometer.h" +extern int32_t AltHold; +extern int32_t vario; + void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig); void applyAltHold(airplaneConfig_t *airplaneConfig); void updateAltHoldState(void); diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 053542d06..252fb1543 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -48,7 +48,7 @@ extern uint16_t cycleTime; extern uint8_t motorCount; -int16_t heading, magHold; +int16_t heading; int16_t axisPID[3]; #ifdef BLACKBOX diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index 6a6c27b08..b70273416 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -49,39 +49,12 @@ typedef struct pidProfile_s { uint8_t H_sensitivity; } pidProfile_t; -typedef enum { - AI_ROLL = 0, - AI_PITCH, -} angle_index_t; - -#define ANGLE_INDEX_COUNT 2 - -// See http://en.wikipedia.org/wiki/Flight_dynamics -typedef enum { - FD_ROLL = 0, - FD_PITCH, - FD_YAW -} flight_dynamics_index_t; - -#define FLIGHT_DYNAMICS_INDEX_COUNT 3 - #define DEGREES_TO_DECIDEGREES(angle) (angle * 10) #define DECIDEGREES_TO_DEGREES(angle) (angle / 10.0f) -extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT]; -extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT]; - -extern int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT]; -extern int32_t accSum[XYZ_AXIS_COUNT]; extern int16_t axisPID[XYZ_AXIS_COUNT]; - extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; -extern int16_t heading, magHold; - -extern int32_t AltHold; -extern int32_t vario; - void setPIDController(int type); void resetErrorAngle(void); void resetErrorGyro(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 320eb8390..faf92d600 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -48,8 +48,9 @@ extern int16_t debug[4]; -int16_t gyroADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT]; +int16_t accSmooth[XYZ_AXIS_COUNT]; int32_t accSum[XYZ_AXIS_COUNT]; +int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; uint32_t accTimeSum = 0; // keep track for integration of acc int accSumCount = 0; @@ -63,11 +64,6 @@ float fc_acc; float magneticDeclination = 0.0f; // calculated at startup from config float gyroScaleRad; -// ************** -// gyro+acc IMU -// ************** -int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; -int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; rollAndPitchInclination_t inclination = { { 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index f7a8f9188..9d1c83110 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -22,6 +22,11 @@ extern uint32_t accTimeSum; extern int accSumCount; extern float accVelScale; +extern int16_t accSmooth[XYZ_AXIS_COUNT]; +extern int32_t accSum[XYZ_AXIS_COUNT]; +extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT]; + + typedef struct rollAndPitchInclination_s { // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 int16_t rollDeciDegrees; diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 9c1e919cc..224aa6ed3 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -57,6 +57,7 @@ extern int16_t debug[4]; bool areSticksInApModePosition(uint16_t ap_mode); +int16_t magHold; // ********************** // GPS diff --git a/src/main/flight/navigation.h b/src/main/flight/navigation.h index 673cf5cfb..ed9165faa 100644 --- a/src/main/flight/navigation.h +++ b/src/main/flight/navigation.h @@ -17,6 +17,8 @@ #pragma once +extern int16_t magHold; + // navigation mode typedef enum { NAV_MODE_NONE = 0, diff --git a/src/main/io/display.c b/src/main/io/display.c index ef103fd44..045afd406 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -26,32 +26,36 @@ #include "build_config.h" #include "drivers/serial.h" +#include "drivers/system.h" +#include "drivers/display_ug2864hsweg01.h" +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" + #include "common/printf.h" #include "common/maths.h" +#include "common/axis.h" #ifdef DISPLAY -#include "drivers/system.h" -#include "drivers/display_ug2864hsweg01.h" - -#include "drivers/sensor.h" -#include "drivers/compass.h" - #include "sensors/battery.h" - -#include "common/axis.h" -#include "flight/flight.h" #include "sensors/sensors.h" #include "sensors/compass.h" +#include "sensors/acceleration.h" +#include "sensors/gyro.h" + +#include "rx/rx.h" + +#include "io/rc_controls.h" + +#include "flight/flight.h" +#include "flight/imu.h" #ifdef GPS #include "io/gps.h" #include "flight/navigation.h" #endif -#include "rx/rx.h" -#include "io/rc_controls.h" - #include "config/runtime_config.h" #include "config/config.h" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 99b3a29ed..cd5c15d86 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -33,6 +33,8 @@ #include "sensors/gyro.h" uint16_t calibratingG = 0; +int16_t gyroADC[XYZ_AXIS_COUNT]; +int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; static gyroConfig_t *gyroConfig; diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index a064a80dd..caa34281d 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -20,6 +20,9 @@ extern gyro_t gyro; extern sensor_align_e gyroAlign; +extern int16_t gyroADC[XYZ_AXIS_COUNT]; +extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT]; + 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; diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index f30f961eb..f4f2e2b07 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -36,20 +36,28 @@ #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/serial.h" -#include "io/serial.h" -#include "rx/rx.h" -#include "io/rc_controls.h" -#include "config/runtime_config.h" -#include "config/config.h" #include "sensors/sensors.h" +#include "sensors/acceleration.h" #include "sensors/gyro.h" #include "sensors/barometer.h" #include "sensors/battery.h" -#include "flight/flight.h" + +#include "io/serial.h" +#include "io/rc_controls.h" #include "io/gps.h" +#include "rx/rx.h" + +#include "flight/mixer.h" +#include "flight/flight.h" +#include "flight/imu.h" +#include "flight/altitudehold.h" + +#include "config/runtime_config.h" +#include "config/config.h" + #include "telemetry/telemetry.h" #include "telemetry/frsky.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 2add9afcc..1f9dc1a86 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -50,6 +50,7 @@ #include "flight/mixer.h" #include "flight/failsafe.h" #include "flight/navigation.h" +#include "flight/altitudehold.h" #include "telemetry/telemetry.h" #include "telemetry/smartport.h" From 55cac2bdeb5a80676b06054aa72250b9d42b6099 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 1 Feb 2015 00:39:38 +0100 Subject: [PATCH 21/36] Finally rename flight.c/.h to pid.c/.h. Cleanup some dependencies. Relocate more code. --- Makefile | 2 +- src/main/config/config.c | 5 +++-- src/main/flight/altitudehold.c | 9 ++++---- src/main/flight/altitudehold.h | 3 +-- src/main/flight/autotune.c | 2 +- src/main/flight/imu.c | 2 +- src/main/flight/mixer.c | 2 +- src/main/flight/navigation.c | 28 +++++++++++------------- src/main/flight/navigation.h | 2 -- src/main/flight/{flight.c => pid.c} | 2 +- src/main/flight/{flight.h => pid.h} | 0 src/main/io/display.c | 2 +- src/main/io/gps.c | 20 +++++++---------- src/main/io/ledstrip.c | 14 +++++++----- src/main/io/rc_controls.c | 7 +++--- src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 6 +++-- src/main/main.c | 2 +- src/main/mw.c | 6 +++-- src/main/mw.h | 2 ++ src/main/sensors/compass.c | 1 - src/main/sensors/gyro.c | 1 - src/main/sensors/initialisation.c | 1 - src/main/sensors/sonar.c | 2 -- src/main/telemetry/frsky.c | 2 +- src/main/telemetry/hott.c | 2 +- src/main/telemetry/smartport.c | 2 +- src/test/unit/altitude_hold_unittest.cc | 20 +++++++++-------- src/test/unit/flight_imu_unittest.cc | 17 ++++++++------ src/test/unit/ledstrip_unittest.cc | 1 - src/test/unit/rc_controls_unittest.cc | 14 ++++++++++-- src/test/unit/telemetry_hott_unittest.cc | 13 +++++------ 32 files changed, 101 insertions(+), 93 deletions(-) rename src/main/flight/{flight.c => pid.c} (99%) rename src/main/flight/{flight.h => pid.h} (100%) diff --git a/Makefile b/Makefile index aadb9ab50..fa84772d6 100644 --- a/Makefile +++ b/Makefile @@ -203,7 +203,7 @@ COMMON_SRC = build_config.c \ mw.c \ flight/altitudehold.c \ flight/failsafe.c \ - flight/flight.c \ + flight/pid.c \ flight/imu.c \ flight/mixer.c \ drivers/bus_i2c_soft.c \ diff --git a/src/main/config/config.c b/src/main/config/config.c index 45d8a7ab6..7250c45a2 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -58,14 +58,15 @@ #include "telemetry/telemetry.h" #include "flight/mixer.h" -#include "flight/flight.h" +#include "flight/pid.h" +#include "flight/imu.h" #include "flight/failsafe.h" #include "flight/altitudehold.h" -#include "flight/imu.h" #include "flight/navigation.h" #include "config/runtime_config.h" #include "config/config.h" + #include "config/config_profile.h" #include "config/config_master.h" diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 1950a6688..3670b7965 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -35,14 +35,15 @@ #include "sensors/barometer.h" #include "sensors/sonar.h" -#include "flight/mixer.h" -#include "flight/flight.h" -#include "flight/imu.h" - #include "rx/rx.h" + #include "io/rc_controls.h" #include "io/escservo.h" +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" + #include "config/runtime_config.h" extern int16_t debug[4]; diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index 8f45ce3c7..e8ccfec92 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -15,10 +15,9 @@ * along with Cleanflight. If not, see . */ -#include "flight/flight.h" - #include "io/escservo.h" #include "io/rc_controls.h" +#include "flight/pid.h" #include "sensors/barometer.h" diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index 91bef60c3..bfc0f53b2 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -34,7 +34,7 @@ #include "sensors/sensors.h" #include "sensors/acceleration.h" -#include "flight/flight.h" +#include "flight/pid.h" #include "flight/imu.h" #include "config/config.h" diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index faf92d600..23a3805ea 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -40,7 +40,7 @@ #include "sensors/sonar.h" #include "flight/mixer.h" -#include "flight/flight.h" +#include "flight/pid.h" #include "flight/imu.h" #include "config/runtime_config.h" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index cfe9cac0d..9a66157d9 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -43,7 +43,7 @@ #include "sensors/acceleration.h" #include "flight/mixer.h" -#include "flight/flight.h" +#include "flight/pid.h" #include "flight/imu.h" #include "config/runtime_config.h" diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 224aa6ed3..6153f121d 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -25,31 +25,31 @@ #include "platform.h" #include "common/maths.h" +#include "common/axis.h" #include "drivers/system.h" - #include "drivers/serial.h" #include "drivers/serial_uart.h" -#include "io/serial.h" - #include "drivers/gpio.h" #include "drivers/light_led.h" -#include "common/axis.h" -#include "flight/flight.h" - #include "sensors/sensors.h" +#include "io/serial.h" +#include "io/gps.h" +#include "io/rc_controls.h" + +#include "flight/pid.h" +#include "flight/navigation.h" +#include "flight/gps_conversion.h" + +#include "rx/rx.h" + + #include "config/config.h" #include "config/runtime_config.h" -#include "flight/gps_conversion.h" -#include "io/gps.h" - -#include "rx/rx.h" -#include "io/rc_controls.h" - -#include "flight/navigation.h" +extern int16_t magHold; #ifdef GPS @@ -57,8 +57,6 @@ extern int16_t debug[4]; bool areSticksInApModePosition(uint16_t ap_mode); -int16_t magHold; - // ********************** // GPS // ********************** diff --git a/src/main/flight/navigation.h b/src/main/flight/navigation.h index ed9165faa..673cf5cfb 100644 --- a/src/main/flight/navigation.h +++ b/src/main/flight/navigation.h @@ -17,8 +17,6 @@ #pragma once -extern int16_t magHold; - // navigation mode typedef enum { NAV_MODE_NONE = 0, diff --git a/src/main/flight/flight.c b/src/main/flight/pid.c similarity index 99% rename from src/main/flight/flight.c rename to src/main/flight/pid.c index 252fb1543..fccab9ab5 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/pid.c @@ -38,7 +38,7 @@ #include "io/rc_controls.h" #include "io/gps.h" -#include "flight/flight.h" +#include "flight/pid.h" #include "flight/imu.h" #include "flight/navigation.h" #include "flight/autotune.h" diff --git a/src/main/flight/flight.h b/src/main/flight/pid.h similarity index 100% rename from src/main/flight/flight.h rename to src/main/flight/pid.h diff --git a/src/main/io/display.c b/src/main/io/display.c index 045afd406..3da875005 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -48,7 +48,7 @@ #include "io/rc_controls.h" -#include "flight/flight.h" +#include "flight/pid.h" #include "flight/imu.h" #ifdef GPS diff --git a/src/main/io/gps.c b/src/main/io/gps.c index db0f726d3..fd923b2e9 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -27,30 +27,26 @@ #include "build_config.h" #include "common/maths.h" +#include "common/axis.h" #include "drivers/system.h" - #include "drivers/serial.h" #include "drivers/serial_uart.h" -#include "io/serial.h" - #include "drivers/gpio.h" #include "drivers/light_led.h" -#include "common/axis.h" -#include "flight/flight.h" - #include "sensors/sensors.h" -#include "config/config.h" -#include "config/runtime_config.h" - -#include "flight/gps_conversion.h" -#include "flight/navigation.h" - +#include "io/serial.h" #include "io/display.h" #include "io/gps.h" +#include "flight/gps_conversion.h" +#include "flight/pid.h" +#include "flight/navigation.h" + +#include "config/config.h" +#include "config/runtime_config.h" #ifdef GPS diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 34adfbf50..1de4ff7b8 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -28,24 +28,26 @@ #ifdef LED_STRIP #include +#include +#include #include "drivers/light_ws2811strip.h" #include "drivers/system.h" #include "drivers/serial.h" -#include #include -#include #include "sensors/battery.h" -#include "config/runtime_config.h" -#include "config/config.h" -#include "rx/rx.h" #include "io/rc_controls.h" +#include "io/ledstrip.h" + +#include "rx/rx.h" + #include "flight/failsafe.h" -#include "io/ledstrip.h" +#include "config/runtime_config.h" +#include "config/config.h" static bool ledStripInitialised = false; static bool ledStripEnabled = true; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 6b74c323e..54ecb963b 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -30,7 +30,6 @@ #include "config/runtime_config.h" #include "drivers/system.h" - #include "drivers/sensor.h" #include "drivers/accgyro.h" @@ -40,15 +39,15 @@ #include "sensors/gyro.h" #include "sensors/acceleration.h" +#include "rx/rx.h" + #include "io/gps.h" #include "io/beeper.h" - -#include "rx/rx.h" #include "io/escservo.h" #include "io/rc_controls.h" #include "io/rc_curves.h" -#include "flight/flight.h" +#include "flight/pid.h" #include "flight/navigation.h" #include "mw.h" diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index bb950a69b..8fa43b5c5 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -64,7 +64,7 @@ #include "sensors/compass.h" #include "sensors/barometer.h" -#include "flight/flight.h" +#include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/navigation.h" diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 0494b99e6..929acf953 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -62,13 +62,15 @@ #include "sensors/compass.h" #include "sensors/gyro.h" -#include "flight/flight.h" -#include "flight/imu.h" #include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" #include "flight/failsafe.h" #include "flight/navigation.h" #include "flight/altitudehold.h" +#include "mw.h" + #include "config/runtime_config.h" #include "config/config.h" #include "config/config_profile.h" diff --git a/src/main/main.c b/src/main/main.c index 27093e9e5..ba7453265 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -69,7 +69,7 @@ #include "telemetry/telemetry.h" #include "blackbox/blackbox.h" -#include "flight/flight.h" +#include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/failsafe.h" diff --git a/src/main/mw.c b/src/main/mw.c index 34c9a6e39..4291bd4a4 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -66,10 +66,10 @@ #include "blackbox/blackbox.h" #include "flight/mixer.h" -#include "flight/flight.h" +#include "flight/pid.h" +#include "flight/imu.h" #include "flight/altitudehold.h" #include "flight/failsafe.h" -#include "flight/imu.h" #include "flight/autotune.h" #include "flight/navigation.h" @@ -94,6 +94,8 @@ int16_t debug[4]; uint32_t currentTime = 0; uint32_t previousTime = 0; uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop + +int16_t magHold; int16_t headFreeModeHold; int16_t telemTemperature1; // gyro sensor temperature diff --git a/src/main/mw.h b/src/main/mw.h index aebee40be..d1d49bdf7 100644 --- a/src/main/mw.h +++ b/src/main/mw.h @@ -17,6 +17,8 @@ #pragma once +extern int16_t magHold; + void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta); void handleInflightCalibrationStickPosition(); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 343c9d535..e3c18036a 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -28,7 +28,6 @@ #include "drivers/gpio.h" #include "drivers/light_led.h" -#include "flight/flight.h" #include "sensors/boardalignment.h" #include "config/runtime_config.h" #include "config/config.h" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index cd5c15d86..71ac374bb 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -25,7 +25,6 @@ #include "drivers/sensor.h" #include "drivers/accgyro.h" -#include "flight/flight.h" #include "sensors/sensors.h" #include "io/statusindicator.h" #include "sensors/boardalignment.h" diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 099e6fe00..f278893e7 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -52,7 +52,6 @@ #include "drivers/gpio.h" #include "drivers/system.h" -#include "flight/flight.h" #include "config/runtime_config.h" #include "sensors/sensors.h" diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 842df6404..b3a5438b9 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -28,8 +28,6 @@ #include "config/runtime_config.h" #include "config/config.h" -#include "flight/flight.h" - #include "sensors/sensors.h" #include "sensors/sonar.h" diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index f4f2e2b07..25691fb47 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -51,7 +51,7 @@ #include "rx/rx.h" #include "flight/mixer.h" -#include "flight/flight.h" +#include "flight/pid.h" #include "flight/imu.h" #include "flight/altitudehold.h" diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 356c1f2b6..d0bfd56bf 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -74,7 +74,7 @@ #include "sensors/sensors.h" #include "sensors/battery.h" -#include "flight/flight.h" +#include "flight/pid.h" #include "flight/navigation.h" #include "io/gps.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 1f9dc1a86..01c8d74a6 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -45,7 +45,7 @@ #include "sensors/compass.h" #include "sensors/gyro.h" -#include "flight/flight.h" +#include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/failsafe.h" diff --git a/src/test/unit/altitude_hold_unittest.cc b/src/test/unit/altitude_hold_unittest.cc index bc2b7e467..b895a1b1b 100644 --- a/src/test/unit/altitude_hold_unittest.cc +++ b/src/test/unit/altitude_hold_unittest.cc @@ -24,25 +24,27 @@ extern "C" { #include "common/axis.h" - #include "flight/flight.h" + #include "common/maths.h" - #include "sensors/sensors.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" + + #include "sensors/sensors.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" - #include "flight/mixer.h" - #include "flight/mixer.h" - #include "io/escservo.h" - #include "rx/rx.h" #include "io/rc_controls.h" - #include "config/runtime_config.h" + #include "rx/rx.h" + #include "flight/mixer.h" + #include "flight/pid.h" + #include "flight/imu.h" #include "flight/altitudehold.h" + #include "config/runtime_config.h" + } #include "unittest_macros.h" @@ -148,7 +150,7 @@ uint8_t armingFlags; int32_t sonarAlt; -void gyroGetADC(void) {}; +void gyroUpdate(void) {}; bool sensors(uint32_t mask) { UNUSED(mask); @@ -159,7 +161,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) UNUSED(rollAndPitchTrims); } -void accSum_reset(void) {}; +void imuResetAccelerationSum(void) {}; int32_t applyDeadband(int32_t, int32_t) { return 0; } uint32_t micros(void) { return 0; } diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index d82d12fb1..379b2cfaf 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -25,7 +25,6 @@ extern "C" { #include "common/axis.h" #include "common/maths.h" - #include "flight/flight.h" #include "sensors/sensors.h" #include "drivers/sensor.h" @@ -39,6 +38,7 @@ extern "C" { #include "config/runtime_config.h" #include "flight/mixer.h" + #include "flight/pid.h" #include "flight/imu.h" } @@ -53,19 +53,19 @@ TEST(FlightImuTest, TestCalculateHeading) { //TODO: Add test cases using the Z dimension. t_fp_vector north = {.A={1.0f, 0.0f, 0.0f}}; - EXPECT_EQ(calculateHeading(&north), 0); + EXPECT_EQ(imuCalculateHeading(&north), 0); t_fp_vector east = {.A={0.0f, 1.0f, 0.0f}}; - EXPECT_EQ(calculateHeading(&east), 90); + EXPECT_EQ(imuCalculateHeading(&east), 90); t_fp_vector south = {.A={-1.0f, 0.0f, 0.0f}}; - EXPECT_EQ(calculateHeading(&south), 180); + EXPECT_EQ(imuCalculateHeading(&south), 180); t_fp_vector west = {.A={0.0f, -1.0f, 0.0f}}; - EXPECT_EQ(calculateHeading(&west), 270); + EXPECT_EQ(imuCalculateHeading(&west), 270); t_fp_vector north_east = {.A={1.0f, 1.0f, 0.0f}}; - EXPECT_EQ(calculateHeading(&north_east), 45); + EXPECT_EQ(imuCalculateHeading(&north_east), 45); } // STUBS @@ -86,9 +86,12 @@ uint16_t flightModeFlags; uint8_t armingFlags; int32_t sonarAlt; +int16_t accADC[XYZ_AXIS_COUNT]; +int16_t gyroADC[XYZ_AXIS_COUNT]; -void gyroGetADC(void) {}; + +void gyroUpdate(void) {}; bool sensors(uint32_t mask) { UNUSED(mask); diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 8ad6e3144..6cd1bf9dc 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -24,7 +24,6 @@ extern "C" { #include "common/color.h" #include "common/axis.h" - #include "flight/flight.h" #include "sensors/battery.h" #include "config/runtime_config.h" diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index c97348bff..0c3580f06 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -20,14 +20,24 @@ extern "C" { #include "platform.h" + #include "common/maths.h" #include "common/axis.h" - #include "flight/flight.h" - #include "rx/rx.h" + #include "drivers/sensor.h" + #include "drivers/accgyro.h" + + #include "sensors/sensors.h" + #include "sensors/acceleration.h" + #include "io/escservo.h" #include "io/rc_controls.h" + + #include "rx/rx.h" + + #include "flight/pid.h" } + #include "unittest_macros.h" #include "gtest/gtest.h" diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index ad277ba00..51fca8d5a 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -27,22 +27,21 @@ extern "C" { #include "common/axis.h" #include "drivers/system.h" - #include "drivers/serial.h" - #include "io/serial.h" - - #include "config/runtime_config.h" #include "sensors/sensors.h" - - #include "flight/flight.h" - #include "io/gps.h" #include "sensors/battery.h" + #include "io/serial.h" + #include "io/gps.h" + #include "telemetry/telemetry.h" #include "telemetry/hott.h" + #include "flight/pid.h" #include "flight/gps_conversion.h" + + #include "config/runtime_config.h" } #include "unittest_macros.h" From 0fd7a2568c9b0c522a86f8d9e736ddd05c82ef1b Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 1 Feb 2015 17:18:20 +0100 Subject: [PATCH 22/36] Fix for BaroAlt resetting to 0. This needs a little more investigation since it only seems to happen on the Sparky (F3 target). The BaroAlt resets to 0 because of floating point divide by 0 when calculating accZ_tmp which results in accAlt becoming not-a-number (NaN). --- src/main/flight/altitudehold.c | 8 ++++++-- src/main/target/SPARKY/target.h | 4 ++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 3670b7965..66195165a 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -280,7 +280,11 @@ void calculateEstimatedAltitude(uint32_t currentTime) dt = accTimeSum * 1e-6f; // delta acc reading time in seconds // Integrator - velocity, cm/sec - accZ_tmp = (float)accSum[2] / (float)accSumCount; + if (accSumCount) { + accZ_tmp = (float)accSum[2] / (float)accSumCount; + } else { + accZ_tmp = 0; + } vel_acc = accZ_tmp * accVelScale * (float)accTimeSum; // Integrator - Altitude in cm @@ -288,7 +292,7 @@ void calculateEstimatedAltitude(uint32_t currentTime) accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) vel += vel_acc; -#if 0 +#if 1 debug[1] = accSum[2] / accSumCount; // acceleration debug[2] = vel; // velocity debug[3] = accAlt; // height diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index aee9257cd..d9bfe31e3 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -39,8 +39,8 @@ #define ACC_MPU6050_ALIGN CW270_DEG -//#define BARO -//#define USE_BARO_MS5611 +#define BARO +#define USE_BARO_MS5611 #define MAG #define USE_MAG_AK8975 From 5512ff08c365759773bcd69f3a14dd91eef12180 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 1 Feb 2015 18:32:14 +0000 Subject: [PATCH 23/36] Allow thrust color to be combined with throttle. --- docs/LedStrip.md | 2 +- src/main/io/ledstrip.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/LedStrip.md b/docs/LedStrip.md index dffc9d99f..07f08a94f 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -178,7 +178,7 @@ Note: Armed State cannot be used with Flight Mode. This mode fades the LED current LED color to the previous/next color in the HSB color space depending on throttle stick position. When the throttle is in the middle position the color is unaffected, thus it can be mixed with orientation colors to indicate orientation and throttle at -the same time. +the same time. Thrust should normally be combined with Color or Mode/Orientation. #### Thrust ring state diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 1de4ff7b8..5d40671a5 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -771,7 +771,7 @@ void applyLedThrottleLayer() int scaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, -60, +60); scaled += HSV_HUE_MAX; - color.h = scaled % HSV_HUE_MAX; + color.h = (color.h + scaled) % HSV_HUE_MAX; setLedHsv(ledIndex, &color); } } From 7ff15b0ea713e8db209df1c639adf548f2587fef Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 2 Feb 2015 00:16:07 +0000 Subject: [PATCH 24/36] Updating sparky state in TODO list now that baro is confirmed working on a board with a good baro. As of this commit the following was observed: * Baro's on Sparky boards supplied by Witespy / RTF Quads are bad (very very noisy). * Baro's on Sparky boards supplied by RadioC are fine. --- docs/Board - Sparky.md | 1 - 1 file changed, 1 deletion(-) diff --git a/docs/Board - Sparky.md b/docs/Board - Sparky.md index 0169e6d55..8bef9801f 100644 --- a/docs/Board - Sparky.md +++ b/docs/Board - Sparky.md @@ -17,7 +17,6 @@ Flyable! Tested with revision 1 board. ## TODO -* Baro - detection works but sending bad readings, disabled for now. * ADC * Sonar * Display (via Flex port) From 973efdd5fcd6527a878a6ed216425cffc95f0132 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 2 Feb 2015 00:31:35 +0000 Subject: [PATCH 25/36] Temporary workaround for CC3D non-functional VCP when using OpenPilot bootloader that some users have reported. This allows MSP connection via USART so the board can be reconfigured. --- src/main/config/config.c | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 7250c45a2..83d43605d 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -234,15 +234,26 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig) batteryConfig->currentMeterType = CURRENT_SENSOR_ADC; } +#ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS +#define FIRST_PORT_INDEX 1 +#define SECOND_PORT_INDEX 0 +#else +#define FIRST_PORT_INDEX 0 +#define SECOND_PORT_INDEX 1 +#endif + void resetSerialConfig(serialConfig_t *serialConfig) { -#ifdef SWAP_SERIAL_PORT_1_AND_2_DEFAULTS - serialConfig->serial_port_scenario[0] = lookupScenarioIndex(SCENARIO_UNUSED); - serialConfig->serial_port_scenario[1] = lookupScenarioIndex(SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH); + serialConfig->serial_port_scenario[FIRST_PORT_INDEX] = lookupScenarioIndex(SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH); + +#ifdef CC3D + // Temporary workaround for CC3D non-functional VCP when using OpenPilot bootloader. + // This allows MSP connection via USART so the board can be reconfigured. + serialConfig->serial_port_scenario[SECOND_PORT_INDEX] = lookupScenarioIndex(SCENARIO_MSP_ONLY); #else - serialConfig->serial_port_scenario[0] = lookupScenarioIndex(SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH); - serialConfig->serial_port_scenario[1] = lookupScenarioIndex(SCENARIO_UNUSED); + serialConfig->serial_port_scenario[SECOND_PORT_INDEX] = lookupScenarioIndex(SCENARIO_UNUSED); #endif + #if (SERIAL_PORT_COUNT > 2) serialConfig->serial_port_scenario[2] = lookupScenarioIndex(SCENARIO_UNUSED); #if (SERIAL_PORT_COUNT > 3) From 395a1bb9a3ea4b437db7c8c1f7acc8f2b5a240fe Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 2 Feb 2015 22:05:30 +0000 Subject: [PATCH 26/36] Update cli version command to show the version number now that there is one available. --- src/main/io/serial_cli.c | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 8fa43b5c5..22911ca28 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1393,7 +1393,15 @@ static void cliVersion(char *cmdline) { UNUSED(cmdline); - printf("Cleanflight/%s %s / %s (%s)", targetName, buildDate, buildTime, shortGitRevision); + printf("Cleanflight/%s %d.%d.%d %s / %s (%s)", + targetName, + FC_VERSION_MAJOR, + FC_VERSION_MINOR, + FC_VERSION_PATCH_LEVEL, + buildDate, + buildTime, + shortGitRevision + ); } void cliProcess(void) From 6de42dd0de19cd19069260176d061f3f94ec9531 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Mon, 2 Feb 2015 22:30:59 +0000 Subject: [PATCH 27/36] Display version on the OLED display. --- src/main/io/display.c | 8 +++++--- src/main/io/serial_cli.c | 6 ++---- src/main/version.h | 4 ++++ 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/main/io/display.c b/src/main/io/display.c index 3da875005..849560f8d 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -240,12 +240,14 @@ void showRxPage(void) void showWelcomePage(void) { - tfp_sprintf(lineBuffer, "Rev: %s", shortGitRevision); - i2c_OLED_set_line(PAGE_TITLE_LINE_COUNT + 0); + uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; + + tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision); + i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); tfp_sprintf(lineBuffer, "Target: %s", targetName); - i2c_OLED_set_line(PAGE_TITLE_LINE_COUNT + 1); + i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 22911ca28..276c9310a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1393,11 +1393,9 @@ static void cliVersion(char *cmdline) { UNUSED(cmdline); - printf("Cleanflight/%s %d.%d.%d %s / %s (%s)", + printf("Cleanflight/%s %s %s / %s (%s)", targetName, - FC_VERSION_MAJOR, - FC_VERSION_MINOR, - FC_VERSION_PATCH_LEVEL, + FC_VERSION_STRING, buildDate, buildTime, shortGitRevision diff --git a/src/main/version.h b/src/main/version.h index cb3706b0c..0c8c645ae 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -19,6 +19,10 @@ #define FC_VERSION_MINOR 7 // 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 +#define STR(x) STR_HELPER(x) +#define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL) + #define MW_VERSION 231 extern char* targetName; From 8aee0b25e20fa78586a66f1a9904c2922e31d791 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 3 Feb 2015 16:20:14 +0000 Subject: [PATCH 28/36] Fix bug where current meter didn't work unless VBAT feature was enabled. --- src/main/main.c | 3 +-- src/main/version.h | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index ba7453265..549cd9058 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -387,8 +387,7 @@ void init(void) // Now that everything has powered up the voltage and cell count be determined. - // Check battery type/voltage - if (feature(FEATURE_VBAT)) + if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) batteryInit(&masterConfig.batteryConfig); #ifdef DISPLAY diff --git a/src/main/version.h b/src/main/version.h index 0c8c645ae..faf7da75e 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -17,7 +17,7 @@ #define FC_VERSION_MAJOR 1 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 7 // 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 430ccd233869f1409cf9a814c15f1af7dd02e2e5 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 4 Feb 2015 19:12:13 +0000 Subject: [PATCH 29/36] STM32F3DISCOVERY/CHEBUZZF3 - Move ADC pin definitions to target.h files --- src/main/drivers/adc.c | 4 ++- src/main/drivers/adc_stm32f10x.c | 2 +- src/main/drivers/adc_stm32f30x.c | 31 ++++++++++++++++------- src/main/target/CHEBUZZF3/target.h | 17 +++++++++++++ src/main/target/STM32F3DISCOVERY/target.h | 16 ++++++++++++ 5 files changed, 59 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index eae243340..958a831b4 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -25,6 +25,8 @@ #include "adc.h" +//#define DEBUG_ADC_CHANNELS + #ifdef USE_ADC adc_config_t adcConfig[ADC_CHANNEL_COUNT]; volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; @@ -33,7 +35,7 @@ extern int16_t debug[4]; uint16_t adcGetChannel(uint8_t channel) { -#if DEBUG_ADC_CHANNELS +#ifdef DEBUG_ADC_CHANNELS if (adcConfig[0].enabled) { debug[0] = adcValues[adcConfig[0].dmaIndex]; } diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 5f591ed0a..c6c97c2f4 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -64,7 +64,7 @@ void adcInit(drv_adc_config_t *init) GPIO_StructInit(&GPIO_InitStructure); GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; -#ifdef VBAT_ADC_GPIO_PIN +#ifdef VBAT_ADC_GPIO GPIO_InitStructure.GPIO_Pin = VBAT_ADC_GPIO_PIN; GPIO_Init(VBAT_ADC_GPIO, &GPIO_InitStructure); adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL; diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index 6c129d546..ecaa1c000 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -44,42 +44,56 @@ void adcInit(drv_adc_config_t *init) memset(&adcConfig, 0, sizeof(adcConfig)); GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_3; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ; - adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_6; +#ifdef VBAT_ADC_GPIO + GPIO_InitStructure.GPIO_Pin = VBAT_ADC_GPIO_PIN; + GPIO_Init(VBAT_ADC_GPIO, &GPIO_InitStructure); + + adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL; adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount; adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_BATTERY].enabled = true; adcChannelCount++; +#endif +#ifdef CURRENT_METER_ADC_GPIO if (init->enableCurrentMeter) { - GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_1; + GPIO_InitStructure.GPIO_Pin = CURRENT_METER_ADC_GPIO_PIN; + GPIO_Init(CURRENT_METER_ADC_GPIO, &GPIO_InitStructure); - adcConfig[ADC_CURRENT].adcChannel = ADC_Channel_7; + adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL; adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount; adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_CURRENT].enabled = true; adcChannelCount++; - } +#endif +#ifdef RSSI_ADC_GPIO if (init->enableRSSI) { - GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_2; + GPIO_InitStructure.GPIO_Pin = RSSI_ADC_GPIO_PIN; + GPIO_Init(RSSI_ADC_GPIO, &GPIO_InitStructure); - adcConfig[ADC_RSSI].adcChannel = ADC_Channel_8; + adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL; adcConfig[ADC_RSSI].dmaIndex = adcChannelCount; adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_RSSI].enabled = true; adcChannelCount++; } +#endif - adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_9; +#ifdef EXTERNAL1_ADC_GPIO + GPIO_InitStructure.GPIO_Pin = EXTERNAL1_ADC_GPIO_PIN; + GPIO_Init(EXTERNAL1_ADC_GPIO, &GPIO_InitStructure); + + adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL; adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount; adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_601Cycles5; adcConfig[ADC_EXTERNAL1].enabled = true; adcChannelCount++; +#endif RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1 | RCC_AHBPeriph_ADC12, ENABLE); @@ -103,7 +117,6 @@ void adcInit(drv_adc_config_t *init) DMA_Cmd(DMA1_Channel1, ENABLE); - GPIO_Init(GPIOC, &GPIO_InitStructure); // calibrate diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index cc699343c..c5f896f3c 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -70,6 +70,23 @@ #define USE_ADC +#define VBAT_ADC_GPIO GPIOC +#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_CHANNEL ADC_Channel_6 + +#define CURRENT_METER_ADC_GPIO GPIOC +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 + +#define RSSI_ADC_GPIO GPIOC +#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_CHANNEL ADC_Channel_8 + +#define EXTERNAL1_ADC_GPIO GPIOC +#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3 +#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 + + #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG) #define GPS diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index ab840213f..2990341ea 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -61,6 +61,22 @@ #define USE_ADC +#define VBAT_ADC_GPIO GPIOC +#define VBAT_ADC_GPIO_PIN GPIO_Pin_0 +#define VBAT_ADC_CHANNEL ADC_Channel_6 + +#define CURRENT_METER_ADC_GPIO GPIOC +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7 + +#define RSSI_ADC_GPIO GPIOC +#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_CHANNEL ADC_Channel_8 + +#define EXTERNAL1_ADC_GPIO GPIOC +#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3 +#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9 + #define SENSORS_SET (SENSOR_ACC | SENSOR_MAG) #define BLACKBOX From fdb81ed3b96df93844d97fdb3970cff8c27262f7 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 4 Feb 2015 21:48:14 +0000 Subject: [PATCH 30/36] SPRACINGF3 - Enable ADC for VBAT, Current and RSSI. --- src/main/drivers/adc_stm32f30x.c | 42 +++++++++++++---------- src/main/target/CHEBUZZF3/target.h | 4 +++ src/main/target/SPRACINGF3/target.h | 22 ++++++++++++ src/main/target/STM32F3DISCOVERY/target.h | 4 +++ 4 files changed, 54 insertions(+), 18 deletions(-) diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index ecaa1c000..c18b37417 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -32,6 +32,12 @@ extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; +#ifndef ADC_INSTANCE +#define ADC_INSTANCE ADC1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 +#define ADC_DMA_CHANNEL DMA1_Channel1 +#endif + void adcInit(drv_adc_config_t *init) { ADC_InitTypeDef ADC_InitStructure; @@ -96,12 +102,12 @@ void adcInit(drv_adc_config_t *init) #endif RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1 | RCC_AHBPeriph_ADC12, ENABLE); + RCC_AHBPeriphClockCmd(ADC_AHB_PERIPHERAL | RCC_AHBPeriph_ADC12, ENABLE); - DMA_DeInit(DMA1_Channel1); + DMA_DeInit(ADC_DMA_CHANNEL); DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC_INSTANCE->DR; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)adcValues; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; DMA_InitStructure.DMA_BufferSize = adcChannelCount; @@ -113,19 +119,19 @@ void adcInit(drv_adc_config_t *init) DMA_InitStructure.DMA_Priority = DMA_Priority_High; DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; - DMA_Init(DMA1_Channel1, &DMA_InitStructure); + DMA_Init(ADC_DMA_CHANNEL, &DMA_InitStructure); - DMA_Cmd(DMA1_Channel1, ENABLE); + DMA_Cmd(ADC_DMA_CHANNEL, ENABLE); // calibrate - ADC_VoltageRegulatorCmd(ADC1, ENABLE); + ADC_VoltageRegulatorCmd(ADC_INSTANCE, ENABLE); delay(10); - ADC_SelectCalibrationMode(ADC1, ADC_CalibrationMode_Single); - ADC_StartCalibration(ADC1); - while(ADC_GetCalibrationStatus(ADC1) != RESET); - ADC_VoltageRegulatorCmd(ADC1, DISABLE); + ADC_SelectCalibrationMode(ADC_INSTANCE, ADC_CalibrationMode_Single); + ADC_StartCalibration(ADC_INSTANCE); + while(ADC_GetCalibrationStatus(ADC_INSTANCE) != RESET); + ADC_VoltageRegulatorCmd(ADC_INSTANCE, DISABLE); ADC_CommonInitTypeDef ADC_CommonInitStructure; @@ -136,7 +142,7 @@ void adcInit(drv_adc_config_t *init) ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1; ADC_CommonInitStructure.ADC_DMAMode = ADC_DMAMode_Circular; ADC_CommonInitStructure.ADC_TwoSamplingDelay = 0; - ADC_CommonInit(ADC1, &ADC_CommonInitStructure); + ADC_CommonInit(ADC_INSTANCE, &ADC_CommonInitStructure); ADC_StructInit(&ADC_InitStructure); @@ -149,24 +155,24 @@ void adcInit(drv_adc_config_t *init) ADC_InitStructure.ADC_AutoInjMode = ADC_AutoInjec_Disable; ADC_InitStructure.ADC_NbrOfRegChannel = adcChannelCount; - ADC_Init(ADC1, &ADC_InitStructure); + ADC_Init(ADC_INSTANCE, &ADC_InitStructure); uint8_t rank = 1; for (i = 0; i < ADC_CHANNEL_COUNT; i++) { if (!adcConfig[i].enabled) { continue; } - ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime); + ADC_RegularChannelConfig(ADC_INSTANCE, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime); } - ADC_Cmd(ADC1, ENABLE); + ADC_Cmd(ADC_INSTANCE, ENABLE); - while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_RDY)); + while(!ADC_GetFlagStatus(ADC_INSTANCE, ADC_FLAG_RDY)); - ADC_DMAConfig(ADC1, ADC_DMAMode_Circular); + ADC_DMAConfig(ADC_INSTANCE, ADC_DMAMode_Circular); - ADC_DMACmd(ADC1, ENABLE); + ADC_DMACmd(ADC_INSTANCE, ENABLE); - ADC_StartConversion(ADC1); + ADC_StartConversion(ADC_INSTANCE); } diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index c5f896f3c..e27951b9f 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -70,6 +70,10 @@ #define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 +#define ADC_DMA_CHANNEL DMA1_Channel1 + #define VBAT_ADC_GPIO GPIOC #define VBAT_ADC_GPIO_PIN GPIO_Pin_0 #define VBAT_ADC_CHANNEL ADC_Channel_6 diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index af35c8a18..4fd50c96b 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -62,6 +62,28 @@ //#define USE_SPI //#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 +#define USE_ADC + +#define ADC_INSTANCE ADC2 +#define ADC_DMA_CHANNEL DMA2_Channel1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2 + +#define VBAT_ADC_GPIO GPIOA +#define VBAT_ADC_GPIO_PIN GPIO_Pin_4 +#define VBAT_ADC_CHANNEL ADC_Channel_1 + +#define CURRENT_METER_ADC_GPIO GPIOA +#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5 +#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 + +#define RSSI_ADC_GPIO GPIOB +#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_CHANNEL ADC_Channel_12 + +#define RSSI_ADC_GPIO GPIOB +#define RSSI_ADC_GPIO_PIN GPIO_Pin_2 +#define RSSI_ADC_CHANNEL ADC_Channel_12 + #define LED_STRIP #define LED_STRIP_TIMER TIM1 diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 2990341ea..29f5f39e8 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -61,6 +61,10 @@ #define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1 +#define ADC_DMA_CHANNEL DMA1_Channel1 + #define VBAT_ADC_GPIO GPIOC #define VBAT_ADC_GPIO_PIN GPIO_Pin_0 #define VBAT_ADC_CHANNEL ADC_Channel_6 From ed51428989d8bdc28599523a619be629819545f6 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 5 Feb 2015 02:08:37 +0000 Subject: [PATCH 31/36] SPRACINGF3 - Enable DISPLAY. Specify serial port I/O in target.h. --- Makefile | 1 + src/main/target/SPRACINGF3/target.h | 20 +++++++++++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/Makefile b/Makefile index fa84772d6..742235d82 100644 --- a/Makefile +++ b/Makefile @@ -498,6 +498,7 @@ SPRACINGF3_SRC = \ drivers/accgyro_mpu6050.c \ drivers/barometer_ms5611.c \ drivers/compass_hmc5883l.c \ + drivers/display_ug2864hsweg01.h \ $(HIGHEND_SRC) \ $(COMMON_SRC) diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 4fd50c96b..7e63241e4 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -49,6 +49,15 @@ #define USE_USART3 #define SERIAL_PORT_COUNT 3 +#ifndef UART1_GPIO +#define UART1_TX_PIN GPIO_Pin_9 // PA9 +#define UART1_RX_PIN GPIO_Pin_10 // PA10 +#define UART1_GPIO GPIOA +#define UART1_GPIO_AF GPIO_AF_7 +#define UART1_TX_PINSOURCE GPIO_PinSource9 +#define UART1_RX_PINSOURCE GPIO_PinSource10 +#endif + #define UART2_TX_PIN GPIO_Pin_14 // PA14 / SWCLK #define UART2_RX_PIN GPIO_Pin_15 // PA15 #define UART2_GPIO GPIOA @@ -56,6 +65,15 @@ #define UART2_TX_PINSOURCE GPIO_PinSource14 #define UART2_RX_PINSOURCE GPIO_PinSource15 +#ifndef UART3_GPIO +#define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7) +#define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7) +#define UART2_GPIO_AF GPIO_AF_7 +#define UART3_GPIO GPIOB +#define UART3_TX_PINSOURCE GPIO_PinSource10 +#define UART3_RX_PINSOURCE GPIO_PinSource11 +#endif + #define USE_I2C #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA @@ -105,4 +123,4 @@ #define TELEMETRY #define SERIAL_RX #define AUTOTUNE - +#define DISPLAY From 012e8fac31a2ac95dcbd83dc7d3c28645baa4c2a Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 5 Feb 2015 17:02:14 +0000 Subject: [PATCH 32/36] SPRACINGF3 - Configure beeper GPIO correctly. --- src/main/main.c | 8 +++++++- src/main/target/SPRACINGF3/target.h | 1 + 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/main.c b/src/main/main.c index 549cd9058..b3f01f9e6 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -180,11 +180,16 @@ void init(void) #ifdef BEEPER beeperConfig_t beeperConfig = { - .gpioMode = Mode_Out_OD, .gpioPin = BEEP_PIN, .gpioPort = BEEP_GPIO, .gpioPeripheral = BEEP_PERIPHERAL, +#ifdef BEEPER_INVERTED + .gpioMode = Mode_Out_PP, + .isInverted = true +#else + .gpioMode = Mode_Out_OD, .isInverted = false +#endif }; #ifdef NAZE if (hardwareRevision >= NAZE32_REV5) { @@ -273,6 +278,7 @@ void init(void) LED0_OFF; LED1_OFF; + imuInit(); mixerInit(masterConfig.mixerMode, masterConfig.customMixer); diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 7e63241e4..d50d9ce2e 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -26,6 +26,7 @@ #define BEEP_GPIO GPIOC #define BEEP_PIN Pin_15 #define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC +#define BEEPER_INVERTED #define USABLE_TIMER_CHANNEL_COUNT 17 From b4b16fa28c3ca909696965aa88b3fb36a823f8e2 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 5 Feb 2015 17:49:32 +0000 Subject: [PATCH 33/36] SPRACINGF3 - Set sensor alignments. --- src/main/target/SPRACINGF3/target.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index d50d9ce2e..5f4ce6693 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -32,15 +32,18 @@ #define GYRO #define USE_GYRO_MPU6050 +#define GYRO_MPU6050_ALIGN CW270_DEG #define ACC #define USE_ACC_MPU6050 +#define ACC_MPU6050_ALIGN CW270_DEG #define BARO #define USE_BARO_MS5611 #define MAG #define USE_MAG_HMC5883 +#define MAG_HMC5883_ALIGN CW270_DEG #define BEEPER #define LED0 From 3272b44410a3aed50dfc1a1261e05b20c95a8acf Mon Sep 17 00:00:00 2001 From: tricopterY Date: Thu, 12 Feb 2015 00:10:57 +1100 Subject: [PATCH 34/36] Update inverter.c Fixes #494 --- src/main/drivers/inverter.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/inverter.c b/src/main/drivers/inverter.c index b8f014c8f..000597087 100644 --- a/src/main/drivers/inverter.c +++ b/src/main/drivers/inverter.c @@ -33,7 +33,8 @@ void initInverter(void) gpio_config_t cfg; } gpio_setup = { .gpio = INVERTER_GPIO, - .cfg = { INVERTER_PIN, Mode_Out_OD, Speed_2MHz } + // configure for Push-Pull + .cfg = { INVERTER_PIN, Mode_Out_PP, Speed_2MHz } }; RCC_APB2PeriphClockCmd(INVERTER_PERIPHERAL, ENABLE); From 519cc5f238a0dda4bd2a09bb3a0ecec8a3342363 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 11 Feb 2015 19:44:00 +0000 Subject: [PATCH 35/36] CC3D - Updating docs regarding inverter. --- docs/Board - CC3D.md | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/docs/Board - CC3D.md b/docs/Board - CC3D.md index 0f725e533..e672fee61 100644 --- a/docs/Board - CC3D.md +++ b/docs/Board - CC3D.md @@ -66,16 +66,24 @@ The 6 pin RC_Output connector has the following pinouts when used in RX_PARALLEL # Serial Ports -| Value | Identifier | Board Markings | Notes | -| ----- | ------------ | -------------- | -----------------------------------------| -| 1 | VCP | USB PORT | | -| 2 | USART1 | MAIN PORT | Has a hardware inverter for S.BUS | -| 3 | USART3 | FLEX PORT | | -| 4 | SoftSerial | RC connector | Pins 4 and 5 (Tx and Rx respectively) | +| Value | Identifier | Board Markings | Notes | +| ----- | ------------ | -------------- | ------------------------------------------| +| 1 | VCP | USB PORT | | +| 2 | USART1 | MAIN PORT | Connected to an MCU controllable inverter | +| 3 | USART3 | FLEX PORT | | +| 4 | SoftSerial | RC connector | Pins 4 and 5 (Tx and Rx respectively) | The SoftSerial port is not available when RX_PARALLEL_PWM is used. The transmission data rate is limited to 19200 baud. -To connect the GUI to the flight controller you just need a USB cable to use the Virtual Com Port (VCP). +To connect the GUI to the flight controller you just need a USB cable to use the Virtual Com Port (VCP) or you can use UART1 (Main Port). + +CLI access is only available via the VCP by default. + +# Main Port + +The main port has MSP support enabled on it by default. + +The main port is connected to an inverter which is automatically enabled as required. For example, if the main port is used for SBus Serial RX then an external inverter is not required. # Flex Port From d12d1952ebcb8c59de71ba8ad080448baea94ff6 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 12 Feb 2015 01:58:59 +0000 Subject: [PATCH 36/36] Fix PWM/UART2 clash on F1 targets caused by using wrong #define value. --- src/main/main.c | 2 +- src/main/version.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index b3f01f9e6..d99a40fdc 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -295,7 +295,7 @@ void init(void) pwm_params.airplane = true; else pwm_params.airplane = false; -#if defined(SERIAL_PORT_USART2) && defined(STM32F10X) +#if defined(USE_USART2) && defined(STM32F10X) pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); #endif pwm_params.useVbat = feature(FEATURE_VBAT); diff --git a/src/main/version.h b/src/main/version.h index faf7da75e..eb315aaae 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -17,7 +17,7 @@ #define FC_VERSION_MAJOR 1 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 7 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x)