| /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> |