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

added ability to swap sensor axises dynamically. still needs a sane way to configire in CLI, though. 

adjusted all drivers for the new align stuff.
commented out default config setting values to zero - memset already did that.


git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@227 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2012-10-07 18:03:42 +00:00
parent 193902079c
commit 021b486916
8 changed files with 3184 additions and 3144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6149
obj/baseflight.hex
View File

File diff suppressed because it is too large Load Diff

51
src/config.c
View File

@ -12,7 +12,7 @@
config_t cfg;
const char rcChannelLetters[] = "AERT1234";
static uint8_t EEPROM_CONF_VERSION = 32;
static uint8_t EEPROM_CONF_VERSION = 33;
static uint32_t enabledSensors = 0;
static void resetConf(void);
@ -123,6 +123,8 @@ void checkFirstTime(bool reset)
static void resetConf(void)
{
int i;
const int8_t default_align[3][3] = { /* GYRO */ { 0, 0, 0 }, /* ACC */ { 0, 0, 0 }, /* MAG */ { -2, -3, 1 } };
memset(&cfg, 0, sizeof(config_t));
cfg.version = EEPROM_CONF_VERSION;
@ -130,7 +132,7 @@ static void resetConf(void)
featureClearAll();
featureSet(FEATURE_VBAT);
cfg.looptime = 0;
// cfg.looptime = 0;
cfg.P8[ROLL] = 40;
cfg.I8[ROLL] = 30;
cfg.D8[ROLL] = 23;
@ -139,13 +141,13 @@ static void resetConf(void)
cfg.D8[PITCH] = 23;
cfg.P8[YAW] = 85;
cfg.I8[YAW] = 45;
cfg.D8[YAW] = 0;
// cfg.D8[YAW] = 0;
cfg.P8[PIDALT] = 16;
cfg.I8[PIDALT] = 15;
cfg.D8[PIDALT] = 7;
cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100;
cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
cfg.D8[PIDPOS] = 0;
// cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100;
// cfg.D8[PIDPOS] = 0;
cfg.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10;
cfg.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100;
cfg.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000;
@ -156,24 +158,25 @@ static void resetConf(void)
cfg.I8[PIDLEVEL] = 10;
cfg.D8[PIDLEVEL] = 20;
cfg.P8[PIDMAG] = 40;
cfg.P8[PIDVEL] = 0;
cfg.I8[PIDVEL] = 0;
cfg.D8[PIDVEL] = 0;
// cfg.P8[PIDVEL] = 0;
// cfg.I8[PIDVEL] = 0;
// cfg.D8[PIDVEL] = 0;
cfg.rcRate8 = 90;
cfg.rcExpo8 = 65;
cfg.rollPitchRate = 0;
cfg.yawRate = 0;
cfg.dynThrPID = 0;
// cfg.rollPitchRate = 0;
// cfg.yawRate = 0;
// cfg.dynThrPID = 0;
cfg.thrMid8 = 50;
cfg.thrExpo8 = 0;
for (i = 0; i < CHECKBOXITEMS; i++)
cfg.activate[i] = 0;
cfg.angleTrim[0] = 0;
cfg.angleTrim[1] = 0;
cfg.accZero[0] = 0;
cfg.accZero[1] = 0;
cfg.accZero[2] = 0;
cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
// cfg.thrExpo8 = 0;
// for (i = 0; i < CHECKBOXITEMS; i++)
// cfg.activate[i] = 0;
// cfg.angleTrim[0] = 0;
// cfg.angleTrim[1] = 0;
// cfg.accZero[0] = 0;
// cfg.accZero[1] = 0;
// cfg.accZero[2] = 0;
// cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
memcpy(&cfg.align, default_align, sizeof(cfg.align));
cfg.acc_hardware = ACC_DEFAULT; // default/autodetect
cfg.acc_lpf_factor = 4;
cfg.acc_lpf_for_velocity = 10;
@ -191,14 +194,14 @@ static void resetConf(void)
// Radio
parseRcChannels("AETR1234");
cfg.deadband = 0;
cfg.yawdeadband = 0;
// cfg.deadband = 0;
// cfg.yawdeadband = 0;
cfg.alt_hold_throttle_neutral = 20;
cfg.spektrum_hires = 0;
// cfg.spektrum_hires = 0;
cfg.midrc = 1500;
cfg.mincheck = 1100;
cfg.maxcheck = 1900;
cfg.retarded_arm = 0; // disable arm/disarm on roll left/right
// cfg.retarded_arm = 0; // disable arm/disarm on roll left/right
// Failsafe Variables
cfg.failsafe_delay = 10; // 1sec

12
src/drv_l3g4200d.c
View File

@ -82,9 +82,9 @@ static void l3g4200dInit(void)
static void l3g4200dAlign(int16_t *gyroData)
{
gyroData[0] = -gyroData[0] / 4;
gyroData[1] = gyroData[1] / 4;
gyroData[2] = -gyroData[2] / 4;
gyroData[0] = -gyroData[0];
gyroData[1] = gyroData[1];
gyroData[2] = -gyroData[2];
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
@ -92,7 +92,7 @@ static void l3g4200dRead(int16_t *gyroData)
{
uint8_t buf[6];
i2cRead(L3G4200D_ADDRESS, L3G4200D_GYRO_OUT, 6, buf);
gyroData[1] = (buf[0] << 8) | buf[1];
gyroData[0] = (buf[2] << 8) | buf[3];
gyroData[2] = (buf[4] << 8) | buf[5];
gyroData[1] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
gyroData[0] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
}

16
src/drv_mma845x.c
View File

@ -97,18 +97,14 @@ static void mma8452Read(int16_t *accelData)
uint8_t buf[6];
i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf);
accelData[0] = (buf[0] << 8) | buf[1];
accelData[1] = (buf[2] << 8) | buf[3];
accelData[2] = (buf[4] << 8) | buf[5];
accelData[0] >>= 2;
accelData[1] >>= 2;
accelData[2] >>= 2;
accelData[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
accelData[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
accelData[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
}
static void mma8452Align(int16_t *accelData)
{
accelData[0] = -accelData[0] / 4;
accelData[1] = -accelData[1] / 4;
accelData[2] = accelData[2] / 4;
accelData[0] = -accelData[0];
accelData[1] = -accelData[1];
accelData[2] = accelData[2];
}

12
src/drv_mpu3050.c
View File

@ -94,9 +94,9 @@ static void mpu3050Init(void)
static void mpu3050Align(int16_t *gyroData)
{
// official direction is RPY
gyroData[0] = gyroData[0] / 4;
gyroData[1] = gyroData[1] / 4;
gyroData[2] = -gyroData[2] / 4;
gyroData[0] = gyroData[0];
gyroData[1] = gyroData[1];
gyroData[2] = -gyroData[2];
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
@ -104,9 +104,9 @@ static void mpu3050Read(int16_t *gyroData)
{
uint8_t buf[6];
i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
gyroData[0] = (buf[0] << 8) | buf[1];
gyroData[1] = (buf[2] << 8) | buf[3];
gyroData[2] = (buf[4] << 8) | buf[5];
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
}
static void mpu3050ReadTemp(int16_t *tempData)

34
src/drv_mpu6050.c
View File

@ -189,30 +189,30 @@ static void mpu6050AccInit(void)
acc_1G = 1023;
}
static void mpu6050AccRead(int16_t * accData)
static void mpu6050AccRead(int16_t *accData)
{
uint8_t buf[6];
#ifndef MPU6050_DMP
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
accData[0] = (buf[0] << 8) | buf[1];
accData[1] = (buf[2] << 8) | buf[3];
accData[2] = (buf[4] << 8) | buf[5];
accData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 8;
accData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 8;
accData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 8;
#else
accData[0] = accData[1] = accData[2] = 0;
#endif
}
static void mpu6050AccAlign(int16_t * accData)
static void mpu6050AccAlign(int16_t *accData)
{
int16_t temp[2];
temp[0] = accData[0];
temp[1] = accData[1];
// official direction is RPY
accData[0] = temp[1] / 8;
accData[1] = -temp[0] / 8;
accData[2] = accData[2] / 8;
accData[0] = temp[1];
accData[1] = -temp[0];
accData[2] = accData[2];
}
static void mpu6050GyroInit(void)
@ -251,22 +251,22 @@ static void mpu6050GyroRead(int16_t * gyroData)
uint8_t buf[6];
#ifndef MPU6050_DMP
i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
gyroData[0] = (buf[0] << 8) | buf[1];
gyroData[1] = (buf[2] << 8) | buf[3];
gyroData[2] = (buf[4] << 8) | buf[5];
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
#else
gyroData[0] = dmpGyroData[0];
gyroData[1] = dmpGyroData[1];
gyroData[2] = dmpGyroData[2];
gyroData[0] = dmpGyroData[0] / 4 ;
gyroData[1] = dmpGyroData[1] / 4;
gyroData[2] = dmpGyroData[2] / 4;
#endif
}
static void mpu6050GyroAlign(int16_t * gyroData)
{
// official direction is RPY
gyroData[0] = gyroData[0] / 4;
gyroData[1] = gyroData[1] / 4;
gyroData[2] = -gyroData[2] / 4;
gyroData[0] = gyroData[0];
gyroData[1] = gyroData[1];
gyroData[2] = -gyroData[2];
}
#ifdef MPU6050_DMP

9
src/mw.h
View File

@ -113,6 +113,12 @@ typedef struct mixer_t {
const motorMixer_t *motor;
} mixer_t;
enum {
ALIGN_GYRO = 0,
ALIGN_ACCEL = 1,
ALIGN_MAG = 2
};
typedef struct config_t {
uint8_t version;
uint16_t size;
@ -141,11 +147,12 @@ typedef struct config_t {
int16_t angleTrim[2]; // accelerometer trim
// sensor-related stuff
int8_t align[3][3]; // acc, gyro, mag alignment (ex: with sensor output of X, Y, Z, align of 1 -3 2 would return X, -Z, Y)
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_lpf; // mpuX050 LPF setting (TODO make it work on L3GD as well)
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.

45
src/sensors.c
View File

@ -152,6 +152,28 @@ void batteryInit(void)
batteryWarningVoltage = i * cfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI
}
// ALIGN_GYRO = 0,
// ALIGN_ACCEL = 1,
// ALIGN_MAG = 2
static void alignSensors(uint8_t type, int16_t *data)
{
int i;
int16_t tmp[3];
// make a copy :(
tmp[0] = data[0];
tmp[1] = data[1];
tmp[2] = data[2];
for (i = 0; i < 3; i++) {
int8_t axis = cfg.align[type][i];
if (axis > 0)
data[axis - 1] = tmp[i];
else
data[-axis - 1] = -tmp[i];
}
}
static void ACC_Common(void)
{
static int32_t a[3];
@ -237,7 +259,11 @@ static void ACC_Common(void)
void ACC_getADC(void)
{
acc.read(accADC);
acc.align(accADC);
// if we have CUSTOM alignment configured, user is "assumed" to know what they're doing
if (cfg.align[ALIGN_ACCEL][0])
alignSensors(ALIGN_ACCEL, accADC);
else
acc.align(accADC);
ACC_Common();
}
@ -316,7 +342,11 @@ void Gyro_getADC(void)
{
// range: +/- 8192; +/- 2000 deg/sec
gyro.read(gyroADC);
gyro.align(gyroADC);
// if we have CUSTOM alignment configured, user is "assumed" to know what they're doing
if (cfg.align[ALIGN_GYRO][0])
alignSensors(ALIGN_GYRO, gyroADC);
else
gyro.align(gyroADC);
GYRO_Common();
}
@ -327,13 +357,14 @@ static uint8_t magInit = 0;
static void Mag_getRawADC(void)
{
static int16_t rawADC[3];
hmc5883lRead(rawADC);
hmc5883lRead(magADC);
// Default mag orientation is -2, -3, 1 or
// no way? is THIS finally the proper orientation?? (by GrootWitBaas)
magADC[ROLL] = rawADC[2]; // X
magADC[PITCH] = -rawADC[0]; // Y
magADC[YAW] = -rawADC[1]; // Z
// magADC[ROLL] = rawADC[2]; // X
// magADC[PITCH] = -rawADC[0]; // Y
// magADC[YAW] = -rawADC[1]; // Z
alignSensors(ALIGN_MAG, magADC);
}
void Mag_init(void)