WebSVN - FlightCtrl - Diff - Rev 903 and 916 - /branches/V0.69k CRK HexaLotte/analog.c



#include "printf_P.h"
#include "eeprom.h"

 
volatile int16_t Current_AccZ = 0;
volatile int16_t UBat = 100;
volatile int16_t AdValueGyrNick = 0, AdValueGyrRoll = 0,  AdValueGyrYaw = 0;
volatile int16_t AdValueAccRoll = 0,  AdValueAccNick = 0, AdValueAccTop = 0;
volatile int32_t AirPressure = 32000;
volatile int16_t StartAirPressure;
volatile uint16_t ReadingAirPressure = 1023;
uint8_t PressureSensorOffset;

// and the update of further ads is stopped
// 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 (nick 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 (nick gyro average with first reading -> AdValueGyrNick)
// 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, nick1;
    static uint8_t average_pressure = 0;
    static int16_t tmpAirPressure = 0;
    // disable further AD conversion
    ADC_Disable();

            adc_channel = 1; // set next channel to ADC1 = ROLL GYRO
            MeasurementCounter++; // increment total measurement counter
            break;
        case 1:
            roll1 = ADC; // get Gyro Roll Voltage 1st sample
            adc_channel = 2; // set next channel to ADC2 = NICK GYRO
            break;
        case 2:
            nick1 = ADC; // get Gyro Nick Voltage 1st sample
            adc_channel = 4; // set next channel to ADC4 = UBAT
            break;
        case 3:
                // get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3
            UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value

        case 4:
            AdValueAccRoll = NeutralAccY - ADC; // get acceleration in Y direction
            adc_channel = 7; // set next channel to ADC7 = ACC_X
            break;
        case 5:
            AdValueAccNick = 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;

            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
            adc_channel = 2; // set next channel to ADC2 = NICK GYRO
            break;
        case 8:
                // average over two samples to create current ADValueNick
            if(BoardRelease == 10)  AdValueGyrNick = (ADC + nick1) / 2;
                        else                                    AdValueGyrNick = ADC + nick1; // 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(AdValueAccNick) / 4 + abs(AdValueAccRoll) / 4;
            if(AdValueAccTop > 1)
             {
                if(NeutralAccZ < 750)
                {
                                        NeutralAccZ += 0.02;

Rev 903	Rev 916
Line 15...	Line 15...
15	#include "printf_P.h"	15	#include "printf_P.h"
16	#include "eeprom.h"	16	#include "eeprom.h"
Line 17...	Line 17...
17		17
18	volatile int16_t Current_AccZ = 0;	18	volatile int16_t Current_AccZ = 0;
19	volatile int16_t UBat = 100;	19	volatile int16_t UBat = 100;
20	volatile int16_t AdValueGyrPitch = 0, AdValueGyrRoll = 0, AdValueGyrYaw = 0;	20	volatile int16_t AdValueGyrNick = 0, AdValueGyrRoll = 0, AdValueGyrYaw = 0;
21	volatile int16_t AdValueAccRoll = 0, AdValueAccPitch = 0, AdValueAccTop = 0;	21	volatile int16_t AdValueAccRoll = 0, AdValueAccNick = 0, AdValueAccTop = 0;
22	volatile int32_t AirPressure = 32000;	22	volatile int32_t AirPressure = 32000;
23	volatile int16_t StartAirPressure;	23	volatile int16_t StartAirPressure;
24	volatile uint16_t ReadingAirPressure = 1023;	24	volatile uint16_t ReadingAirPressure = 1023;
25	uint8_t PressureSensorOffset;	25	uint8_t PressureSensorOffset;
Line 85...	Line 85...
85	// and the update of further ads is stopped	85	// and the update of further ads is stopped
86	// The routine changes the ADC input muxer running	86	// The routine changes the ADC input muxer running
87	// thru the state machine by the following order.	87	// thru the state machine by the following order.
88	// state 0: ch0 (yaw gyro)	88	// state 0: ch0 (yaw gyro)
89	// state 1: ch1 (roll gyro)	89	// state 1: ch1 (roll gyro)
90	// state 2: ch2 (pitch gyro)	90	// state 2: ch2 (nick gyro)
91	// state 3: ch4 (battery voltage -> UBat)	91	// state 3: ch4 (battery voltage -> UBat)
92	// state 4: ch6 (acc y -> Current_AccY)	92	// state 4: ch6 (acc y -> Current_AccY)
93	// state 5: ch7 (acc x -> Current_AccX)	93	// state 5: ch7 (acc x -> Current_AccX)
94	// state 6: ch0 (yaw gyro average with first reading -> AdValueGyrYaw)	94	// state 6: ch0 (yaw gyro average with first reading -> AdValueGyrYaw)
95	// state 7: ch1 (roll gyro average with first reading -> AdValueGyrRoll)	95	// state 7: ch1 (roll gyro average with first reading -> AdValueGyrRoll)
96	// state 8: ch2 (pitch gyro average with first reading -> AdValueGyrPitch)	96	// state 8: ch2 (nick gyro average with first reading -> AdValueGyrNick)
97	// state 9: ch5 (acc z add also 4th part of acc x and acc y to reading)	97	// state 9: ch5 (acc z add also 4th part of acc x and acc y to reading)
98	// state10: ch3 (air pressure averaging over 5 single readings -> tmpAirPressure)	98	// state10: ch3 (air pressure averaging over 5 single readings -> tmpAirPressure)
Line 99...	Line 99...
99		99
100	ISR(ADC_vect)	100	ISR(ADC_vect)
101	{	101	{
102	static uint8_t adc_channel = 0, state = 0;	102	static uint8_t adc_channel = 0, state = 0;
103	static uint16_t yaw1, roll1, pitch1;	103	static uint16_t yaw1, roll1, nick1;
104	static uint8_t average_pressure = 0;	104	static uint8_t average_pressure = 0;
105	static int16_t tmpAirPressure = 0;	105	static int16_t tmpAirPressure = 0;
106	// disable further AD conversion	106	// disable further AD conversion
107	ADC_Disable();	107	ADC_Disable();
Line 113...	Line 113...
113	adc_channel = 1; // set next channel to ADC1 = ROLL GYRO	113	adc_channel = 1; // set next channel to ADC1 = ROLL GYRO
114	MeasurementCounter++; // increment total measurement counter	114	MeasurementCounter++; // increment total measurement counter
115	break;	115	break;
116	case 1:	116	case 1:
117	roll1 = ADC; // get Gyro Roll Voltage 1st sample	117	roll1 = ADC; // get Gyro Roll Voltage 1st sample
118	adc_channel = 2; // set next channel to ADC2 = PITCH GYRO	118	adc_channel = 2; // set next channel to ADC2 = NICK GYRO
119	break;	119	break;
120	case 2:	120	case 2:
121	pitch1 = ADC; // get Gyro Pitch Voltage 1st sample	121	nick1 = ADC; // get Gyro Nick Voltage 1st sample
122	adc_channel = 4; // set next channel to ADC4 = UBAT	122	adc_channel = 4; // set next channel to ADC4 = UBAT
123	break;	123	break;
124	case 3:	124	case 3:
125	// get actual UBat (Volts10) is ADC30V/1024*10 = ADC/3	125	// get actual UBat (Volts10) is ADC30V/1024*10 = ADC/3
126	UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value	126	UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value
Line 129...	Line 129...
129	case 4:	129	case 4:
130	AdValueAccRoll = NeutralAccY - ADC; // get acceleration in Y direction	130	AdValueAccRoll = NeutralAccY - ADC; // get acceleration in Y direction
131	adc_channel = 7; // set next channel to ADC7 = ACC_X	131	adc_channel = 7; // set next channel to ADC7 = ACC_X
132	break;	132	break;
133	case 5:	133	case 5:
134	AdValueAccPitch = ADC - NeutralAccX; // get acceleration in X direction	134	AdValueAccNick = ADC - NeutralAccX; // get acceleration in X direction
135	adc_channel = 0; // set next channel to ADC7 = YAW GYRO	135	adc_channel = 0; // set next channel to ADC7 = YAW GYRO
136	break;	136	break;
137	case 6:	137	case 6:
138	// average over two samples to create current AdValueGyrYaw	138	// average over two samples to create current AdValueGyrYaw
139	if(BoardRelease == 10) AdValueGyrYaw = (ADC + yaw1) / 2;	139	if(BoardRelease == 10) AdValueGyrYaw = (ADC + yaw1) / 2;
Line 142...	Line 142...
142	break;	142	break;
143	case 7:	143	case 7:
144	// average over two samples to create current ADValueGyrRoll	144	// average over two samples to create current ADValueGyrRoll
145	if(BoardRelease == 10) AdValueGyrRoll = (ADC + roll1) / 2;	145	if(BoardRelease == 10) AdValueGyrRoll = (ADC + roll1) / 2;
146	else AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1	146	else AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1
147	adc_channel = 2; // set next channel to ADC2 = PITCH GYRO	147	adc_channel = 2; // set next channel to ADC2 = NICK GYRO
148	break;	148	break;
149	case 8:	149	case 8:
150	// average over two samples to create current ADValuePitch	150	// average over two samples to create current ADValueNick
151	if(BoardRelease == 10) AdValueGyrPitch = (ADC + pitch1) / 2;	151	if(BoardRelease == 10) AdValueGyrNick = (ADC + nick1) / 2;
152	else AdValueGyrPitch = ADC + pitch1; // gain is 2 times lower on FC 1.1	152	else AdValueGyrNick = ADC + nick1; // gain is 2 times lower on FC 1.1
153	adc_channel = 5; // set next channel to ADC5 = ACC_Z	153	adc_channel = 5; // set next channel to ADC5 = ACC_Z
154	break;	154	break;
155	case 9:	155	case 9:
156	// get z acceleration	156	// get z acceleration
157	AdValueAccTop = (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction	157	AdValueAccTop = (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction
158	AdValueAccTop += abs(AdValueAccPitch) / 4 + abs(AdValueAccRoll) / 4;	158	AdValueAccTop += abs(AdValueAccNick) / 4 + abs(AdValueAccRoll) / 4;
159	if(AdValueAccTop > 1)	159	if(AdValueAccTop > 1)
160	{	160	{
161	if(NeutralAccZ < 750)	161	if(NeutralAccZ < 750)
162	{	162	{
163	NeutralAccZ += 0.02;	163	NeutralAccZ += 0.02;

Subversion Repositories FlightCtrl

(root)/branches/V0.69k CRK HexaLotte/analog.c - Rev 903 → 916