/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 " }; |
/****************************************************************/ |