/branches/dongfang_FC_fixedwing/arduino_atmega328/README.txt |
---|
5,7 → 5,7 |
PB0/ICP1 PPM receiver signal in |
PB1,2 Digital outputs (output.c) |
PB3 SPI use |
PB3 SPI use and CLK of 4017 |
PB4 SPI use and red LED (LED1) |
PB5 Green Arduino LED (use as LED2). |
PB6,7 Xtal |
19,8 → 19,8 |
PD0 RxD serial for configuration, diags etc. |
PD1 TxD serial for configuration, diags etc. |
PD2 Performance analysis |
PD3/OC2B: CLK of 4017 change B3 |
PD4: RESET of 4017 |
PD3 RESET of 4017 |
PD4 unused |
PD5 Beeper |
PD6 unused |
PD7 unused |
/branches/dongfang_FC_fixedwing/arduino_atmega328/analog.c |
---|
185,10 → 185,18 |
} |
/* |
* Here the axes of the sensor can be shuffled around. |
* The sensor is installed vertically and the axes are moved around a little... |
*/ |
int16_t rawGyroValue(uint8_t axis) { |
return ITG3200SensorInputs[axis+1]; // skip temperature mesaurement in any case.. |
switch(axis) { |
case PITCH: |
return ITG3200SensorInputs[3]; |
case ROLL: |
return ITG3200SensorInputs[1]; |
case YAW: |
return ITG3200SensorInputs[2]; |
} |
return 0; |
} |
/* |
/branches/dongfang_FC_fixedwing/arduino_atmega328/attitude.h |
---|
62,8 → 62,8 |
*/ |
//#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
#define OVER180 ((int32_t)GYRO_DEG_FACTOR * 180) |
#define OVER360 ((int32_t)GYRO_DEG_FACTOR * 360) |
#define OVER180 ((int32_t)GYRO_DEG_FACTOR * 180L) |
#define OVER360 ((int32_t)GYRO_DEG_FACTOR * 360L) |
/* |
* Rotation rates |
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.c |
---|
90,7 → 90,7 |
// Servos |
staticParams.servoCount = 7; |
staticParams.servosReverse = CONTROL_SERVO_REVERSE_ELEVATOR | CONTROL_SERVO_REVERSE_RUDDER; |
staticParams.servosReverse = CONTROL_SERVO_REVERSE_AILERONS; |
staticParams.gimbalServoMaxManualSpeed = 10; |
for (uint8_t i=0; i<2; i++) { |
146,8 → 146,8 |
IMUConfig.gyroDFilterConstant = 1; |
IMUConfig.rateTolerance = 120; |
IMUConfig.gyroDWindowLength = 3; |
IMUConfig.gyroQuadrant = 0; |
IMUConfig.imuReversedFlags = 0; |
IMUConfig.gyroQuadrant = 4; |
IMUConfig.imuReversedFlags = IMU_REVERSE_GYRO_PR; |
IMUConfig.gyroCalibrationTweak[0] = |
IMUConfig.gyroCalibrationTweak[1] = |
IMUConfig.gyroCalibrationTweak[2] = 0; |
158,8 → 158,8 |
/***************************************************/ |
void channelMap_default(void) { |
channelMap.RCPolarity = 1; |
channelMap.HWTrim = 192; |
channelMap.variableOffset = 128; |
channelMap.HWTrim = 166; |
channelMap.variableOffset = 131; |
channelMap.channels[CH_ELEVATOR] = 1; |
channelMap.channels[CH_AILERONS] = 0; |
channelMap.channels[CH_THROTTLE] = 2; |
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.h |
---|
204,12 → 204,12 |
#define SYSCLK F_CPU |
// Not really a part of configuration, but LEDs and HW s test are the same. |
#define RED_OFF {PORTD &=~(1<<PORTD6);} |
#define RED_ON {PORTD |= (1<<PORTD6);} |
#define RED_FLASH PORTD ^= (1<<PORTD6) |
#define GRN_OFF {PORTD &=~(1<<PORTD7);} |
#define GRN_ON {PORTD |= (1<<PORTD7);} |
#define GRN_FLASH PORTD ^= (1<<PORTD7) |
#define RED_OFF {PORTB &=~(1<<PORTB4);} |
#define RED_ON {PORTB |= (1<<PORTB4);} |
#define RED_FLASH PORTB ^= (1<<PORTB4) |
#define GRN_OFF {PORTB &=~(1<<PORTB5);} |
#define GRN_ON {PORTB |= (1<<PORTB5);} |
#define GRN_FLASH PORTB ^= (1<<PORTB5) |
// Mixer table |
#define MIX_THROTTLE 0 |
/branches/dongfang_FC_fixedwing/arduino_atmega328/flight.c |
---|
22,7 → 22,6 |
/* |
* Error integrals. |
*/ |
int32_t error[3]; |
uint8_t reverse[3]; |
int32_t maxError[3]; |
45,7 → 44,8 |
void flight_updateFlightParametersToFlightMode(void) { |
debugOut.analog[16] = currentFlightMode; |
reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
78,9 → 78,9 |
} |
#define LOG_STICK_SCALE 8 |
#define LOG_P_SCALE 6 |
#define LOG_I_SCALE 10 |
#define LOG_D_SCALE 6 |
#define LOG_P_SCALE 8 |
#define LOG_I_SCALE 12 |
#define LOG_D_SCALE 8 |
/************************************************************************/ |
/* Main Flight Control */ |
122,58 → 122,61 |
target[axis] += OVER360; |
} |
error[axis] = attitude[axis] - target[axis]; |
int32_t error = attitude[axis] - target[axis]; |
#define ROTATETARGET 1 |
// #define TRUNCATEERROR 1 |
#ifdef ROTATETARGET |
if(abs(error[axis]) > OVER180) { |
//if(abs(error) > OVER180) { // doesnt work!!! |
if(error > OVER180 || error < -OVER180) { |
// The shortest way from attitude to target crosses -180. |
// Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative. |
// Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive. |
if (error[axis] > 0) { |
if (error[axis] < OVER360 - maxError[axis]) { |
if (error > 0) { |
if (error < OVER360 - maxError[axis]) { |
// too much err. |
error[axis] = -maxError[axis]; |
error = -maxError[axis]; |
target[axis] = attitude[axis] + maxError[axis]; |
if (target[axis] > OVER180) target[axis] -= OVER360; |
} else { |
// Normal case, we just need to correct for the wrap. Error will be negative. |
error[axis] -= OVER360; |
error -= OVER360; |
} |
} else { |
if (error[axis] > maxError[axis] - OVER360) { |
if (error > maxError[axis] - OVER360) { |
// too much err. |
error[axis] = maxError[axis]; |
error = maxError[axis]; |
target[axis] = attitude[axis] - maxError[axis]; |
if (target[axis] < -OVER180) target[axis] += OVER360; |
} else { |
// Normal case, we just need to correct for the wrap. Error will be negative. |
error[axis] += OVER360; |
error += OVER360; |
} |
} |
} else { |
// Simple case, linear range. |
if (error[axis] > maxError[axis]) { |
error[axis] = maxError[axis]; |
if (error > maxError[axis]) { |
error = maxError[axis]; |
target[axis] = attitude[axis] - maxError[axis]; |
} else if (error[axis] < -maxError[axis]) { |
error[axis] = -maxError[axis]; |
} else if (error < -maxError[axis]) { |
error = -maxError[axis]; |
target[axis] = attitude[axis] + maxError[axis]; |
} |
} |
#endif |
#ifdef TUNCATEERROR |
if (error[axis] > maxError[axis]) { |
error[axis] = maxError[axis]; |
} else if (error[axis] < -maxError[axis]) { |
error[axis] = -maxError[axis]; |
if (error > maxError[axis]) { |
error = maxError[axis]; |
} else if (error < -maxError[axis]) { |
error = -maxError[axis]; |
} else { |
// update I parts here for angles mode. I parts in rate mode is something different. |
} |
#endif |
debugOut.analog[9+axis] = error / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
/************************************************************************/ |
/* Calculate control feedback from angle (gyro integral) */ |
/* and angular velocity (gyro signal) */ |
189,7 → 192,7 |
} |
if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
PDPart[axis] += anglePart; |
} |
200,11 → 203,6 |
term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
} |
debugOut.analog[12] = term[PITCH]; |
debugOut.analog[13] = term[ROLL]; |
debugOut.analog[14] = term[YAW]; |
debugOut.analog[15] = term[THROTTLE]; |
for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) { |
int16_t tmp; |
if (servoTestActive) { |
251,14 → 249,11 |
debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
debugOut.analog[12] = term[PITCH]; |
debugOut.analog[13] = term[ROLL]; |
debugOut.analog[14] = term[YAW]; |
debugOut.analog[15] = term[THROTTLE]; |
debugOut.analog[12] = term[PITCH]; |
debugOut.analog[13] = term[ROLL]; |
debugOut.analog[14] = term[YAW]; |
//DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
//DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
//debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg |
/branches/dongfang_FC_fixedwing/arduino_atmega328/main.c |
---|
34,6 → 34,10 |
static uint8_t profileTimer; |
#endif |
// Set up LED output. This is here and not in configuration.c because the setBoardRelease function where it used to reside no longer exists. |
DDRB |= ((1<<DDB4)|(1<<DDB5)); |
PORTB &= ~((1<<DDB4)|(1<<DDB5)); |
// disable interrupts global |
cli(); |
/branches/dongfang_FC_fixedwing/arduino_atmega328/main.h |
---|
0,0 → 1,6 |
#ifndef _MAIN_H |
#define _MAIN_H |
void reset(void); |
#endif //_MAIN_H |
/branches/dongfang_FC_fixedwing/arduino_atmega328/makefile |
---|
15,7 → 15,7 |
GYRO=IMU3200 |
GYRO_HW_NAME=IMU3200 |
GYRO_HW_FACTOR=1.45f |
GYRO_HW_FACTOR=3.0f |
GYRO_CORRECTION=1.0f |
#------------------------------------------------------------------- |
/branches/dongfang_FC_fixedwing/arduino_atmega328/output.c |
---|
8,7 → 8,7 |
void output_init(void) { |
// set PC2 & PC3 as output (control of J16 & J17) |
DDRB |= (1 << DDB4) | (1 << DDB5); |
DDRB |= (1 << DDB1) | (1 << DDB2); |
outputSet(0,0); |
outputSet(1,0); |
flashCnt[0] = flashCnt[1] = 0; |
/branches/dongfang_FC_fixedwing/arduino_atmega328/output.h |
---|
10,9 → 10,9 |
// invert means: An "1" bit in digital debug data make a LOW on the output. |
#define DIGITAL_DEBUG_INVERT 0 |
#define OUTPUT_HIGH(num) {PORTB |= ((1<<4) << (num));} |
#define OUTPUT_LOW(num) {PORTB &= ~((1<<4) << (num));} |
#define OUTPUT_TOGGLE(num) ( {PORTB ^= ((1<<4) << (num));} |
#define OUTPUT_HIGH(num) {PORTB |= ((1<<1) << (num));} |
#define OUTPUT_LOW(num) {PORTB &= ~((1<<1) << (num));} |
#define OUTPUT_TOGGLE(num) ( {PORTB ^= ((1<<1) << (num));} |
/* |
* Some digital debugs. A digital debug is 2 signals on the 2 LED outputs, |
/branches/dongfang_FC_fixedwing/arduino_atmega328/rc.c |
---|
163,6 → 163,11 |
PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) - rcTrim.trim[CH_AILERONS]; |
PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) - rcTrim.trim[CH_RUDDER]; |
PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); // no trim on throttle! |
debugOut.analog[20] = PRYT[CONTROL_ELEVATOR]; |
debugOut.analog[21] = PRYT[CONTROL_AILERONS]; |
debugOut.analog[22] = PRYT[CONTROL_RUDDER]; |
debugOut.analog[23] = PRYT[CONTROL_THROTTLE]; |
} |
/* |
169,9 → 174,12 |
* Get other channel value |
*/ |
int16_t RC_getVariable(uint8_t varNum) { |
if (varNum < 4) |
if (varNum < 4) { |
// 0th variable is 5th channel (1-based) etc. |
return (RCChannel(varNum + CH_POTS) >> 3) + channelMap.variableOffset; |
int16_t result = (RCChannel(varNum + CH_POTS) / 6) + channelMap.variableOffset; |
if (varNum<2) debugOut.analog[18+varNum] = result; |
return result; |
} |
/* |
* Let's just say: |
* The RC variable i is hardwired to channel i, i>=4 |
/branches/dongfang_FC_fixedwing/arduino_atmega328/timer0.c |
---|
144,7 → 144,6 |
t_stop = setDelay(w); |
while (!checkDelay(t_stop)) { |
wdt_reset(); |
debugOut.analog[30]++; |
if (sensorDataReady == ALL_DATA_READY) { |
analog_update(); |
startAnalogConversionCycle(); |
153,7 → 152,7 |
if (stop) { |
// Wait for new samples to get prepared but do not restart AD conversion after that! |
// Caller MUST to that. |
while (!sensorDataReady != ALL_DATA_READY) wdt_reset(); |
// while (sensorDataReady != ALL_DATA_READY) wdt_reset(); |
} |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/timer2.c |
---|
9,21 → 9,21 |
// #define COARSERESOLUTION 1 |
#ifdef COARSERESOLUTION |
#define NEUTRAL_PULSELENGTH 938 |
#define NEUTRAL_PULSELENGTH ((int16_t)(F_CPU/32000*1.5f + 0.5f)) |
#define STABILIZATION_LOG_DIVIDER 6 |
#define SERVOLIMIT 500 |
#define SERVOLIMIT ((int16_t)(F_CPU/32000*0.8f + 0.5f)) |
#define SCALE_FACTOR 4 |
#define CS2 ((1<<CS21)|(1<<CS20)) |
#else |
#define NEUTRAL_PULSELENGTH 3750 |
#define NEUTRAL_PULSELENGTH ((int16_t)(F_CPU/8000.0f * 1.5f + 0.5f)) |
#define STABILIZATION_LOG_DIVIDER 4 |
#define SERVOLIMIT 2000 |
#define SERVOLIMIT ((int16_t)(F_CPU/8000.0f * 0.8f + 0.5f)) |
#define SCALE_FACTOR 16 |
#define CS2 (1<<CS21) |
#endif |
#define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128) |
#define FRAMELENGTH ((uint16_t)(NEUTRAL_PULSELENGTH + SERVOLIMIT) * (uint16_t)staticParams.servoCount + 128) |
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
43,7 → 43,7 |
// disable all interrupts before reconfiguration |
cli(); |
// set PD7 as output of the PWM for pitch servo |
// set PD7 as output of the 4017 clk |
DDRB |= (1 << DDB3); |
PORTB &= ~(1 << PORTB3); // set PD7 to low |
169,7 → 169,7 |
servoIndex++; |
} else { |
// There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
remainingPulseTime = FRAMELENGTH - sumOfPulseTimes; |
sumOfPulseTimes = servoIndex = 0; |
recalculateServoTimes = 1; |
HEF4017R_ON; |
/branches/dongfang_FC_fixedwing/arduino_atmega328/uart0.c |
---|
96,8 → 96,8 |
"Error Y ", |
"Term P ", |
"Term R ", |
"Throttle ", |
"Term Y ", //15 |
"Term Y ", |
"Throttle ", //15 |
"Flight mode ", |
"Ultralow thr. ", |
"Var0 ", |
104,8 → 104,8 |
"Var1 ", |
"RC P ", //20 |
"RC R ", |
"RC Y ", |
"RC T ", |
"RC Y ", |
"Servo P ", |
"Servo R ", //25 |
"Servo T ", |
112,8 → 112,8 |
"Servo Y ", |
"I2C cycles ", |
"I2C restarts ", |
" ", //30 |
" " |
"case0 ", //30 |
"case1 " |
}; |
/****************************************************************/ |