englebert
/
inav-simplefc
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
master
timecop@gmail.com 12 years ago
parent
193902079c
commit
021b486916
8 changed files with 3184 additions and 3144 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6149
      obj/baseflight.hex
  2. 51
      src/config.c
  3. 12
      src/drv_l3g4200d.c
  4. 16
      src/drv_mma845x.c
  5. 12
      src/drv_mpu3050.c
  6. 34
      src/drv_mpu6050.c
  7. 9
      src/mw.h
  8. 45
      src/sensors.c

6149
obj/baseflight.hex
File diff suppressed because it is too large
View File

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)

Write Preview
Loading…
Cancel
Save