Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 03:20:00 +03:00
Code Issues Wiki Activity

more mahowik althold changes. at least it doesn't shoot up in the air now on enable... but still nothing impressive.

Browse source 
git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@217 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2012-09-12 07:09:45 +00:00
parent a139b96de6
commit a51bb66ad4
3 changed files with 27 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/config.c
View file

@ -13,7 +13,7 @@ config_t cfg;
const char rcChannelLetters[] = "AERT1234";
static uint32_t enabledSensors = 0;
uint8_t checkNewConf = 30;
uint8_t checkNewConf = 31;
void parseRcChannels(const char *input)
{
@ -136,9 +136,13 @@ void checkFirstTime(bool reset)
cfg.acc_hardware = ACC_DEFAULT; // default/autodetect
cfg.acc_lpf_factor = 4;
cfg.acc_lpf_for_velocity = 10;
cfg.accz_deadband = 50;
cfg.gyro_cmpf_factor = 400; // default MWC
cfg.gyro_lpf = 42;
cfg.mpu6050_scale = 1; // fuck invensense
cfg.baro_tab_size = 21;
cfg.baro_noise_lpf = 0.6f;
cfg.baro_cf = 0.985f;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
cfg.vbatscale = 110;
cfg.vbatmaxcellvoltage = 43;

37
src/imu.c
View file

@ -262,6 +262,7 @@ static void getEstimatedAttitude(void)
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay
#define BARO_TAB_SIZE 40
#define BARO_TAB_SIZE_MAX 48
#define ACC_Z_DEADBAND (acc_1G / 50)
#ifdef NEW_ACCZ_HOLD
@ -307,12 +308,11 @@ int32_t isq(int32_t x)
void getEstimatedAltitude(void)
{
uint8_t index;
static uint32_t deadLine = INIT_DELAY;
static int16_t BaroHistTab[BARO_TAB_SIZE];
static int8_t BaroHistIdx;
static int32_t BaroHigh;
uint16_t dTime;
static int16_t baroHistTab[BARO_TAB_SIZE_MAX];
static int8_t baroHistIdx;
static int32_t baroHigh;
uint32_t dTime;
int16_t error;
float invG;
int16_t accZ;
@ -326,21 +326,21 @@ void getEstimatedAltitude(void)
deadLine = currentTime;
//**** Alt. Set Point stabilization PID ****
BaroHistTab[BaroHistIdx] = BaroAlt / 10;
BaroHigh += BaroHistTab[BaroHistIdx];
index = (BaroHistIdx + (BARO_TAB_SIZE / 2)) % BARO_TAB_SIZE;
BaroHigh -= BaroHistTab[index];
baroHistTab[baroHistIdx] = BaroAlt / 10;
baroHigh += baroHistTab[baroHistIdx];
baroHigh -= baroHistTab[(baroHistIdx + 1) % cfg.baro_tab_size];
BaroHistIdx++;
if (BaroHistIdx == BARO_TAB_SIZE)
BaroHistIdx = 0;
baroHistIdx++;
if (baroHistIdx == cfg.baro_tab_size)
baroHistIdx = 0;
// EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
EstAlt = EstAlt * 0.6f + (BaroHigh * 10.0f / (BARO_TAB_SIZE / 2)) * 0.4f; // additional LPF to reduce baro noise
EstAlt = EstAlt * cfg.baro_noise_lpf + (baroHigh * 10.0f / (cfg.baro_tab_size - 1)) * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise
// P
error = applyDeadband16(AltHold - EstAlt, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain((cfg.P8[PIDALT] * error / 100), -100, +100);
error = constrain(AltHold - EstAlt, -300, 300);
error = applyDeadband16(error, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain((cfg.P8[PIDALT] * error / 100), -150, +150);
// I
errorAltitudeI += error * cfg.I8[PIDALT] / 50;
@ -352,14 +352,13 @@ void getEstimatedAltitude(void)
invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
// int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - acc_1G;
// int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - 1/invG;
accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
accZ = applyDeadband16(accZ, ACC_Z_DEADBAND);
accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
accZ = applyDeadband16(accZ, acc_1G / cfg.accz_deadband);
debug[0] = accZ;
// Integrator - velocity, cm/sec
vel += accZ * accVelScale * dTime;
lastBaroAlt = EstAlt;
baroVel = (EstAlt - lastBaroAlt) / (dTime / 1000000.0f);
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero
@ -367,7 +366,7 @@ void getEstimatedAltitude(void)
debug[1] = baroVel;
// apply Complimentary Filter to keep near zero caluculated velocity based on baro velocity
vel = vel * 0.985f + baroVel * 0.015f;
vel = vel * cfg.baro_cf + baroVel * (1.0f - cfg.baro_cf);
// vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
debug[2] = vel;
// debug[3] = applyDeadbandFloat(vel, 5);

4
src/mw.h
View file

@ -140,10 +140,14 @@ typedef struct config_t {
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
uint8_t acc_lpf_for_velocity; // ACC lowpass for AccZ height hold
uint8_t accz_deadband; // ??
uint16_t gyro_lpf; // mpuX050 LPF setting
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.
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 mpu6050_scale; // seems es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES, need this to be dynamic. fucking invenshit won't release chip IDs so I can't autodetect it.
uint8_t baro_tab_size; // size of baro filter array
float baro_noise_lpf; // additional LPF to reduce baro noise
float baro_cf; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
uint16_t activate[CHECKBOXITEMS]; // activate switches
uint8_t vbatscale; // adjust this to match battery voltage to reported value