got rid of feature_gyro_smoothing. nobody used that, and its retarded to do anyway.

This commit is contained in:
dongie 2014-02-18 15:27:53 +09:00
parent 202fc17608
commit 30afff2578
6 changed files with 3409 additions and 3431 deletions

File diff suppressed because it is too large Load Diff

View File

@ -66,7 +66,7 @@ typedef enum {
FEATURE_SERIALRX = 1 << 3,
FEATURE_MOTOR_STOP = 1 << 4,
FEATURE_SERVO_TILT = 1 << 5,
FEATURE_GYRO_SMOOTHING = 1 << 6,
FEATURE_SOFTSERIAL = 1 << 6,
FEATURE_LED_RING = 1 << 7,
FEATURE_GPS = 1 << 8,
FEATURE_FAILSAFE = 1 << 9,
@ -75,7 +75,6 @@ typedef enum {
FEATURE_POWERMETER = 1 << 12,
FEATURE_VARIO = 1 << 13,
FEATURE_3D = 1 << 14,
FEATURE_SOFTSERIAL = 1 << 15,
} AvailableFeatures;
typedef enum {

View File

@ -50,8 +50,8 @@ static const char * const mixerNames[] = {
// sync this with AvailableFeatures enum from board.h
static const char * const featureNames[] = {
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "SERIALRX", "MOTOR_STOP",
"SERVO_TILT", "GYRO_SMOOTHING", "LED_RING", "GPS",
"FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D", "SOFTSERIAL",
"SERVO_TILT", "SOFTSERIAL", "LED_RING", "GPS",
"FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D",
NULL
};

View File

@ -187,7 +187,6 @@ static void resetConf(void)
mcfg.max_angle_inclination = 500; // 50 degrees
mcfg.yaw_control_direction = 1;
mcfg.moron_threshold = 32;
mcfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
mcfg.vbatscale = 110;
mcfg.vbatmaxcellvoltage = 43;
mcfg.vbatmincellvoltage = 33;

View File

@ -57,20 +57,7 @@ void computeIMU(void)
accADC[Z] = 0;
}
if (feature(FEATURE_GYRO_SMOOTHING)) {
static uint8_t Smoothing[3] = { 0, 0, 0 };
static int16_t gyroSmooth[3] = { 0, 0, 0 };
if (Smoothing[0] == 0) {
// initialize
Smoothing[ROLL] = (mcfg.gyro_smoothing_factor >> 16) & 0xff;
Smoothing[PITCH] = (mcfg.gyro_smoothing_factor >> 8) & 0xff;
Smoothing[YAW] = (mcfg.gyro_smoothing_factor) & 0xff;
}
for (axis = 0; axis < 3; axis++) {
gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroADC[axis] + 1) / Smoothing[axis]);
gyroSmooth[axis] = gyroData[axis];
}
} else if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroADC[YAW]) / 3;
gyroYawSmooth = gyroData[YAW];
gyroData[ROLL] = gyroADC[ROLL];

View File

@ -247,7 +247,6 @@ typedef struct master_t {
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter.
uint16_t gyro_cmpfm_factor; // Set the Gyro Weight for Gyro/Magnetometer complementary filter. Increasing this value would reduce and delay Magnetometer influence on the output of the filter
uint32_t gyro_smoothing_factor; // How much to smoothen with per axis (32bit value with Roll, Pitch, Yaw in bits 24, 16, 8 respectively
uint8_t moron_threshold; // 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.
uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees).
int16_t accZero[3];