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:
parent
193902079c
commit
021b486916
6149
obj/baseflight.hex
6149
obj/baseflight.hex
File diff suppressed because it is too large
Load Diff
51
src/config.c
51
src/config.c
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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];
|
||||
}
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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
9
src/mw.h
|
@ -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.
|
||||
|
|
|
@ -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)
|
||||
|
|
Loading…
Reference in New Issue