rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add delta from measurement // rc smooth interval // More MSP

This commit is contained in:
borisbstyle 2016-06-22 01:20:15 +02:00
parent 9160a2adbc
commit 4c59769b02
9 changed files with 79 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View File

@ -1226,7 +1226,7 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.mag_hardware);
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm);
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation);
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing);
BLACKBOX_PRINT_HEADER_LINE("rc_smooth_interval:%d", masterConfig.rxConfig.rcSmoothInterval);
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold);
BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures);

9
src/main/config/config.c
View File

@ -145,7 +145,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 140;
static const uint8_t EEPROM_CONF_VERSION = 141;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -158,9 +158,9 @@ static void resetPidProfile(pidProfile_t *pidProfile)
{
#if (defined(STM32F10X))
pidProfile->pidController = 1;
pidProfile->pidController = PID_CONTROLLER_INTEGER;
#else
pidProfile->pidController = 2;
pidProfile->pidController = PID_CONTROLLER_FLOAT;
#endif
pidProfile->P8[ROLL] = 45;
@ -197,6 +197,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->rollPitchItermIgnoreRate = 200;
pidProfile->yawItermIgnoreRate = 35;
pidProfile->dterm_lpf_hz = 50; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_ERROR;
pidProfile->dynamic_pid = 1;
#ifdef GTUNE
@ -463,7 +464,7 @@ static void resetConf(void)
masterConfig.rxConfig.rssi_channel = 0;
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
masterConfig.rxConfig.rssi_ppm_invert = 0;
masterConfig.rxConfig.rcSmoothing = 0; // TODO - Cleanup with next EEPROM changes
masterConfig.rxConfig.rcSmoothInterval = 0; // 0 is predefined
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
masterConfig.rxConfig.max_aux_channel = 6;
masterConfig.rxConfig.airModeActivateThreshold = 1350;

60
src/main/flight/pid.c
View File

@ -67,10 +67,10 @@ static int32_t errorGyroI[3];
static float errorGyroIf[3];
#endif
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii
pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we using
void setTargetPidLooptime(uint8_t pidProcessDenom) {
targetPidLooptime = targetLooptime * pidProcessDenom;
@ -114,7 +114,7 @@ static filterStatePt1_t deltaFilterState[3];
static filterStatePt1_t yawFilterState;
#ifndef SKIP_PID_LUXFLOAT
static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
float RateError, gyroRate, RateErrorSmooth;
@ -126,7 +126,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli
float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
// Scaling factors for Pids to match rewrite and use same defaults
// Scaling factors for Pids for better tunable range in configurator
static const float luxPTermScale = 1.0f / 128;
static const float luxITermScale = 1000000.0f / 0x1000000;
static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 508;
@ -168,7 +168,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli
}
}
gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled to old rewrite scale
gyroRate = gyroADCf[axis] / 4.0f; // gyro output scaled for easier int conversion
// --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
@ -176,11 +176,12 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = angleRate[axis] - gyroRate;
// Smoothed Error for Derivative
if (flightModeFlags && axis != YAW) {
RateErrorSmooth = RateError;
} else {
RateErrorSmooth = angleRateSmooth[axis] - gyroRate;
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
// Smoothed Error for Derivative when delta from error selected
if (flightModeFlags && axis != YAW)
RateErrorSmooth = RateError;
else
RateErrorSmooth = angleRateSmooth[axis] - gyroRate;
}
// -----calculate P component
@ -215,8 +216,13 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli
DTerm = 0.0f; // needed for blackbox
} else {
delta = RateErrorSmooth - lastRateError[axis];
lastRateError[axis] = RateErrorSmooth;
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
delta = RateErrorSmooth - lastRateError[axis];
lastRateError[axis] = RateErrorSmooth;
} else {
delta = -(gyroRate - lastRateError[axis]);
lastRateError[axis] = gyroRate;
}
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta *= (1.0f / getdT());
@ -245,7 +251,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, uint16_t max_angle_incli
}
#endif
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
{
int axis;
@ -301,11 +307,12 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angl
RateError = AngleRateTmp - gyroRate;
// Smoothed Error for Derivative
if (flightModeFlags && axis != YAW) {
RateErrorSmooth = RateError;
} else {
RateErrorSmooth = AngleRateTmpSmooth - gyroRate;
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
// Smoothed Error for Derivative when delta from error selected
if (flightModeFlags && axis != YAW)
RateErrorSmooth = RateError;
else
RateErrorSmooth = AngleRateTmpSmooth - gyroRate;
}
// -----calculate P component
@ -348,8 +355,13 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, uint16_t max_angl
DTerm = 0; // needed for blackbox
} else {
delta = RateErrorSmooth - lastRateError[axis];
lastRateError[axis] = RateErrorSmooth;
if (pidProfile->deltaMethod == DELTA_FROM_ERROR) {
delta = RateErrorSmooth - lastRateError[axis];
lastRateError[axis] = RateErrorSmooth;
} else {
delta = -(gyroRate - lastRateError[axis]);
lastRateError[axis] = gyroRate;
}
// Divide delta by targetLooptime to get differential (ie dr/dt)
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;
@ -382,12 +394,12 @@ void pidSetController(pidControllerType_e type)
{
switch (type) {
default:
case PID_CONTROLLER_MWREWRITE:
pid_controller = pidMultiWiiRewrite;
case PID_CONTROLLER_INTEGER:
pid_controller = pidInteger;
break;
#ifndef SKIP_PID_LUXFLOAT
case PID_CONTROLLER_LUX_FLOAT:
pid_controller = pidLuxFloat;
case PID_CONTROLLER_FLOAT:
pid_controller = pidFloat;
#endif
}
}

9
src/main/flight/pid.h
View File

@ -41,14 +41,14 @@ typedef enum {
} pidIndex_e;
typedef enum {
PID_CONTROLLER_MWREWRITE = 1,
PID_CONTROLLER_LUX_FLOAT,
PID_CONTROLLER_INTEGER = 1, // Integer math to gain some more performance from F1 targets
PID_CONTROLLER_FLOAT,
PID_COUNT
} pidControllerType_e;
typedef enum {
DELTA_FROM_ERROR = 0,
DELTA_FROM_MEASUREMENT
DELTA_FROM_ERROR = 0,
DELTA_FROM_MEASUREMENT
} pidDeltaType_e;
typedef enum {
@ -68,6 +68,7 @@ typedef struct pidProfile_s {
uint16_t dterm_lpf_hz; // Delta Filter in hz
uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy
uint8_t deltaMethod; // Alternative delta Calculation
uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates
uint16_t yaw_p_limit;

14
src/main/io/serial_cli.c
View File

@ -381,7 +381,7 @@ static const char * const lookupTableBlackboxDevice[] = {
static const char * const lookupTablePidController[] = {
"UNUSED", "MWREWRITE", "LUX"
"UNUSED", "INT", "FLOAT"
};
static const char * const lookupTableSerialRX[] = {
@ -455,6 +455,10 @@ static const char * const lookupTableFastPwm[] = {
"OFF", "ONESHOT125", "ONESHOT42", "MULTISHOT"
};
static const char * const lookupDeltaMethod[] = {
"ERROR", "MEASUREMENT"
};
typedef struct lookupTableEntry_s {
const char * const *values;
const uint8_t valueCount;
@ -479,9 +483,10 @@ typedef enum {
TABLE_ACC_HARDWARE,
TABLE_BARO_HARDWARE,
TABLE_MAG_HARDWARE,
TABLE_DEBUG,
TABLE_DEBUG,
TABLE_SUPEREXPO_YAW,
TABLE_FAST_PWM,
TABLE_DELTA_METHOD,
} lookupTableIndex_e;
static const lookupTableEntry_t lookupTables[] = {
@ -506,6 +511,7 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) },
{ lookupTableSuperExpoYaw, sizeof(lookupTableSuperExpoYaw) / sizeof(char *) },
{ lookupTableFastPwm, sizeof(lookupTableFastPwm) / sizeof(char *) },
{ lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) }
};
#define VALUE_TYPE_OFFSET 0
@ -558,12 +564,12 @@ typedef struct {
const clivalue_t valueTable[] = {
// { "emf_avoidance", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.emf_avoidance, .config.lookup = { TABLE_OFF_ON } },
{ "mid_rc", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.midrc, .config.minmax = { 1200, 1700 } },
{ "min_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.mincheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "max_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.maxcheck, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT } },
{ "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX } },
{ "rc_smooth_interval_ms", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rcSmoothInterval, .config.minmax = { 0, 255 } },
{ "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rssi_ppm_invert, .config.lookup = { TABLE_OFF_ON } },
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON } },
{ "roll_yaw_cam_mix_degrees", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0, 50 } },
@ -667,7 +673,7 @@ const clivalue_t valueTable[] = {
{ "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180, 360 } },
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
{ "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },

18
src/main/io/serial_msp.c
View File

@ -1259,14 +1259,20 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize16(currentProfile->pidProfile.yaw_lpf_hz);
break;
case MSP_ADVANCED_TUNING:
headSerialReply(3 * 2);
headSerialReply(4 * 2 + 2);
serialize16(currentProfile->pidProfile.rollPitchItermIgnoreRate);
serialize16(currentProfile->pidProfile.yawItermIgnoreRate);
serialize16(currentProfile->pidProfile.yaw_p_limit);
serialize16(masterConfig.rxConfig.airModeActivateThreshold);
serialize8(currentProfile->pidProfile.deltaMethod);
serialize8(masterConfig.batteryConfig.vbatPidCompensation);
break;
case MSP_TEMPORARY_COMMANDS:
headSerialReply(1);
case MSP_SPECIAL_PARAMETERS:
headSerialReply(1 + 2 + 1 + 2);
serialize8(currentControlRateProfile->rcYawRate8);
serialize16(masterConfig.rxConfig.airModeActivateThreshold);
serialize8(masterConfig.rxConfig.rcSmoothInterval);
serialize16(masterConfig.escAndServoConfig.escDesyncProtection);
break;
case MSP_SENSOR_CONFIG:
headSerialReply(3);
@ -1803,9 +1809,13 @@ static bool processInCommand(void)
currentProfile->pidProfile.rollPitchItermIgnoreRate = read16();
currentProfile->pidProfile.yawItermIgnoreRate = read16();
currentProfile->pidProfile.yaw_p_limit = read16();
currentProfile->pidProfile.deltaMethod = read8();
break;
case MSP_SET_TEMPORARY_COMMANDS:
case MSP_SET_SPECIAL_PARAMETERS:
currentControlRateProfile->rcYawRate8 = read8();
masterConfig.rxConfig.airModeActivateThreshold = read16();
masterConfig.rxConfig.rcSmoothInterval = read8();
masterConfig.escAndServoConfig.escDesyncProtection = read16();
break;
case MSP_SET_SENSOR_CONFIG:
masterConfig.acc_hardware = read8();

4
src/main/io/serial_msp.h
View File

@ -197,8 +197,8 @@ static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
#define MSP_SENSOR_CONFIG 96
#define MSP_SET_SENSOR_CONFIG 97
#define MSP_TEMPORARY_COMMANDS 98 // Temporary Commands before cleanup
#define MSP_SET_TEMPORARY_COMMANDS 99 // Temporary Commands before cleanup
#define MSP_SPECIAL_PARAMETERS 98 // Temporary betaflight parameters before cleanup and keep CF compatibility
#define MSP_SET_SPECIAL_PARAMETERS 99 // Temporary betaflight parameters before cleanup and keep CF compatibility
//
// Multwii original MSP commands

6
src/main/mw.c
View File

@ -193,7 +193,11 @@ void processRcCommand(void)
int axis;
// Set RC refresh rate for sampling and channels to filter
initRxRefreshRate(&rxRefreshRate);
if (masterConfig.rxConfig.rcSmoothInterval) {
rxRefreshRate = 1000 * masterConfig.rxConfig.rcSmoothInterval;
} else {
initRxRefreshRate(&rxRefreshRate);
}
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;

2
src/main/rx/rx.h
View File

@ -121,7 +121,7 @@ typedef struct rxConfig_s {
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end
uint8_t rcSmoothing;
uint8_t rcSmoothInterval;
uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands
uint8_t max_aux_channel;
uint16_t airModeActivateThreshold; // Throttle setpoint where airmode gets activated