| /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; |