Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2019 → Rev 2020

/branches/dongfang_FC_rewrite/analog.c
134,12 → 134,10
* 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed
*/
 
//void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {}
 
void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {
static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1};
// Pitch to Pitch part
int8_t xx = (reverse & 1) ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant];
int8_t xx = reverse ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant];
// Roll to Pitch part
int8_t xy = rotationTab[(quadrant+2)%8];
// Pitch to Roll part
364,7 → 362,7
}
 
// 2.1: Transform axes.
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1);
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR);
 
for (uint8_t axis=0; axis<2; axis++) {
// 3) Filter.
386,7 → 384,7
/*
* Now process the data for attitude angles.
*/
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1);
rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR);
 
gyro_ATT[PITCH] = tempOffsetGyro[PITCH];
gyro_ATT[ROLL] = tempOffsetGyro[ROLL];
402,7 → 400,7
// gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL]) / staticParams.attitudeGyroFilter;
 
// Yaw gyro.
if (staticParams.imuReversedFlags & 2)
if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW)
yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
else
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
414,7 → 412,7
acc[axis] = rawAccValue(axis) - accOffset.offsets[axis];
}
 
rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & 4);
rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_ACC_XY);
 
for(uint8_t axis=0; axis<3; axis++) {
filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant;
599,7 → 597,7
accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;
int16_t min,max;
if (axis==Z) {
if (staticParams.imuReversedFlags & 8) {
if (staticParams.imuReversedFlags & IMU_REVERSE_ACC_Z) {
// TODO: This assumes a sensitivity of +/- 2g.
min = (256-200) * ACC_OVERSAMPLING_Z;
max = (256+200) * ACC_OVERSAMPLING_Z;
/branches/dongfang_FC_rewrite/configuration.c
311,6 → 311,7
mixerMatrix.motor[i][MIX_ROLL] = 0;
mixerMatrix.motor[i][MIX_YAW] = 0;
}
/*
// default = Quadro
mixerMatrix.motor[0][MIX_PITCH] = +64;
mixerMatrix.motor[0][MIX_YAW] = +64;
321,6 → 322,26
mixerMatrix.motor[3][MIX_ROLL] = +64;
mixerMatrix.motor[3][MIX_YAW] = -64;
memcpy(mixerMatrix.name, "Quadro\0", 7);
*/
 
// default = X
mixerMatrix.motor[0][MIX_PITCH] = +45;
mixerMatrix.motor[0][MIX_ROLL] = +45;
mixerMatrix.motor[0][MIX_YAW] = +64;
 
mixerMatrix.motor[1][MIX_PITCH] = -45;
mixerMatrix.motor[1][MIX_ROLL] = -45;
mixerMatrix.motor[1][MIX_YAW] = +64;
 
mixerMatrix.motor[2][MIX_PITCH] = +45;
mixerMatrix.motor[2][MIX_ROLL] = -45;
mixerMatrix.motor[2][MIX_YAW] = -64;
 
mixerMatrix.motor[3][MIX_PITCH] = -45;
mixerMatrix.motor[3][MIX_ROLL] = +45;
mixerMatrix.motor[3][MIX_YAW] = -64;
 
memcpy(mixerMatrix.name, "X\0", 7);
}
 
/***************************************************/
/branches/dongfang_FC_rewrite/configuration.h
214,6 → 214,11
#define CFG_AXIS_COUPLING_ACTIVE (1<<6)
#define CFG_GYRO_SATURATION_PREVENTION (1<<7)
 
#define IMU_REVERSE_GYRO_PR (1<<0)
#define IMU_REVERSE_GYRO_YAW (1<<1)
#define IMU_REVERSE_ACC_XY (1<<2)
#define IMU_REVERSE_ACC_Z (1<<3)
 
#define ATMEGA644 0
#define ATMEGA644P 1
#define SYSCLK F_CPU