diff --git a/src/main/config/config.c b/src/main/config/config.c index 7f2b45743e..4c6e124f54 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -152,7 +152,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[PITCH] = 40; pidProfile->I8[PITCH] = 30; pidProfile->D8[PITCH] = 18; - pidProfile->P8[YAW] = 95; + pidProfile->P8[YAW] = 100; pidProfile->I8[YAW] = 50; pidProfile->D8[YAW] = 10; pidProfile->P8[PIDALT] = 50; @@ -167,8 +167,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10; pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100; pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000; - pidProfile->P8[PIDLEVEL] = 20; - pidProfile->I8[PIDLEVEL] = 20; + pidProfile->P8[PIDLEVEL] = 50; + pidProfile->I8[PIDLEVEL] = 50; pidProfile->D8[PIDLEVEL] = 100; pidProfile->P8[PIDMAG] = 40; pidProfile->P8[PIDVEL] = 120; diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c index 6cc0b9ae99..1400e00148 100644 --- a/src/main/drivers/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro_l3gd20.c @@ -86,7 +86,7 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx) spiSetDivisor(L3GD20_SPI, SPI_9MHZ_CLOCK_DIVIDER); } -void l3gd20GyroInit(uint16_t lpf) +void l3gd20GyroInit(uint8_t lpf) { UNUSED(lpf); // FIXME use it! diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 6f9e32fc34..63e4201bdc 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -359,6 +359,11 @@ static const char * const lookupTableGyroSampling[] = { "1KHZ" }; +static const char * const lookupTablePidAtMinThrottle[] = { + "OFF", + "PD", + "PID" +}; typedef struct lookupTableEntry_s { const char * const *values; @@ -379,6 +384,7 @@ typedef enum { TABLE_SERIAL_RX, TABLE_GYRO_FILTER, TABLE_GYRO_SAMPLING, + TABLE_PID_AT_MIN_THROTTLE, } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -394,7 +400,8 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) }, { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, { lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) }, - { lookupTableGyroSampling, sizeof(lookupTableGyroSampling) / sizeof(char *) } + { lookupTableGyroSampling, sizeof(lookupTableGyroSampling) / sizeof(char *) }, + { lookupTablePidAtMinThrottle, sizeof(lookupTablePidAtMinThrottle) / sizeof(char *) } }; #define VALUE_TYPE_OFFSET 0 @@ -501,6 +508,17 @@ const clivalue_t valueTable[] = { { "nav_speed_max", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].gpsProfile.nav_speed_max, .config.minmax = { 10, 2000 } }, { "nav_slew_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gpsProfile.nav_slew_rate, .config.minmax = { 0, 100 } }, #endif +#ifdef GTUNE + { "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10, 200 } }, + { "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], .config.minmax = { 10, 200 } }, + { "gtune_loP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_YAW], .config.minmax = { 10, 200 } }, + { "gtune_hiP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], .config.minmax = { 0, 200 } }, + { "gtune_hiP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], .config.minmax = { 0, 200 } }, + { "gtune_hiP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], .config.minmax = { 0, 200 } }, + { "gtune_pwr", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_pwr, .config.minmax = { 0, 10 } }, + { "gtune_settle_time", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_settle_time, .config.minmax = { 200, 1000 } }, + { "gtune_average_cycles", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_average_cycles, .config.minmax = { 8, 128 } }, +#endif { "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.serialrx_provider, .config.lookup = { TABLE_SERIAL_RX } }, { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX} }, @@ -549,7 +567,7 @@ const clivalue_t valueTable[] = { { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, .config.minmax = { -1, 1 } }, - { "pid_at_min_throttle", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.pid_at_min_throttle, .config.minmax = { 0, 5 } }, + { "pid_at_min_throttle", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.pid_at_min_throttle, .config.lookup = { TABLE_PID_AT_MIN_THROTTLE } }, { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 } }, { "yaw_jump_prevention_limit", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH } }, #ifdef USE_SERVOS @@ -643,18 +661,6 @@ const clivalue_t valueTable[] = { { "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } }, { "yaw_pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz, .config.minmax = {0, 200 } }, -#ifdef GTUNE - { "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10, 200 } }, - { "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], .config.minmax = { 10, 200 } }, - { "gtune_loP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_YAW], .config.minmax = { 10, 200 } }, - { "gtune_hiP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], .config.minmax = { 0, 200 } }, - { "gtune_hiP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], .config.minmax = { 0, 200 } }, - { "gtune_hiP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], .config.minmax = { 0, 200 } }, - { "gtune_pwr", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_pwr, .config.minmax = { 0, 10 } }, - { "gtune_settle_time", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_settle_time, .config.minmax = { 200, 1000 } }, - { "gtune_average_cycles", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_average_cycles, .config.minmax = { 8, 128 } }, -#endif - #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 } }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 } }, diff --git a/src/main/mw.c b/src/main/mw.c index 4ccfada9bc..db607b9683 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -100,6 +100,7 @@ enum { #define PREVENT_RX_PROCESS_PRE_LOOP_TRIGGER 80 // Prevent RX processing before expected loop trigger #define PREVENT_BARO_READ_PRE_LOOP_TRIGGER 150 // Prevent BARO processing before expected loop trigger #define GYRO_RATE 0.001f // Gyro refresh rate 1khz +#define PID_AT_MIN_THROTTLE_ITERM_DELAY (3 * 1000) uint32_t currentTime = 0; uint32_t previousTime = 0; @@ -578,7 +579,7 @@ void processRx(void) pidResetErrorGyro(); } } else { - pidResetErrorGyroTimeout = millis() + (masterConfig.mixerConfig.pid_at_min_throttle * 1000); + pidResetErrorGyroTimeout = millis() + PID_AT_MIN_THROTTLE_ITERM_DELAY; } // When armed and motors aren't spinning, do beeps and then disarm diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 8f5da398d2..75e9fdd7cd 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -161,14 +161,14 @@ static void sbusDataReceive(uint16_t c) if (sbusFramePosition < SBUS_FRAME_SIZE) { sbusFrame.bytes[sbusFramePosition++] = (uint8_t)c; - } - if (sbusFramePosition < SBUS_FRAME_SIZE) { - sbusFrameDone = false; - } else { - sbusFrameDone = true; + if (sbusFramePosition < SBUS_FRAME_SIZE) { + sbusFrameDone = false; + } else { + sbusFrameDone = true; #ifdef DEBUG_SBUS_PACKETS debug[2] = sbusFrameTime; #endif + } } } diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 05c0ad91f2..5457607d6b 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -106,11 +106,13 @@ static void spektrumDataReceive(uint16_t c) spekFramePosition = 0; } - spekFrame[spekFramePosition] = (uint8_t)c; - if (spekFramePosition == SPEK_FRAME_SIZE - 1) { - rcFrameComplete = true; - } else { - spekFramePosition++; + if (spekFramePosition < SPEK_FRAME_SIZE) { + spekFrame[spekFramePosition++] = (uint8_t)c; + if (spekFramePosition < SPEK_FRAME_SIZE) { + rcFrameComplete = false; + } else { + rcFrameComplete = true; + } } }