/branches/dongfang_FC_rewrite/attitudeControl.c |
---|
16,7 → 16,7 |
void AC_getPRTY(int16_t* PRTY) { |
int16_t throttle = PRTY[CONTROL_THROTTLE]; |
int32_t projection; |
uint8_t effect = dynamicParams.attitudeControl; // Userparam 3 |
uint8_t effect = dynamicParams.attitudeControl; |
int16_t deltaThrottle, y; |
int16_t rollAngleInDegrees = attitude[ROLL]/GYRO_DEG_FACTOR_PITCHROLL; |
/branches/dongfang_FC_rewrite/configuration.c |
---|
10,7 → 10,7 |
int16_t variables[VARIABLE_COUNT]; |
ParamSet_t staticParams; |
ChannelMap_t channelMap; |
MixerMatrix_t mixerMatrix; |
MotorMixer_t motorMixer; |
IMUConfig_t IMUConfig; |
volatile DynamicParams_t dynamicParams; |
151,6 → 151,7 |
staticParams.minThrottle = 8; |
staticParams.maxThrottle = 230; |
staticParams.externalControl = 0; |
staticParams.attitudeControl = 0; |
staticParams.motorSmoothing = 0; |
staticParams.gyroP = 60; |
191,7 → 192,7 |
staticParams.outputFlash[1].bitmask = 3; //0b11110011; |
staticParams.outputFlash[1].timing = 15; |
staticParams.outputDebugMask = 8; |
staticParams.outputDebugMask = DEBUG_ACC0THORDER; |
staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS; |
staticParams.naviMode = 0; // free. |
215,8 → 216,7 |
staticParams.userParams[i] = i; |
} |
staticParams.bitConfig = |
CFG_GYRO_SATURATION_PREVENTION | CFG_COMPASS_ENABLED; |
staticParams.bitConfig = CFG_GYRO_SATURATION_PREVENTION; |
memcpy(staticParams.name, "Default\0", 6); |
} |
224,7 → 224,7 |
void IMUConfig_default(void) { |
IMUConfig.gyroPIDFilterConstant = 1; |
IMUConfig.gyroDWindowLength = 3; |
IMUConfig.gyroDFilterConstant = 1; |
// IMUConfig.gyroDFilterConstant = 1; |
IMUConfig.accFilterConstant = 10; |
IMUConfig.rateTolerance = 120; |
IMUConfig.gyroActivityDamping = 24; |
235,27 → 235,34 |
/***************************************************/ |
/* Default Values for Mixer Table */ |
/***************************************************/ |
void mixerMatrix_default(void) { // Quadro |
void motorMixer_default(void) { // Quadro |
uint8_t i; |
// mixerMatric.revision = EEMIXER_REVISION; |
// clear mixer table (but preset throttle) |
for (i = 0; i < 16; i++) { |
mixerMatrix.motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0; |
mixerMatrix.motor[i][MIX_PITCH] = 0; |
mixerMatrix.motor[i][MIX_ROLL] = 0; |
mixerMatrix.motor[i][MIX_YAW] = 0; |
for (i = 0; i < MAX_MOTORS; i++) { |
motorMixer.matrix[i][MIX_THROTTLE] = i < 4 ? 64 : 0; |
motorMixer.matrix[i][MIX_PITCH] = 0; |
motorMixer.matrix[i][MIX_ROLL] = 0; |
motorMixer.matrix[i][MIX_YAW] = 0; |
} |
// default = Quadro |
mixerMatrix.motor[0][MIX_PITCH] = +64; |
mixerMatrix.motor[0][MIX_YAW] = +64; |
mixerMatrix.motor[1][MIX_PITCH] = -64; |
mixerMatrix.motor[1][MIX_YAW] = +64; |
mixerMatrix.motor[2][MIX_ROLL] = -64; |
mixerMatrix.motor[2][MIX_YAW] = -64; |
mixerMatrix.motor[3][MIX_ROLL] = +64; |
mixerMatrix.motor[3][MIX_YAW] = -64; |
memcpy(mixerMatrix.name, "Quadro\0", 7); |
motorMixer.matrix[0][MIX_PITCH] = +64; |
motorMixer.matrix[0][MIX_YAW] = +64; |
motorMixer.matrix[0][MIX_OPPOSITE_MOTOR] = 1; |
motorMixer.matrix[1][MIX_PITCH] = -64; |
motorMixer.matrix[1][MIX_YAW] = +64; |
motorMixer.matrix[1][MIX_OPPOSITE_MOTOR] = 0; |
motorMixer.matrix[2][MIX_ROLL] = -64; |
motorMixer.matrix[2][MIX_YAW] = -64; |
motorMixer.matrix[2][MIX_OPPOSITE_MOTOR] = 3; |
motorMixer.matrix[3][MIX_ROLL] = +64; |
motorMixer.matrix[3][MIX_YAW] = -64; |
motorMixer.matrix[3][MIX_OPPOSITE_MOTOR] = 2; |
memcpy(motorMixer.name, "Quadro +\0", 9); |
/* |
// default = X |
mixerMatrix.motor[0][MIX_PITCH] = +45; |
273,9 → 280,9 |
mixerMatrix.motor[3][MIX_PITCH] = -45; |
mixerMatrix.motor[3][MIX_ROLL] = +45; |
mixerMatrix.motor[3][MIX_YAW] = -64; |
memcpy(motorMixer.name, "Quadro X\0", 9); |
*/ |
memcpy(mixerMatrix.name, "X\0", 7); |
} |
/***************************************************/ |
/branches/dongfang_FC_rewrite/configuration.h |
---|
92,9 → 92,10 |
typedef struct { |
char name[12]; |
int8_t motor[MAX_MOTORS][4]; |
}__attribute__((packed)) MixerMatrix_t; |
extern MixerMatrix_t mixerMatrix; |
int8_t matrix[MAX_MOTORS][5]; |
//int8_t oppositeMotor[MAX_MOTORS]; |
}__attribute__((packed)) MotorMixer_t; |
extern MotorMixer_t motorMixer; |
typedef struct { |
int16_t offsets[3]; |
122,7 → 123,7 |
uint8_t gyroPIDFilterConstant; |
uint8_t gyroDWindowLength; |
uint8_t gyroDFilterConstant; |
// uint8_t gyroDFilterConstant; |
uint8_t accFilterConstant; |
uint8_t zerothOrderCorrection; |
264,12 → 265,13 |
#define GRN_FLASH PORTB ^= (1<<PORTB1) |
// Mixer table |
#define MIX_THROTTLE 0 |
#define MIX_PITCH 1 |
#define MIX_ROLL 2 |
#define MIX_YAW 3 |
#define MIX_PITCH 0 |
#define MIX_ROLL 1 |
#define MIX_THROTTLE 2 |
#define MIX_YAW 3 |
#define MIX_OPPOSITE_MOTOR 4 |
#define VARIABLE_COUNT 8 |
#define VARIABLE_COUNT 8 |
extern volatile uint8_t MKFlags; |
extern uint8_t requiredMotors; |
283,7 → 285,7 |
void IMUConfig_default(void); |
void channelMap_default(void); |
void paramSet_default(uint8_t setnumber); |
void mixerMatrix_default(void); |
void motorMixer_default(void); |
void configuration_setNormalFlightParameters(void); |
void configuration_setFailsafeFlightParameters(void); |
/branches/dongfang_FC_rewrite/eeprom.c |
---|
149,25 → 149,25 |
/***************************************************/ |
/* MixerTable */ |
/***************************************************/ |
void mixerMatrix_writeToEEProm(void) { |
writeChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(MixerMatrix_t)); |
void motorMixer_writeToEEProm(void) { |
writeChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&motorMixer, EEPROM_ADR_MIXER_TABLE, sizeof(MotorMixer_t)); |
} |
void mixerMatrix_readOrDefault(void) { |
void motorMixer_readOrDefault(void) { |
// load mixer table |
if (readChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(MixerMatrix_t))) { |
printf("\n\rwriting default mixerMatrix"); |
mixerMatrix_default(); // Quadro |
mixerMatrix_writeToEEProm(); |
if (readChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&motorMixer, EEPROM_ADR_MIXER_TABLE, sizeof(MotorMixer_t))) { |
printf("\n\rwriting default motor mixer"); |
motorMixer_default(); // Quadro |
motorMixer_writeToEEProm(); |
} |
// determine motornumber |
requiredMotors = 0; |
for (uint8_t i=0; i<MAX_MOTORS; i++) { |
if (mixerMatrix.motor[i][MIX_THROTTLE]) |
if (motorMixer.matrix[i][MIX_THROTTLE]) |
requiredMotors++; |
} |
printf("\n\rMixer-Config: '%s' (%u Motors)",mixerMatrix.name, requiredMotors); |
printf("\n\rMixer-Config: '%s' (%u Motors)", motorMixer.name, requiredMotors); |
printf("\n\r==================================="); |
} |
/branches/dongfang_FC_rewrite/eeprom.h |
---|
25,14 → 25,14 |
#define EEPROM_ADR_PARAMSET_BEGIN 256 |
#define CHANNELMAP_REVISION 0 |
#define EEPARAM_REVISION 4 |
#define EEMIXER_REVISION 0 |
#define EEPARAM_REVISION 3 |
#define EEMIXER_REVISION 1 |
#define SENSOROFFSET_REVISION 0 |
#define IMUCONFIG_REVISION 0 |
void paramSet_readOrDefault(void); |
void channelMap_readOrDefault(void); |
void mixerMatrix_readOrDefault(void); |
void motorMixer_readOrDefault(void); |
void IMUConfig_readOrDefault(void); |
uint8_t paramSet_readFromEEProm(uint8_t setnumber); |
41,8 → 41,8 |
//uint8_t channelMap_readFromEEProm(void); |
void channelMap_writeToEEProm(void); |
//uint8_t mixerMatrix_readFromEEProm(void); |
void mixerMatrix_writeToEEProm(void); |
//uint8_t motorMixer_readFromEEProm(void); |
void motorMixer_writeToEEProm(void); |
uint8_t gyroAmplifierOffset_readFromEEProm(void); |
void gyroAmplifierOffset_writeToEEProm(void); |
/branches/dongfang_FC_rewrite/flight.c |
---|
264,10 → 264,10 |
int32_t tmp; |
uint8_t throttle; |
tmp = (int32_t) throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE]; |
tmp += (int32_t) term[PITCH] * mixerMatrix.motor[i][MIX_PITCH]; |
tmp += (int32_t) term[ROLL] * mixerMatrix.motor[i][MIX_ROLL]; |
tmp += (int32_t) yawTerm * mixerMatrix.motor[i][MIX_YAW]; |
tmp = (int32_t) throttleTerm * motorMixer.matrix[i][MIX_THROTTLE]; |
tmp += (int32_t) term[PITCH] * motorMixer.matrix[i][MIX_PITCH]; |
tmp += (int32_t) term[ROLL] * motorMixer.matrix[i][MIX_ROLL]; |
tmp += (int32_t) yawTerm * motorMixer.matrix[i][MIX_YAW]; |
tmp = tmp >> 6; |
motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
// Now we scale back down to a 0..255 range. |
288,7 → 288,7 |
debugOut.analog[10 + i] = throttle; |
*/ |
if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) { |
if ((MKFlags & MKFLAG_MOTOR_RUN) && motorMixer.matrix[i][MIX_THROTTLE] > 0) { |
motor[i].throttle = throttle; |
} else if (motorTestActive) { |
motor[i].throttle = motorTest[i]; |
/branches/dongfang_FC_rewrite/main.c |
---|
67,7 → 67,7 |
// Parameter Set handling |
IMUConfig_readOrDefault(); |
channelMap_readOrDefault(); |
mixerMatrix_readOrDefault(); |
motorMixer_readOrDefault(); |
paramSet_readOrDefault(); |
// enable interrupts global |
/branches/dongfang_FC_rewrite/menu.c |
---|
80,7 → 80,7 |
1, |
"HW:V%d.%d SW:%d.%d%c", |
boardRelease/10, boardRelease%10, VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a'); |
LCD_printfxy(0, 2, "Setting: %d %s", getActiveParamSet(), mixerMatrix.name); |
LCD_printfxy(0, 2, "Setting: %d %s", getActiveParamSet(), motorMixer.name); |
if (I2CTimeout < 6) { |
LCD_printfxy(0, 3, "I2C Error!!!"); |
} else if (missingMotor) { |
/branches/dongfang_FC_rewrite/twimaster.c |
---|
144,7 → 144,7 |
// Master Transmit |
case 0: // TWI_STATE_MOTOR_TX |
// skip motor if not used in mixer |
while ((mixerMatrix.motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS)) |
while ((motorMixer.matrix[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS)) |
motor_write++; |
if (motor_write >= MAX_MOTORS) { // writing finished, read now |
motor_write = 0; |
257,7 → 257,7 |
} |
for (i = 0; i < MAX_MOTORS; i++) { |
if (!motor[i].present && mixerMatrix.motor[i][MIX_THROTTLE] > 0) |
if (!motor[i].present && motorMixer.matrix[i][MIX_THROTTLE] > 0) |
printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
motor[i].error = 0; |
} |
/branches/dongfang_FC_rewrite/uart0.c |
---|
477,16 → 477,18 |
externalControlActive = 255; |
break; |
case 'n':// "Get Mixer Table |
while (!txd_complete) |
; // wait for previous frame to be sent |
sendOutData('N', FC_ADDRESS, 1, (uint8_t *) &mixerMatrix, sizeof(mixerMatrix)); |
case 'n':// Read motor mixer |
tempchar[0] = EEMIXER_REVISION; |
tempchar[1] = sizeof(MotorMixer_t); |
while (!txd_complete) |
; // wait for previous frame to be sent |
sendOutData('N', FC_ADDRESS, 2, &tempchar, 2, (uint8_t *)&motorMixer, sizeof(motorMixer)); |
break; |
case 'm':// "Set Mixer Table |
if (pRxData[0] == EEMIXER_REVISION) { |
memcpy(&mixerMatrix, (uint8_t*) pRxData, sizeof(mixerMatrix)); |
mixerMatrix_writeToEEProm(); |
if (pRxData[0] == EEMIXER_REVISION && (pRxData[1] == sizeof(MotorMixer_t))) { |
memcpy(&motorMixer, (uint8_t*)pRxData[2], sizeof(motorMixer)); |
motorMixer_writeToEEProm(); |
while (!txd_complete) |
; // wait for previous frame to be sent |
tempchar[0] = 1; |