/branches/V0.68d Code Redesign killagreg/analog.c |
---|
15,17 → 15,15 |
#include "printf_P.h" |
#include "eeprom.h" |
volatile int16_t Current_Pitch = 0, Current_Roll = 0, Current_Yaw = 0; |
volatile int16_t Current_AccX = 0, Current_AccY = 0, Current_AccZ = 0; |
volatile int16_t Current_AccZ = 0; |
volatile int16_t UBat = 100; |
volatile int16_t AdValueGyrPitch = 0, AdValueGyrRoll = 0, AdValueGyrYaw = 0; |
volatile int16_t AdValueAccRoll = 0, AdValueAccPitch = 0, AdValueAccTop = 0; |
volatile int16_t AdValueGyrPitch = 0, AdValueGyrRoll = 0, AdValueGyrYaw = 0; |
volatile int16_t AdValueAccRoll = 0, AdValueAccPitch = 0, AdValueAccTop = 0; |
volatile int32_t AirPressure = 32000; |
volatile int16_t StartAirPressure; |
volatile uint16_t ReadingAirPressure = 1023; |
uint8_t PressureSensorOffset; |
volatile int16_t HightD = 0; |
volatile int16_t tmpAirPressure; |
volatile uint16_t MeasurementCounter = 0; |
/*****************************************************/ |
60,22 → 58,22 |
void SearchAirPressureOffset(void) |
{ |
uint8_t off; |
off = GetParamByte(PID_LAST_OFFSET); |
if(off > 20) off -= 10; |
OCR0A = off; |
Delay_ms_Mess(100); |
if(ReadingAirPressure < 850) off = 0; |
for(; off < 250;off++) |
{ |
OCR0A = off; |
Delay_ms_Mess(50); |
printf("."); |
if(ReadingAirPressure < 900) break; |
} |
SetParamByte(PID_LAST_OFFSET, off); |
PressureSensorOffset = off; |
Delay_ms_Mess(300); |
uint8_t off; |
off = GetParamByte(PID_PRESSURE_OFFSET); |
if(off > 20) off -= 10; |
OCR0A = off; |
Delay_ms_Mess(100); |
if(ReadingAirPressure < 850) off = 0; |
for(; off < 250;off++) |
{ |
OCR0A = off; |
Delay_ms_Mess(50); |
printf("."); |
if(ReadingAirPressure < 900) break; |
} |
SetParamByte(PID_PRESSURE_OFFSET, off); |
PressureSensorOffset = off; |
Delay_ms_Mess(300); |
} |
82,11 → 80,26 |
/*****************************************************/ |
/* Interrupt Service Routine for ADC */ |
/*****************************************************/ |
// The routine changes the ADC input muxer running |
// thru the state machine by the following order. |
// state 0: ch0 (yaw gyro) |
// state 1: ch1 (roll gyro) |
// state 2: ch2 (pitch gyro) |
// state 3: ch4 (battery voltage -> UBat) |
// state 4: ch6 (acc y -> Current_AccY) |
// state 5: ch7 (acc x -> Current_AccX) |
// state 6: ch0 (yaw gyro average with first reading -> AdValueGyrYaw) |
// state 7: ch1 (roll gyro average with first reading -> AdValueGyrRoll) |
// state 8: ch2 (pitch gyro average with first reading -> AdValueGyrPitch) |
// state 9: ch5 (acc z add also 4th part of acc x and acc y to reading) |
// state10: ch3 (air pressure averaging over 5 single readings -> tmpAirPressure) |
ISR(ADC_vect) |
{ |
static uint8_t adc_channel = 0, state = 0; |
static uint16_t yaw1, roll1, pitch1; |
static uint8_t average_pressure = 0; |
static int16_t tmpAirPressure = 0; |
// disable further AD conversion |
ADC_Disable(); |
// state machine |
111,44 → 124,42 |
adc_channel = 6; // set next channel to ADC6 = ACC_Y |
break; |
case 4: |
Current_AccY = NeutralAccY - ADC; // get acceleration in Y direction |
AdValueAccRoll = Current_AccY; |
AdValueAccRoll = NeutralAccY - ADC; // get acceleration in Y direction |
adc_channel = 7; // set next channel to ADC7 = ACC_X |
break; |
case 5: |
Current_AccX = ADC - NeutralAccX; // get acceleration in X direction |
AdValueAccPitch = Current_AccX; |
AdValueAccPitch = ADC - NeutralAccX; // get acceleration in X direction |
adc_channel = 0; // set next channel to ADC7 = YAW GYRO |
break; |
case 6: |
// average over two samples to create current AdValueGyrYaw |
if(BoardRelease == 10) AdValueGyrYaw = (ADC + yaw1) / 2; |
else AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1 |
else AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1 |
adc_channel = 1; // set next channel to ADC7 = ROLL GYRO |
break; |
case 7: |
// average over two samples to create current ADValueGyrRoll |
if(BoardRelease == 10) AdValueGyrRoll = (ADC + roll1) / 2; |
else AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1 |
else AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1 |
adc_channel = 2; // set next channel to ADC2 = PITCH GYRO |
break; |
case 8: |
// average over two samples to create current ADValuePitch |
if(BoardRelease == 10) AdValueGyrPitch = (ADC + pitch1) / 2; |
else AdValueGyrPitch = ADC + pitch1; // gain is 2 times lower on FC 1.1 |
else AdValueGyrPitch = ADC + pitch1; // gain is 2 times lower on FC 1.1 |
adc_channel = 5; // set next channel to ADC5 = ACC_Z |
break; |
case 9: |
// get z acceleration |
AdValueAccTop = (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction |
AdValueAccTop += abs(Current_AccY) / 4 + abs(Current_AccX) / 4; |
AdValueAccTop += abs(AdValueAccPitch) / 4 + abs(AdValueAccRoll) / 4; |
if(AdValueAccTop > 1) |
{ |
if(NeutralAccZ < 800) NeutralAccZ+= 0.02; |
if(NeutralAccZ < 800) NeutralAccZ+= 0.02; |
} |
else if(AdValueAccTop < -1) |
{ |
if(NeutralAccZ > 600) NeutralAccZ-= 0.02; |
if(NeutralAccZ > 600) NeutralAccZ-= 0.02; |
} |
Current_AccZ = ADC; |
Reading_Integral_Top += AdValueAccTop; // Integrieren |
158,16 → 169,16 |
case 10: |
tmpAirPressure += ADC; // sum vadc values |
if(++average_pressure >= 5) // if 5 values are summerized for averaging |
{ |
{ |
ReadingAirPressure = ADC; // update measured air pressure |
average_pressure = 0; // reset air pressure measurement counter |
HightD = (int16_t)(StartAirPressure - tmpAirPressure - ReadingHight); // D-Anteil = neuerWert - AlterWert |
AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history |
ReadingHight = StartAirPressure - AirPressure; |
average_pressure = 0; // reset air pressure measurement counter |
tmpAirPressure = 0; |
} |
} |
adc_channel = 0; // set next channel to ADC0 = GIER GYRO |
state = 0; // reset state |
state = 0; // reset state machine |
break; |
default: |
adc_channel = 0; |
176,6 → 187,6 |
} |
// set adc muxer to next adc_channel |
ADMUX = (ADMUX & 0xE0) | adc_channel; |
// ?? |
// after full cycle stop further interrupts |
if(state != 0) ADC_Enable(); |
} |
/branches/V0.68d Code Redesign killagreg/analog.h |
---|
6,7 → 6,6 |
extern volatile int16_t UBat; |
extern volatile int16_t AdValueGyrPitch, AdValueGyrRoll, AdValueGyrYaw; |
extern volatile int16_t AdValueAccRoll, AdValueAccPitch, AdValueAccTop; |
extern volatile int16_t Current_Pitch, Current_Roll, Current_Yaw; |
extern volatile int16_t Current_AccX, Current_AccY, Current_AccZ; |
extern volatile int32_t AirPressure; |
extern volatile uint16_t MeasurementCounter; |
/branches/V0.68d Code Redesign killagreg/eeprom.h |
---|
6,7 → 6,7 |
#define EEPROM_ADR_PARAM_BEGIN 0 |
#define PID_VERSION 1 // byte |
#define PID_ACTIVE_SET 2 // byte |
#define PID_LAST_OFFSET 3 // byte |
#define PID_PRESSURE_OFFSET 3 // byte |
#define PID_ACC_PITCH 4 // word |
#define PID_ACC_ROLL 6 // word |
#define PID_ACC_Z 8 // word |
/branches/V0.68d Code Redesign killagreg/fc.c |
---|
66,7 → 66,7 |
#include "twimaster.h" |
#include "mm3.h" |
volatile unsigned int I2CTimeout = 100; |
volatile uint16_t I2CTimeout = 100; |
// gyro readings |
volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw; |
// gyro neutral readings |
90,7 → 90,7 |
volatile int32_t MeanIntegralRoll; |
// attitude acceleration integrals |
volatile int32_t IntegralAccPitch = 0, IntegralAccRoll = 0, IntegralAccZ = 0; |
volatile int32_t IntegralAccPitch = 0, IntegralAccRoll = 0; |
volatile int32_t Reading_Integral_Top = 0; |
// compass course |
177,7 → 177,7 |
AdNeutralYaw = AdValueGyrYaw; |
StartNeutralRoll = AdNeutralRoll; |
StartNeutralPitch = AdNeutralPitch; |
if(GetParamByte(PID_ACC_PITCH) > 4) |
if(GetParamWord(PID_ACC_PITCH) > 1023) |
{ |
NeutralAccY = abs(Mean_AccRoll) / ACC_AMPLIFY; |
NeutralAccX = abs(Mean_AccPitch) / ACC_AMPLIFY; |
232,7 → 232,6 |
// sum sensor readings for later averaging |
IntegralAccPitch += ACC_AMPLIFY * AdValueAccPitch; |
IntegralAccRoll += ACC_AMPLIFY * AdValueAccRoll; |
IntegralAccZ += Current_AccZ - NeutralAccZ; |
// Yaw |
// calculate yaw gyro intergral (~ to rotation angle) |
350,9 → 349,10 |
Mean_AccPitch = ACC_AMPLIFY * (int32_t)AdValueAccPitch; |
Mean_AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll; |
Mean_AccTop = (int32_t)AdValueAccTop; |
// start ADC |
// start ADC (enables internal trigger so that the ISR in analog.c |
// updates the readings once) |
ADC_Enable(); |
//update poti values by rc-signals (why not +127?) |
//update poti values by rc-signals |
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--; |
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--; |
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--; |
364,7 → 364,7 |
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
} |
/************************************************************************/ |
379,8 → 379,8 |
Motor_Right = 0; |
Motor_Left = 0; |
if(MotorTest[0]) Motor_Front = MotorTest[0]; |
if(MotorTest[1]) Motor_Rear = MotorTest[1]; |
if(MotorTest[2]) Motor_Left = MotorTest[2]; |
if(MotorTest[1]) Motor_Rear = MotorTest[1]; |
if(MotorTest[2]) Motor_Left = MotorTest[2]; |
if(MotorTest[3]) Motor_Right = MotorTest[3]; |
} |
684,7 → 684,7 |
StickYaw += ExternStickYaw; |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Analoge Control via serial communication |
//+ Analog control via serial communication |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128) |
783,7 → 783,6 |
IntegralAccPitch = 0; |
IntegralAccRoll = 0; |
IntegralAccZ = 0; |
MeanIntegralPitch = 0; |
MeanIntegralRoll = 0; |
805,12 → 804,12 |
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll); |
tmp_long2 /= 16; |
if((MaxStickPitch > 15) || (MaxStickRoll > 15)) |
if((MaxStickPitch > 15) || (MaxStickRoll > 15)) // reduce effect during stick commands |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(MaxStickYaw > 25) |
if(MaxStickYaw > 25) // reduce further is yaw stick is active |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
817,7 → 816,7 |
} |
#define BALANCE 32 |
// limit correction |
// limit correction effect |
if(tmp_long > BALANCE) tmp_long = BALANCE; |
if(tmp_long < -BALANCE) tmp_long =-BALANCE; |
if(tmp_long2 > BALANCE) tmp_long2 = BALANCE; |
830,9 → 829,9 |
// MeasurementCounter is incremented in the isr of analog.c |
if(MeasurementCounter >= BALANCE_NUMBER) // averaging number has reached |
{ |
static int cnt = 0; |
static char last_n_p, last_n_n, last_r_p, last_r_n; |
static long MeanIntegralPitch_old, MeanIntegralRoll_old; |
static int16_t cnt = 0; |
static int8_t last_n_p, last_n_n, last_r_p, last_r_n; |
static int32_t MeanIntegralPitch_old, MeanIntegralRoll_old; |
// if not lopping in any direction (this should be alwais the case, |
// because the Measurement counter is reset to 0 if looping in any direction is active.) |
845,7 → 844,6 |
// Calculate mean of the acceleration values |
IntegralAccPitch = (ParamSet.GyroAccFaktor * IntegralAccPitch) / BALANCE_NUMBER; |
IntegralAccRoll = (ParamSet.GyroAccFaktor * IntegralAccRoll ) / BALANCE_NUMBER; |
IntegralAccZ = IntegralAccZ / BALANCE_NUMBER; |
// Pitch ++++++++++++++++++++++++++++++++++++++++++++++++ |
// Calculate deviation of the averaged gyro integral and the averaged acceleration integral |
983,7 → 981,6 |
// reset variables used for averaging |
IntegralAccPitch = 0; |
IntegralAccRoll = 0; |
IntegralAccZ = 0; |
MeanIntegralPitch = 0; |
MeanIntegralRoll = 0; |
MeasurementCounter = 0; |
1039,7 → 1036,7 |
StoreNewCompassCourse = 0; |
} |
w = (w * FCParam.CompassYawEffect) / 64; // scale to parameter |
w = FCParam.CompassYawEffect - w; // reduce commpass effect with increasing declination |
w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination |
if(w > 0) // if there is any compass effect (avoid negative compass feedback) |
{ |
Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32; |
1129,7 → 1126,7 |
// Hight Control |
// The higth control algorithm reduces the thrust but does not increase the thrust. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// If hight control is activated and no emergency landing is activre |
// If hight control is activated and no emergency landing is active |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) ) |
{ |
int tmp_int; |
/branches/V0.68d Code Redesign killagreg/main.c |
---|
98,7 → 98,12 |
ROT_ON; |
GRN_OFF; |
// diable watchdog |
// set PC2 & PC3 as output (ctrl J16 & J17) |
DDRC |= (1<<DDC2)|(1<<DDC3); |
J16_OFF; |
J17_OFF; |
// disable watchdog |
MCUSR &=~(1<<WDRF); |
WDTCSR |= (1<<WDCE)|(1<<WDE); |
WDTCSR = 0; |
/branches/V0.68d Code Redesign killagreg/main.h |
---|
25,6 → 25,10 |
#define GRN_ON PORTB |= (1<<PORTB1) |
#define GRN_FLASH PORTB ^= (1<<PORTB1) |
#define J16_ON PORTC |= (1<<PORTC2) |
#define J16_OFF PORTC &= ~(1<<PORTC2) |
#define J17_ON PORTC |= (1<<PORTC3) |
#define J17_OFF PORTC &= ~(1<<PORTC3) |
#include <inttypes.h> |