| /branches/dongfang_FC_rewrite/ENC-03_FC1.3.c |
|---|
| 65,8 → 65,8 |
| void gyro_setDefaults(void) { |
| staticParams.GyroD = 3; |
| staticParams.DriftComp = 250; |
| staticParams.GyroAccFactor = 10; |
| staticParams.DriftComp = 100; |
| staticParams.GyroAccFactor = 2; |
| staticParams.GyroAccTrim = 1; |
| // Not used. |
| /branches/dongfang_FC_rewrite/analog.c |
|---|
| 51,8 → 51,9 |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include <avr/pgmspace.h> |
| #include "analog.h" |
| #include "attitude.h" |
| #include "sensors.h" |
| // for Delay functions |
| 446,7 → 447,7 |
| // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v). |
| // This is divided by 3 --> 10.34 counts per volt. |
| UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
| DebugOut.Analog[11] = UBat; |
| DebugOut.Analog[11] = HIRES_GYRO_INTEGRATION_FACTOR;//UBat; |
| analogDataReady = 1; // mark |
| ADCycleCount++; |
| // Stop the sampling. Cycle is over. |
| /branches/dongfang_FC_rewrite/attitude.c |
|---|
| 156,7 → 156,7 |
| ************************************************************************/ |
| int32_t getAngleEstimateFromAcc(uint8_t axis) { |
| return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
| return GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis]; |
| } |
| void setStaticAttitudeAngles(void) { |
| 181,9 → 181,6 |
| // Calibrate hardware. |
| analog_calibrate(); |
| // reset gyro readings |
| // rate_ATT[PITCH] = rate_ATT[ROLL] = yawRate = 0; |
| // reset gyro integrals to acc guessing |
| setStaticAttitudeAngles(); |
| yawAngleDiff = 0; |
| 207,10 → 204,8 |
| uint8_t axis; |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| rate_PID[axis] = (gyro_PID[axis] + driftComp[axis]) |
| / HIRES_GYRO_INTEGRATION_FACTOR; |
| rate_ATT[axis] = (gyro_ATT[axis] + driftComp[axis]) |
| / HIRES_GYRO_INTEGRATION_FACTOR; |
| rate_PID[axis] = gyro_PID[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis]; |
| rate_ATT[axis] = gyro_ATT[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis]; |
| differential[axis] = gyroD[axis]; |
| averageAcc[axis] += acc[axis]; |
| } |
| 297,7 → 292,7 |
| // are less than ....., or reintroduce Kalman. |
| // Well actually the Z axis acc. check is not so silly. |
| uint8_t axis; |
| int32_t correction; |
| int32_t temp; |
| if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
| <= dynamicParams.UserParams[7]) { |
| DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| 329,11 → 324,10 |
| DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
| // 1000 * the correction amount that will be added to the gyro angle in next line. |
| correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
| angle[axis] = ((int32_t) (1000L - permilleAcc) * angle[axis] |
| temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
| angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
| + (int32_t) permilleAcc * accDerived) / 1000L; |
| correctionSum[axis] += angle[axis] - correction; |
| DebugOut.Analog[16 + axis] = angle[axis] - correction; |
| correctionSum[axis] += angle[axis] - temp; |
| } |
| } else { |
| DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
| 368,13 → 362,12 |
| timer = DRIFTCORRECTION_TIME; |
| for (axis = PITCH; axis <= ROLL; axis++) { |
| // Take the sum of corrections applied, add it to delta |
| deltaCorrection = (correctionSum[axis] * HIRES_GYRO_INTEGRATION_FACTOR |
| + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME; |
| deltaCorrection = (correctionSum[axis] + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME; |
| // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
| CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
| // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| DebugOut.Analog[16 + axis] = correctionSum[axis]; |
| DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim; |
| DebugOut.Analog[28 + axis] = driftComp[axis]; |
| /branches/dongfang_FC_rewrite/configuration.h |
|---|
| 5,128 → 5,128 |
| #include <avr/io.h> |
| typedef struct { |
| /*PMM*/uint8_t HeightD; |
| /* P */uint8_t MaxHeight; |
| /*PMM*/uint8_t HeightP; |
| /* P */uint8_t Height_ACC_Effect; |
| /* P */uint8_t CompassYawEffect; |
| /* P */uint8_t GyroD; |
| /*PMM*/uint8_t GyroP; |
| /* P */uint8_t GyroI; |
| /* Never used */uint8_t StickYawP; |
| /* P */uint8_t IFactor; |
| /* P */uint8_t UserParams[8]; |
| /* P */uint8_t ServoPitchControl; |
| /* P */uint8_t LoopGasLimit; |
| /* P */uint8_t AxisCoupling1; |
| /* P */uint8_t AxisCoupling2; |
| /* P */uint8_t AxisCouplingYawCorrection; |
| /* P */uint8_t DynamicStability; |
| /* P */uint8_t ExternalControl; |
| /*PMM*/uint8_t J16Timing; |
| /*PMM*/uint8_t J17Timing; |
| /* P */uint8_t NaviGpsModeControl; |
| /* P */uint8_t NaviGpsGain; |
| /* P */uint8_t NaviGpsP; |
| /* P */uint8_t NaviGpsI; |
| /* P */uint8_t NaviGpsD; |
| /* P */uint8_t NaviGpsACC; |
| /*PMM*/uint8_t NaviOperatingRadius; |
| /* P */uint8_t NaviWindCorrection; |
| /* P */uint8_t NaviSpeedCompensation; |
| int8_t KalmanK; |
| int8_t KalmanMaxDrift; |
| int8_t KalmanMaxFusion; |
| /*PMM*/uint8_t HeightD; |
| /* P */uint8_t MaxHeight; |
| /*PMM*/uint8_t HeightP; |
| /* P */uint8_t Height_ACC_Effect; |
| /* P */uint8_t CompassYawEffect; |
| /* P */uint8_t GyroD; |
| /*PMM*/uint8_t GyroP; |
| /* P */uint8_t GyroI; |
| /* Never used */uint8_t StickYawP; |
| /* P */uint8_t IFactor; |
| /* P */uint8_t UserParams[8]; |
| /* P */uint8_t ServoPitchControl; |
| /* P */uint8_t LoopGasLimit; |
| /* P */uint8_t AxisCoupling1; |
| /* P */uint8_t AxisCoupling2; |
| /* P */uint8_t AxisCouplingYawCorrection; |
| /* P */uint8_t DynamicStability; |
| /* P */uint8_t ExternalControl; |
| /*PMM*/uint8_t J16Timing; |
| /*PMM*/uint8_t J17Timing; |
| /* P */uint8_t NaviGpsModeControl; |
| /* P */uint8_t NaviGpsGain; |
| /* P */uint8_t NaviGpsP; |
| /* P */uint8_t NaviGpsI; |
| /* P */uint8_t NaviGpsD; |
| /* P */uint8_t NaviGpsACC; |
| /*PMM*/uint8_t NaviOperatingRadius; |
| /* P */uint8_t NaviWindCorrection; |
| /* P */uint8_t NaviSpeedCompensation; |
| int8_t KalmanK; |
| int8_t KalmanMaxDrift; |
| int8_t KalmanMaxFusion; |
| } dynamicParam_t; |
| extern dynamicParam_t dynamicParams; |
| typedef struct { |
| uint8_t sourceIdx, targetIdx; |
| uint8_t min, max; |
| uint8_t sourceIdx, targetIdx; |
| uint8_t min, max; |
| } MMXLATION; |
| typedef struct { |
| uint8_t sourceIdx, targetIdx; |
| uint8_t sourceIdx, targetIdx; |
| } XLATION; |
| // values above 250 representing poti1 to poti4 |
| typedef struct { |
| uint8_t ChannelAssignment[8]; // see upper defines for details |
| uint8_t GlobalConfig; // see upper defines for bitcoding |
| uint8_t HeightMinGas; // Value : 0-100 |
| uint8_t HeightD; // Value : 0-250 |
| uint8_t MaxHeight; // Value : 0-32 |
| uint8_t HeightP; // Value : 0-32 |
| uint8_t Height_Gain; // Value : 0-50 |
| uint8_t Height_ACC_Effect; // Value : 0-250 |
| uint8_t StickP; // Value : 1-6 |
| uint8_t StickD; // Value : 0-64 |
| uint8_t StickYawP; // Value : 1-20 |
| uint8_t MinThrottle; // Value : 0-32 |
| uint8_t MaxThrottle; // Value : 33-250 |
| uint8_t GyroAccFactor; // Value : 1-64 |
| uint8_t CompassYawEffect; // Value : 0-32 |
| uint8_t GyroP; // Value : 10-250 |
| uint8_t GyroI; // Value : 0-250 |
| uint8_t GyroD; // Value : 0-250 |
| uint8_t LowVoltageWarning; // Value : 0-250 |
| uint8_t EmergencyGas; // Value : 0-250 //Gaswert bei Emp�ngsverlust |
| uint8_t EmergencyGasDuration; // Value : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen |
| uint8_t Unused0; // |
| uint8_t IFactor; // Value : 0-250 |
| uint8_t UserParams1[4]; // Value : 0-250 |
| /* |
| uint8_t UserParam2; // Value : 0-250 |
| 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 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 |
| uint8_t LoopThreshold; // Value: 0-250 Schwelle f�r Stickausschlag |
| uint8_t LoopHysteresis; // Value: 0-250 Hysterese f�r Stickausschlag |
| uint8_t AxisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
| uint8_t AxisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
| uint8_t AxisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
| uint8_t AngleTurnOverPitch; // Value: 0-250 180�-Punkt |
| uint8_t AngleTurnOverRoll; // Value: 0-250 180�-Punkt |
| uint8_t GyroAccTrim; // 1/k (Koppel_ACC_Wirkung) |
| uint8_t DriftComp; // limit for gyrodrift compensation |
| uint8_t DynamicStability; // PID limit for Attitude controller |
| uint8_t UserParams2[4]; // Value : 0-250 |
| /* |
| uint8_t UserParam6; // Value : 0-250 |
| uint8_t UserParam7; // Value : 0-250 |
| uint8_t UserParam8; // Value : 0-250 |
| */ |
| uint8_t J16Bitmask; // for the J16 Output |
| uint8_t J16Timing; // for the J16 Output |
| uint8_t J17Bitmask; // for the J17 Output |
| uint8_t J17Timing; // for the J17 Output |
| uint8_t NaviGpsModeControl; // Parameters for the Naviboard |
| uint8_t NaviGpsGain; // overall gain for GPS-PID controller |
| uint8_t NaviGpsP; // P gain for GPS-PID controller |
| uint8_t NaviGpsI; // I gain for GPS-PID controller |
| uint8_t NaviGpsD; // D gain for GPS-PID controller |
| uint8_t NaviGpsPLimit; // P limit for GPS-PID controller |
| uint8_t NaviGpsILimit; // I limit for GPS-PID controller |
| uint8_t NaviGpsDLimit; // D limit for GPS-PID controller |
| uint8_t NaviGpsACC; // ACC gain for GPS-PID controller |
| uint8_t NaviGpsMinSat; // number of sattelites neccesary for GPS functions |
| uint8_t NaviStickThreshold; // activation threshild for detection of manual stick movements |
| uint8_t NaviWindCorrection; // streng of wind course correction |
| uint8_t NaviSpeedCompensation; // D gain fefore position hold login |
| uint8_t NaviOperatingRadius; // Radius limit in m around start position for GPS flights |
| uint8_t NaviAngleLimitation; // limitation of attitude angle controlled by the gps algorithm |
| uint8_t NaviPHLoginTime; // position hold logintimeout |
| uint8_t ExternalControl; // for serial Control |
| uint8_t BitConfig; // see upper defines for bitcoding |
| uint8_t ServoPitchCompInvert; // Value : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen |
| uint8_t Reserved[4]; |
| int8_t Name[12]; |
| uint8_t ChannelAssignment[8]; // see upper defines for details |
| uint8_t GlobalConfig; // see upper defines for bitcoding |
| uint8_t HeightMinGas; // Value : 0-100 |
| uint8_t HeightD; // Value : 0-250 |
| uint8_t MaxHeight; // Value : 0-32 |
| uint8_t HeightP; // Value : 0-32 |
| uint8_t Height_Gain; // Value : 0-50 |
| uint8_t Height_ACC_Effect; // Value : 0-250 |
| uint8_t StickP; // Value : 1-6 |
| uint8_t StickD; // Value : 0-64 |
| uint8_t StickYawP; // Value : 1-20 |
| uint8_t MinThrottle; // Value : 0-32 |
| uint8_t MaxThrottle; // Value : 33-250 |
| uint8_t GyroAccFactor; // Value : 1-64 |
| uint8_t CompassYawEffect; // Value : 0-32 |
| uint8_t GyroP; // Value : 10-250 |
| uint8_t GyroI; // Value : 0-250 |
| uint8_t GyroD; // Value : 0-250 |
| uint8_t LowVoltageWarning; // Value : 0-250 |
| uint8_t EmergencyGas; // Value : 0-250 //Gaswert bei Emp�ngsverlust |
| uint8_t EmergencyGasDuration; // Value : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen |
| uint8_t Unused0; // |
| uint8_t IFactor; // Value : 0-250 |
| uint8_t UserParams1[4]; // Value : 0-250 |
| /* |
| uint8_t UserParam2; // Value : 0-250 |
| 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 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 |
| uint8_t LoopThreshold; // Value: 0-250 Schwelle f�r Stickausschlag |
| uint8_t LoopHysteresis; // Value: 0-250 Hysterese f�r Stickausschlag |
| uint8_t AxisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
| uint8_t AxisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
| uint8_t AxisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
| uint8_t AngleTurnOverPitch; // Value: 0-250 180�-Punkt |
| uint8_t AngleTurnOverRoll; // Value: 0-250 180�-Punkt |
| uint8_t GyroAccTrim; // 1/k (Koppel_ACC_Wirkung) |
| uint8_t DriftComp; // limit for gyrodrift compensation |
| uint8_t DynamicStability; // PID limit for Attitude controller |
| uint8_t UserParams2[4]; // Value : 0-250 |
| /* |
| uint8_t UserParam6; // Value : 0-250 |
| uint8_t UserParam7; // Value : 0-250 |
| uint8_t UserParam8; // Value : 0-250 |
| */ |
| uint8_t J16Bitmask; // for the J16 Output |
| uint8_t J16Timing; // for the J16 Output |
| uint8_t J17Bitmask; // for the J17 Output |
| uint8_t J17Timing; // for the J17 Output |
| uint8_t NaviGpsModeControl; // Parameters for the Naviboard |
| uint8_t NaviGpsGain; // overall gain for GPS-PID controller |
| uint8_t NaviGpsP; // P gain for GPS-PID controller |
| uint8_t NaviGpsI; // I gain for GPS-PID controller |
| uint8_t NaviGpsD; // D gain for GPS-PID controller |
| uint8_t NaviGpsPLimit; // P limit for GPS-PID controller |
| uint8_t NaviGpsILimit; // I limit for GPS-PID controller |
| uint8_t NaviGpsDLimit; // D limit for GPS-PID controller |
| uint8_t NaviGpsACC; // ACC gain for GPS-PID controller |
| uint8_t NaviGpsMinSat; // number of sattelites neccesary for GPS functions |
| uint8_t NaviStickThreshold; // activation threshild for detection of manual stick movements |
| uint8_t NaviWindCorrection; // streng of wind course correction |
| uint8_t NaviSpeedCompensation; // D gain fefore position hold login |
| uint8_t NaviOperatingRadius; // Radius limit in m around start position for GPS flights |
| uint8_t NaviAngleLimitation; // limitation of attitude angle controlled by the gps algorithm |
| uint8_t NaviPHLoginTime; // position hold logintimeout |
| uint8_t ExternalControl; // for serial Control |
| uint8_t BitConfig; // see upper defines for bitcoding |
| uint8_t ServoPitchCompInvert; // Value : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen |
| uint8_t Reserved[4]; |
| int8_t Name[12]; |
| } paramset_t; |
| #define PARAMSET_STRUCT_LEN sizeof(paramset_t) |
| 134,9 → 134,9 |
| extern paramset_t staticParams; |
| typedef struct { |
| uint8_t Revision; |
| int8_t Name[12]; |
| int8_t Motor[16][4]; |
| uint8_t Revision; |
| int8_t Name[12]; |
| int8_t Motor[16][4]; |
| }__attribute__((packed)) MixerTable_t; |
| extern MixerTable_t Mixer; |
| 162,7 → 162,7 |
| #define CFG_ROTARY_RATE_LIMITER (1<<7) |
| // bit mask for staticParams.BitConfig |
| #define CFG_LOOP_UP (1<<0) |
| #define CFG_LOOP_UP (1<<0) |
| #define CFG_LOOP_DOWN (1<<1) |
| #define CFG_LOOP_LEFT (1<<2) |
| #define CFG_LOOP_RIGHT (1<<3) |
| 187,7 → 187,7 |
| #define MIX_YAW 3 |
| extern volatile uint8_t MKFlags; |
| extern int16_t variables[8]; |
| extern int16_t variables[8]; // The "Poti"s. |
| extern uint8_t BoardRelease; |
| extern uint8_t CPUType; |
| /branches/dongfang_FC_rewrite/eeprom.h |
|---|
| 17,6 → 17,7 |
| #define EEPROM_ADR_MIXER_TABLE 1000 // 1000 - 1076 |
| #define EEPARAM_REVISION 75 // is count up, if paramater stucture has changed (compatibility) |
| #define EEMIXER_REVISION 1 // is count up, if Mixer stucture has changed (compatibility) |
| extern void ParamSet_Init(void); |
| extern void ParamSet_ReadFromEEProm(uint8_t setnumber); |
| extern void ParamSet_WriteToEEProm(uint8_t setnumber); |
| /branches/dongfang_FC_rewrite/flight.c |
|---|
| 181,6 → 181,7 |
| // Fire the main flight attitude calculation, including integration of angles. |
| calculateFlightAttitude(); |
| J5HIGH; |
| throttleTerm = controlThrottle; |
| // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
| if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10; |
| 486,4 → 487,5 |
| DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
| */ |
| } |
| J5LOW; |
| } |
| /branches/dongfang_FC_rewrite/main.c |
|---|
| 78,237 → 78,233 |
| #include "eeprom.h" |
| int16_t main(void) { |
| uint16_t timer; |
| uint16_t timer; |
| // disable interrupts global |
| cli(); |
| // disable interrupts global |
| cli(); |
| // analyze hardware environment |
| CPUType = getCPUType(); |
| BoardRelease = getBoardRelease(); |
| // analyze hardware environment |
| CPUType = getCPUType(); |
| BoardRelease = getBoardRelease(); |
| // disable watchdog |
| MCUSR &= ~(1 << WDRF); |
| WDTCSR |= (1 << WDCE) | (1 << WDE); |
| WDTCSR = 0; |
| // disable watchdog |
| MCUSR &= ~(1 << WDRF); |
| WDTCSR |= (1 << WDCE) | (1 << WDE); |
| WDTCSR = 0; |
| // PPM_in[CH_THROTTLE] = 0; |
| // Why??? They are already initialized to 0. |
| // stickPitch = stickRoll = stickYaw = 0; |
| // PPM_in[CH_THROTTLE] = 0; |
| // Why??? They are already initialized to 0. |
| // stickPitch = stickRoll = stickYaw = 0; |
| RED_OFF; |
| RED_OFF; |
| // initalize modules |
| output_init(); |
| timer0_init(); |
| timer2_init(); |
| usart0_Init(); |
| if (CPUType == ATMEGA644P) |
| usart1_Init(); |
| RC_Init(); |
| analog_init(); |
| I2C_init(); |
| // initalize modules |
| output_init(); |
| timer0_init(); |
| timer2_init(); |
| usart0_Init(); |
| if (CPUType == ATMEGA644P) |
| usart1_Init(); |
| RC_Init(); |
| analog_init(); |
| I2C_init(); |
| #ifdef USE_NAVICTRL |
| SPI_MasterInit(); |
| SPI_MasterInit(); |
| #endif |
| #ifdef USE_MK3MAG |
| MK3MAG_Init(); |
| MK3MAG_Init(); |
| #endif |
| // enable interrupts global |
| sei(); |
| // enable interrupts global |
| sei(); |
| printf("\n\r==================================="); |
| printf("\n\rFlightControl"); |
| printf("\n\rHardware: Custom"); |
| printf("\r\n CPU: Atmega644"); |
| if (CPUType == ATMEGA644P) |
| printf("p"); |
| printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
| printf("\n\r==================================="); |
| printf("\n\r==================================="); |
| printf("\n\rFlightControl"); |
| printf("\n\rHardware: Custom"); |
| printf("\r\n CPU: Atmega644"); |
| if (CPUType == ATMEGA644P) |
| printf("p"); |
| printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
| printf("\n\r==================================="); |
| // Parameter Set handling |
| ParamSet_Init(); |
| // Parameter Set handling |
| ParamSet_Init(); |
| // Wait for a short time (otherwise the RC channel check won't work below) |
| // timer = SetDelay(500); |
| // while(!CheckDelay(timer)); |
| // Wait for a short time (otherwise the RC channel check won't work below) |
| // timer = SetDelay(500); |
| // while(!CheckDelay(timer)); |
| // Instead, while away the time by flashing the 2 outputs: |
| // First J16, then J17. Makes it easier to see which is which. |
| timer = SetDelay(250); |
| OUTPUT_SET(0,1); |
| GRN_OFF; |
| RED_ON; |
| while (!CheckDelay(timer)) |
| ; |
| // Instead, while away the time by flashing the 2 outputs: |
| // First J16, then J17. Makes it easier to see which is which. |
| timer = SetDelay(200); |
| OUTPUT_SET(0,1); |
| GRN_OFF; |
| RED_ON; |
| while (!CheckDelay(timer)) |
| ; |
| OUTPUT_SET(0,0); |
| timer = SetDelay(250); |
| while (!CheckDelay(timer)) |
| ; |
| timer = SetDelay(200); |
| OUTPUT_SET(0,0); |
| OUTPUT_SET(1,1); |
| RED_OFF; |
| GRN_ON; |
| while (!CheckDelay(timer)) |
| ; |
| timer = SetDelay(250); |
| OUTPUT_SET(1,1); |
| RED_OFF; |
| GRN_ON; |
| while (!CheckDelay(timer)) |
| ; |
| timer = SetDelay(200); |
| while (!CheckDelay(timer)) |
| ; |
| OUTPUT_SET(1,0); |
| timer = SetDelay(250); |
| while (!CheckDelay(timer)) |
| ; |
| OUTPUT_SET(1,0); |
| twi_diagnostics(); |
| twi_diagnostics(); |
| printf("\n\r==================================="); |
| printf("\n\r==================================="); |
| /* |
| if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) |
| { |
| printf("\n\rCalibrating air pressure sensor.."); |
| timer = SetDelay(1000); |
| SearchAirPressureOffset(); |
| while (!CheckDelay(timer)); |
| printf("OK\n\r"); |
| } |
| */ |
| /* |
| if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) |
| { |
| printf("\n\rCalibrating air pressure sensor.."); |
| timer = SetDelay(1000); |
| SearchAirPressureOffset(); |
| while (!CheckDelay(timer)); |
| printf("OK\n\r"); |
| } |
| */ |
| #ifdef USE_NAVICTRL |
| printf("\n\rSupport for NaviCtrl"); |
| printf("\n\rSupport for NaviCtrl"); |
| #ifdef USE_RC_DSL |
| printf("\r\nSupport for DSL RC at 2nd UART"); |
| printf("\r\nSupport for DSL RC at 2nd UART"); |
| #endif |
| #ifdef USE_RC_SPECTRUM |
| printf("\r\nSupport for SPECTRUM RC at 2nd UART"); |
| printf("\r\nSupport for SPECTRUM RC at 2nd UART"); |
| #endif |
| #endif |
| #ifdef USE_MK3MAG |
| printf("\n\rSupport for MK3MAG Compass"); |
| printf("\n\rSupport for MK3MAG Compass"); |
| #endif |
| #if (defined (USE_MK3MAG)) |
| if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART"); |
| else printf("\n\rSupport for GPS at 1st UART"); |
| if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART"); |
| else printf("\n\rSupport for GPS at 1st UART"); |
| #endif |
| controlMixer_setNeutral(); |
| controlMixer_setNeutral(); |
| // Cal. attitude sensors and reset integrals. |
| attitude_setNeutral(); |
| // Cal. attitude sensors and reset integrals. |
| attitude_setNeutral(); |
| Servo_On(); |
| Servo_On(); |
| // Init flight parameters |
| flight_setNeutral(); |
| // Init flight parameters |
| flight_setNeutral(); |
| // RED_OFF; |
| // RED_OFF; |
| beep(2000); |
| beep(2000); |
| printf("\n\rControl: "); |
| if (staticParams.GlobalConfig & CFG_HEADING_HOLD) |
| printf("HeadingHold"); |
| else printf("Neutral (ACC-Mode)"); |
| printf("\n\rControl: "); |
| if (staticParams.GlobalConfig & CFG_HEADING_HOLD) |
| printf("HeadingHold"); |
| else printf("Neutral (ACC-Mode)"); |
| printf("\n\n\r"); |
| printf("\n\n\r"); |
| LCD_Clear(); |
| LCD_Clear(); |
| I2CTimeout = 5000; |
| I2CTimeout = 5000; |
| while (1) { |
| if (runFlightControl && analogDataReady) { // control interval |
| runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
| while (1) { |
| if (runFlightControl && analogDataReady) { // control interval |
| runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
| J4HIGH; |
| flight_control(); |
| J4LOW; |
| //J4HIGH; |
| flight_control(); |
| //J4LOW; |
| /* |
| * If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit |
| * the throttle vector just computed. Otherwise, if motor test is engaged, transmit |
| * the test throttle vector. If no testing, stop all motors. |
| */ |
| // Obsoleted. |
| // transmitMotorThrottleData(); |
| /* |
| * If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit |
| * the throttle vector just computed. Otherwise, if motor test is engaged, transmit |
| * the test throttle vector. If no testing, stop all motors. |
| */ |
| // Obsoleted. |
| // transmitMotorThrottleData(); |
| RED_OFF; |
| RED_OFF; |
| /* |
| Does not belong here. Instead, external control should be ignored in |
| controlMixer if there was no new data from there for some time. |
| if(externalControlActive) externalControlActive--; |
| else { |
| externalControl.config = 0; |
| externalStickPitch = 0; |
| externalStickRoll = 0; |
| externalStickYaw = 0; |
| } |
| */ |
| /* |
| Does not belong here. Instead, external control should be ignored in |
| controlMixer if there was no new data from there for some time. |
| if(externalControlActive) externalControlActive--; |
| else { |
| externalControl.config = 0; |
| externalStickPitch = 0; |
| externalStickRoll = 0; |
| externalStickYaw = 0; |
| } |
| */ |
| /* |
| Does not belong here. |
| if(RC_Quality) RC_Quality--; |
| */ |
| /* |
| Does not belong here. |
| if(RC_Quality) RC_Quality--; |
| */ |
| /* Does not belong here. Well since we are not supporting navi right now anyway, leave out. |
| #ifdef USE_NAVICTRL |
| if(NCDataOkay) { |
| if(--NCDataOkay == 0) // no data from NC |
| { // set gps control sticks neutral |
| GPSStickPitch = 0; |
| GPSStickRoll = 0; |
| NCSerialDataOkay = 0; |
| } |
| } |
| #endif |
| */ |
| if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout |
| RED_ON; |
| if (!I2CTimeout) { |
| I2C_Reset(); |
| I2CTimeout = 5; |
| } |
| } else { |
| RED_OFF; |
| } |
| /* Does not belong here. Well since we are not supporting navi right now anyway, leave out. |
| #ifdef USE_NAVICTRL |
| if(NCDataOkay) { |
| if(--NCDataOkay == 0) // no data from NC |
| { // set gps control sticks neutral |
| GPSStickPitch = 0; |
| GPSStickRoll = 0; |
| NCSerialDataOkay = 0; |
| } |
| } |
| #endif |
| */ |
| if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout |
| RED_ON; |
| if (!I2CTimeout) { |
| I2C_Reset(); |
| I2CTimeout = 5; |
| } |
| } else { |
| RED_OFF; |
| } |
| // Allow Serial Data Transmit if motors must not updated or motors are not running |
| if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
| usart0_TransmitTxData(); |
| } |
| // Allow Serial Data Transmit if motors must not updated or motors are not running |
| if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
| usart0_TransmitTxData(); |
| } |
| usart0_ProcessRxData(); |
| usart0_ProcessRxData(); |
| if (CheckDelay(timer)) { |
| if (UBat <= UBAT_AT_5V) { |
| // Do nothing. The voltage on the input side of the regulator is <5V; |
| // we must be running off USB power. Keep it quiet. |
| } else if (UBat < staticParams.LowVoltageWarning) { |
| beepBatteryAlarm(); |
| } |
| if (CheckDelay(timer)) { |
| if (UBat <= UBAT_AT_5V) { |
| // Do nothing. The voltage on the input side of the regulator is <5V; |
| // we must be running off USB power. Keep it quiet. |
| } else if (UBat < staticParams.LowVoltageWarning) { |
| beepBatteryAlarm(); |
| } |
| #ifdef USE_NAVICTRL |
| SPI_StartTransmitPacket(); |
| SendSPI = 4; |
| SPI_StartTransmitPacket(); |
| SendSPI = 4; |
| #endif |
| timer = SetDelay(20); // every 20 ms |
| } |
| output_update(); |
| } |
| timer = SetDelay(20); // every 20 ms |
| } |
| output_update(); |
| } |
| #ifdef USE_NAVICTRL |
| if(!SendSPI) { |
| // SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
| // within the SPI_TransmitByte() routine the value is set to 4. |
| // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |
| // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms. |
| SPI_TransmitByte(); |
| } |
| if(!SendSPI) { |
| // SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
| // within the SPI_TransmitByte() routine the value is set to 4. |
| // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |
| // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms. |
| SPI_TransmitByte(); |
| } |
| #endif |
| } |
| return (1); |
| } |
| return (1); |
| } |
| /branches/dongfang_FC_rewrite/output.h |
|---|
| 3,6 → 3,18 |
| #include <avr/io.h> |
| #define J3HIGH PORTD |= (1<<PORTD5) |
| #define J3LOW PORTD &= ~(1<<PORTD5) |
| #define J3TOGGLE PORTD ^= (1<<PORTD5) |
| #define J4HIGH PORTD |= (1<<PORTD4) |
| #define J4LOW PORTD &= ~(1<<PORTD4) |
| #define J4TOGGLE PORTD ^= (1<<PORTD4) |
| #define J5HIGH PORTD |= (1<<PORTD3) |
| #define J5LOW PORTD &= ~(1<<PORTD3) |
| #define J5TOGGLE PORTD ^= (1<<PORTD3) |
| // invert means: An "1" bit in digital debug data make a LOW on the output. |
| #define DIGITAL_DEBUG_INVERT 1 |
| 46,7 → 58,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/rc.h |
|---|
| 3,18 → 3,6 |
| #include <inttypes.h> |
| #define J3HIGH PORTD |= (1<<PORTD5) |
| #define J3LOW PORTD &= ~(1<<PORTD5) |
| #define J3TOGGLE PORTD ^= (1<<PORTD5) |
| #define J4HIGH PORTD |= (1<<PORTD4) |
| #define J4LOW PORTD &= ~(1<<PORTD4) |
| #define J4TOGGLE PORTD ^= (1<<PORTD4) |
| #define J5HIGH PORTD |= (1<<PORTD3) |
| #define J5LOW PORTD &= ~(1<<PORTD3) |
| #define J5TOGGLE PORTD ^= (1<<PORTD3) |
| #define MAX_CHANNELS 10 |
| // Number of cycles a command must be repeated before commit. |
| /branches/dongfang_FC_rewrite/uart0.c |
|---|
| 61,11 → 61,12 |
| #include "menu.h" |
| #include "timer0.h" |
| #include "uart0.h" |
| #include "attitude.h" |
| #include "rc.h" |
| #include "externalControl.h" |
| #include "output.h" |
| #include "attitude.h" |
| #ifdef USE_MK3MAG |
| #include "mk3mag.h" |
| #endif |
| 133,20 → 134,26 |
| "GyroYaw(AC) ", |
| "AccPitch (angle)", |
| "AccRoll (angle) ", //10 |
| "UBat ", |
| "HIRES_GYRO_INTEG", |
| "Pitch Term ", |
| "Roll Term ", |
| "Yaw Term ", |
| "Throttle Term ", //15 |
| "0th O Corr pitch", "0th O Corr roll ", |
| "0th O Corr pitch", |
| "0th O Corr roll ", |
| "DriftCompDelta P", |
| "DriftCompDelta R", |
| "ADPitchGyroOffs ", //20 |
| "ADRollGyroOffs ", "M1 ", "M2 ", |
| "ADRollGyroOffs ", |
| "M1 ", |
| "M2 ", |
| "M3 ", |
| "M4 ", //25 |
| "ControlYaw ", "Airpress. Range ", "DriftCompPitch ", |
| "DriftCompRoll ", "AirpressFiltered", //30 |
| "ControlYaw ", |
| "Airpress. Range ", |
| "DriftCompPitch ", |
| "DriftCompRoll ", |
| "AirpressFiltered", //30 |
| "AirpressADC " }; |
| /****************************************************************/ |