| /branches/dongfang_FC_rewrite/ENC-03_FC1.3.c |
|---|
| 16,7 → 16,7 |
| uint8_t i, axis, factor, numberOfAxesInRange = 0; |
| uint16_t timeout; |
| // GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
| timeout = SetDelay(2000); |
| timeout = setDelay(2000); |
| for (i = 140; i != 0; i--) { |
| // If all 3 axis are in range, shorten the remaining number of iterations. |
| 50,7 → 50,7 |
| // Wait for I2C to finish transmission. |
| while (twi_state) { |
| // Did it take too long? |
| if (CheckDelay(timeout)) { |
| if (checkDelay(timeout)) { |
| printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl"); |
| break; |
| } |
| 58,9 → 58,9 |
| analog_start(); |
| Delay_ms_Mess(i < 10 ? 10 : 2); |
| delay_ms_Mess(i < 10 ? 10 : 2); |
| } |
| Delay_ms_Mess(70); |
| delay_ms_Mess(70); |
| } |
| void gyro_setDefaults(void) { |
| /branches/dongfang_FC_rewrite/attitude.c |
|---|
| 295,29 → 295,30 |
| // Well actually the Z axis acc. check is not so silly. |
| uint8_t axis; |
| int32_t temp; |
| DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
| DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
| if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
| <= dynamicParams.UserParams[7]) { |
| DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
| uint8_t debugFullWeight = 1; |
| int32_t accDerived; |
| /* |
| if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| } |
| if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands |
| permilleAcc /= 2; |
| debugFullWeight = 0; |
| */ |
| if (controlActivity > 10000) { // reduce effect during stick commands |
| permilleAcc /= 4; |
| DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| if (controlActivity > 20000) { // reduce effect during stick commands |
| permilleAcc /= 4; |
| DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
| } |
| } |
| if (debugFullWeight) |
| DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
| else |
| DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
| /* |
| * Add to each sum: The amount by which the angle is changed just below. |
| */ |
| 332,8 → 333,6 |
| correctionSum[axis] += angle[axis] - temp; |
| } |
| } else { |
| DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
| DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
| DebugOut.Analog[9] = 0; |
| DebugOut.Analog[10] = 0; |
| 452,7 → 451,8 |
| if (w >= 0) { // maxAttitudeAngle < 32 |
| if (!ignoreCompassTimer) { |
| v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
| /*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/ |
| v = 64 + controlActivity / 100; |
| // yawGyroHeading - compassCourse on a -180..179 degree interval. |
| r |
| = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
| /branches/dongfang_FC_rewrite/configuration.c |
|---|
| 133,7 → 133,7 |
| const MMXLATION MMXLATIONS[] = { |
| {offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100}, |
| {offsetof(paramset_t, HeightP), offsetof(dynamicParam_t, HeightD),0,150}, |
| {offsetof(paramset_t, HeightP), offsetof(dynamicParam_t, HeightP),0,150}, |
| {offsetof(paramset_t, GyroP), offsetof(dynamicParam_t, GyroP),0,255}, |
| {offsetof(paramset_t, J16Timing), offsetof(dynamicParam_t, J16Timing),1,255}, |
| {offsetof(paramset_t, J17Timing), offsetof(dynamicParam_t, J17Timing),1,255}, |
| /branches/dongfang_FC_rewrite/configuration.h |
|---|
| 80,9 → 80,9 |
| uint8_t UserParam3; // Value : 0-250 |
| uint8_t UserParam4; // Value : 0-250 |
| */ |
| uint8_t ServoPitchControl; // Value : 0-250 // Stellung des Servos |
| uint8_t ServoPitchComp; // Value : 0-250 // Einfluss Gyro/Servo |
| uint8_t ServoPitchMin; // Value : 0-250 // Anschlag |
| uint8_t ServoPitchControl; |
| uint8_t ServoPitchComp; |
| uint8_t ServoPitchMin; |
| uint8_t ServoPitchMax; // Value : 0-250 // Anschlag |
| uint8_t ServoRefresh; // Value: 0-250 // Refreshrate of servo pwm output |
| uint8_t LoopGasLimit; // Value: 0-250 max. Gas wührend Looping |
| /branches/dongfang_FC_rewrite/controlMixer.c |
|---|
| 60,9 → 60,10 |
| #include "commands.h" |
| #include "output.h" |
| uint16_t maxControl[2] = { 0, 0 }; |
| int16_t control[2] = { 0, 0 }; |
| int16_t controlYaw = 0, controlThrottle = 0; |
| // uint16_t maxControl[2] = { 0, 0 }; |
| uint16_t controlActivity = 0; |
| int16_t controls[4] = { 0, 0, 0, 0 }; |
| //int16_t controlYaw = 0, controlThrottle = 0; |
| uint8_t looping = 0; |
| // Internal variables for reading commands made with an R/C stick. |
| 136,6 → 137,26 |
| return rcQ > ecQ ? rcQ : ecQ; |
| } |
| void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) { |
| int16_t tmp = controls[index]; |
| controls[index] = newValue; |
| tmp -= newValue; |
| tmp /= 2; |
| tmp = tmp * tmp; |
| // tmp += (newValue >= 0) ? newValue : -newValue; |
| if (controlActivity + (uint16_t)tmp >= controlActivity) |
| controlActivity += tmp; |
| else controlActivity = 0xffff; |
| } |
| #define CADAMPING 10 |
| void dampenControlActivity(void) { |
| uint32_t tmp = controlActivity; |
| tmp *= ((1<<CADAMPING)-1); |
| tmp >>= CADAMPING; |
| controlActivity = tmp; |
| } |
| /* |
| * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
| */ |
| 142,7 → 163,6 |
| void controlMixer_update(void) { |
| // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
| // TODO: If no signal --> zero. |
| uint8_t axis; |
| int16_t tempThrottle; |
| // takes almost no time... |
| 154,20 → 174,21 |
| // takes about 80 usec. |
| HC_update(); |
| int16_t* RC_PRTY = RC_getPRTY(); |
| int16_t* EC_PRTY = EC_getPRTY(); |
| int16_t* RC_PRTY = RC_getPRTY(); |
| int16_t* EC_PRTY = EC_getPRTY(); |
| control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
| control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
| updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]); |
| updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]); |
| updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]); |
| dampenControlActivity(); |
| DebugOut.Analog[14] = controlActivity/10; |
| tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
| controlThrottle = AC_getThrottle(tempThrottle); |
| controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle); |
| controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
| // controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
| DebugOut.Analog[12] = control[PITCH]; |
| DebugOut.Analog[13] = control[ROLL]; |
| if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
| controlMixer_updateVariables(); |
| configuration_applyVariablesToParams(); |
| 190,7 → 211,6 |
| /* |
| * Record maxima |
| */ |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
| maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
| 199,6 → 219,7 |
| } else if (maxControl[axis]) |
| maxControl[axis]--; |
| } |
| */ |
| uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
| : COMMAND_NONE; |
| /branches/dongfang_FC_rewrite/controlMixer.h |
|---|
| 71,9 → 71,10 |
| /* |
| * Our output. |
| */ |
| extern int16_t control[2]; |
| extern int16_t controlYaw, controlThrottle; |
| extern uint16_t maxControl[2]; |
| extern int16_t controls[4]; |
| //extern int16_t controlYaw, controlThrottle; |
| extern uint16_t controlActivity; |
| //extern uint16_t maxControl[2]; |
| extern uint8_t looping; |
| extern volatile uint8_t MKFlags; |
| /branches/dongfang_FC_rewrite/eeprom.c |
|---|
| 95,7 → 95,7 |
| * While we are still using userparams for flight parameters, do set |
| * some safe & meaningful default values. |
| */ |
| staticParams.UserParams1[3] = 8; // Throttle stick D=8 |
| staticParams.UserParams1[3] = 12; // Throttle stick D=12 |
| staticParams.UserParams2[0] = 0b11010101; // All gyro filter constants 2; acc. 4 |
| staticParams.UserParams2[1] = 2; // H&I motor smoothing. |
| staticParams.UserParams2[2] = 120; // Yaw I factor |
| 129,7 → 129,7 |
| staticParams.CompassYawEffect = 128; |
| staticParams.GyroP = 80; |
| staticParams.GyroI = 100; |
| staticParams.LowVoltageWarning = 95; |
| staticParams.LowVoltageWarning = 101; // 3.7 each for 3S |
| staticParams.EmergencyGas = 35; |
| staticParams.EmergencyGasDuration = 30; |
| staticParams.Unused0 = 0; |
| /branches/dongfang_FC_rewrite/flight.c |
|---|
| 172,7 → 172,7 |
| // We want that to kick as early as possible, not to delay new AD sampling further. |
| calculateFlightAttitude(); |
| controlMixer_update(); |
| throttleTerm = controlThrottle; |
| throttleTerm = controls[CONTROL_THROTTLE]; |
| // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
| if (throttleTerm < staticParams.MinThrottle + 10) |
| 261,7 → 261,7 |
| /************************************************************************/ |
| /* Yawing */ |
| /************************************************************************/ |
| if (abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
| if (abs(controls[CONTROL_YAW]) > 4 * staticParams.StickYawP) { // yaw stick is activated |
| ignoreCompassTimer = 1000; |
| if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
| updateCompassCourse = 1; |
| 273,7 → 273,7 |
| // Trim drift of yawAngleDiff with controlYaw. |
| // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
| // This seems to be used as: Difference desired <--> real heading. |
| yawAngleDiff -= controlYaw; |
| yawAngleDiff -= controls[CONTROL_YAW]; |
| // limit the effect |
| CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
| 353,7 → 353,7 |
| * between current throttle and maximum throttle). |
| */ |
| #define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
| yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
| yawTerm = PDPartYaw - controls[CONTROL_YAW] * CONTROL_SCALING; |
| // Limit yawTerm |
| DebugOut.Digital[0] &= ~DEBUG_CLIP; |
| if (throttleTerm > MIN_YAWGAS) { |
| 396,11 → 396,11 |
| // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
| // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
| // TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
| IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos. |
| IPart[axis] += PPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos. |
| } else { |
| // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
| // To keep up with a full stick PDPart should be about 156... |
| IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
| IPart[axis] += PDPart[axis] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
| } |
| tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability |
| 409,7 → 409,7 |
| // TODO: From which planet comes the 16000? |
| CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
| // Add (P, D) parts minus stick pos. to the scaled-down I part. |
| term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
| term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
| /* |
| * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
| 432,37 → 432,41 |
| DebugOut.Analog[12] = term[PITCH]; |
| DebugOut.Analog[13] = term[ROLL]; |
| DebugOut.Analog[14] = yawTerm; |
| //DebugOut.Analog[14] = yawTerm; |
| DebugOut.Analog[15] = throttleTerm; |
| for (i = 0; i < MAX_MOTORS; i++) { |
| int32_t tmp; |
| uint8_t throttle; |
| tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
| tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
| tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
| tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]; |
| tmp = tmp >> 6; |
| motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
| // Now we scale back down to a 0..255 range. |
| tmp = motorFilters[i] / MOTOR_SCALING; |
| // So this was the THIRD time a throttle was limited. But should the limitation |
| // apply to the common throttle signal (the one used for setting the "power" of |
| // all motors together) or should it limit the throttle set for each motor, |
| // including mix components of pitch, roll and yaw? I think only the common |
| // throttle should be limited. |
| // --> WRONG. This caused motors to stall completely in tight maneuvers. |
| // Apply to individual signals instead. |
| CHECK_MIN_MAX(tmp, 1, 255); |
| throttle = tmp; |
| if (i < 4) DebugOut.Analog[22 + i] = throttle; |
| if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
| tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
| tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
| tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
| tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]; |
| tmp = tmp >> 6; |
| motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
| // Now we scale back down to a 0..255 range. |
| tmp = motorFilters[i] / MOTOR_SCALING; |
| // So this was the THIRD time a throttle was limited. But should the limitation |
| // apply to the common throttle signal (the one used for setting the "power" of |
| // all motors together) or should it limit the throttle set for each motor, |
| // including mix components of pitch, roll and yaw? I think only the common |
| // throttle should be limited. |
| // --> WRONG. This caused motors to stall completely in tight maneuvers. |
| // Apply to individual signals instead. |
| CHECK_MIN_MAX(tmp, 1, 255); |
| motor[i].SetPoint = tmp; |
| motor[i].SetPoint = throttle; |
| } else if (motorTestActive) { |
| motor[i].SetPoint = motorTest[i]; |
| } else { |
| motor[i].SetPoint = 0; |
| } |
| if (i < 4) |
| DebugOut.Analog[22 + i] = motor[i].SetPoint; |
| } |
| I2C_Start(TWI_STATE_MOTOR_TX); |
| /branches/dongfang_FC_rewrite/output.c |
|---|
| 89,17 → 89,23 |
| static int8_t delay = 0; |
| if (!delay--) { // 10 ms intervals |
| delay = 4; |
| flashingLight(0, staticParams.J16Timing, staticParams.J16Bitmask, |
| dynamicParams.J16Timing); |
| flashingLight(1, staticParams.J17Timing, staticParams.J17Bitmask, |
| dynamicParams.J17Timing); |
| if (beepModulation != BEEP_MODULATION_NONE) { |
| // alarm |
| flashingLight(0, 25, 0x55, 25); |
| flashingLight(1, 25, 0xAA, 25); |
| } else { |
| flashingLight(0, staticParams.J16Timing, staticParams.J16Bitmask, |
| dynamicParams.J16Timing); |
| flashingLight(1, staticParams.J17Timing, staticParams.J17Bitmask, |
| dynamicParams.J17Timing); |
| } |
| } |
| } |
| void output_update(void) { |
| if (!DIGITAL_DEBUG_MASK) |
| if (!DIGITAL_DEBUG_MASK) { |
| flashingLights(); |
| else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_ON) { |
| } else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_ON) { |
| OUTPUT_SET(0, 1); |
| OUTPUT_SET(1, 1); |
| } else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_OFF) { |
| 138,9 → 144,9 |
| * Beep the R/C alarm signal |
| */ |
| void beepRCAlarm(void) { |
| if(beepModulation == 0xFFFF) { // If not already beeping an alarm signal (?) |
| if(beepModulation == BEEP_MODULATION_NONE) { // If not already beeping an alarm signal (?) |
| beepTime = 15000; // 1.5 seconds |
| beepModulation = 0x0C00; |
| beepModulation = BEEP_MODULATION_RCALARM; |
| } |
| } |
| 148,9 → 154,9 |
| * Beep the I2C bus error signal |
| */ |
| void beepI2CAlarm(void) { |
| if((beepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN)) { |
| if((beepModulation == BEEP_MODULATION_NONE) && (MKFlags & MKFLAG_MOTOR_RUN)) { |
| beepTime = 10000; // 1 second |
| beepModulation = 0x0080; |
| beepModulation = BEEP_MODULATION_I2CALARM; |
| } |
| } |
| 158,7 → 164,7 |
| * Beep the battery low alarm signal |
| */ |
| void beepBatteryAlarm(void) { |
| beepModulation = 0x0300; |
| beepModulation = BEEP_MODULATION_BATTERYALARM; |
| if(!beepTime) { |
| beepTime = 6000; // 0.6 seconds |
| } |
| 168,7 → 174,7 |
| * Beep the EEPROM checksum alarm |
| */ |
| void beepEEPROMAlarm(void) { |
| beepModulation = 0x0007; |
| beepModulation = BEEP_MODULATION_EEPROMALARM; |
| if(!beepTime) { |
| beepTime = 6000; // 0.6 seconds |
| } |
| /branches/dongfang_FC_rewrite/output.h |
|---|
| 45,9 → 45,16 |
| * at hand is resolved. |
| */ |
| // Both on |
| #define DEBUG_LEDTEST_ON 1000 |
| // Both off |
| #define DEBUG_LEDTEST_OFF 1001 |
| // #0 on |
| #define DEBUG_LEDTEST_0 1002 |
| // #1 on |
| #define DEBUG_LEDTEST_1 1003 |
| #define DEBUG_MAINLOOP_TIMER 1 |
| 63,7 → 70,7 |
| * Set to 0 for using outputs as the usual flashing lights. |
| * Set to one of the DEBUG_... defines h for using the outputs as debug lights. |
| */ |
| #define DIGITAL_DEBUG_MASK DEBUG_CLIP |
| #define DIGITAL_DEBUG_MASK DEBUG_ACC0THORDER |
| void output_init(void); |
| void output_update(void); |
| /branches/dongfang_FC_rewrite/timer0.c |
|---|
| 54,6 → 54,7 |
| #include "eeprom.h" |
| #include "analog.h" |
| #include "timer0.h" |
| // for debugging! |
| #include "uart0.h" |
| #include "output.h" |
| 66,7 → 67,7 |
| volatile uint8_t runFlightControl = 0; |
| volatile uint16_t cntKompass = 0; |
| volatile uint16_t beepTime = 0; |
| volatile uint16_t beepModulation = 0xFFFF; |
| volatile uint16_t beepModulation = BEEP_MODULATION_NONE; |
| #ifdef USE_NAVICTRL |
| volatile uint8_t SendSPI = 0; |
| 164,7 → 165,7 |
| beeper_On = 0; |
| } else { // beeper off if duration is over |
| beeper_On = 0; |
| beepModulation = 0xFFFF; |
| beepModulation = BEEP_MODULATION_NONE; |
| } |
| // if beeper is on |
| /branches/dongfang_FC_rewrite/timer0.h |
|---|
| 3,6 → 3,12 |
| #include <inttypes.h> |
| #define BEEP_MODULATION_NONE 0xFFFF |
| #define BEEP_MODULATION_RCALARM 0x0C00 |
| #define BEEP_MODULATION_I2CALARM 0x0080 |
| #define BEEP_MODULATION_BATTERYALARM 0x0300 |
| #define BEEP_MODULATION_EEPROMALARM 0x0007 |
| extern volatile uint16_t millisecondsCount; |
| extern volatile uint8_t runFlightControl; |
| extern volatile uint16_t cntKompass; |
| /branches/dongfang_FC_rewrite/uart0.c |
|---|
| 137,8 → 137,8 |
| "UBat ", |
| "Pitch Term ", |
| "Roll Term ", |
| "Yaw Term ", |
| "Throttle Term ", //15 |
| "controlActivity ", |
| "ca debug ", //15 |
| "0th O Corr pitch", |
| "0th O Corr roll ", |
| "unused ", |