WebSVN - FlightCtrl - Diff - Rev 1874 and 1887 - /branches/dongfang_FC



                "GyroPitch(PID)  ",
                "GyroRoll(PID)   ",
                "GyroYaw         ", //5
                "GYRO_RATE_FACTOR",
                "GYRO_RATE_FACTOR",
                "AttitudeControl ",
                "AccPitch (angle)",
                "AccRoll (angle) ", //10
                "UBat            ",
                "Pitch Term      ",
                "Roll Term       ",

        UCSR0B |= (1 << RXCIE0);
        // enable TX-Interrupt
        UCSR0B |= (1 << TXCIE0);

 
        // initialize the debug timer

        DebugData_Timer = setDelay(DebugData_Interval);
 
        // unlock rxd_buffer
        rxd_buffer_locked = FALSE;

        pRxData = 0;
        RxDataLen = 0;

 
        // no bytes to send
        txd_complete = TRUE;

 
#ifdef USE_MK3MAG
        Compass_Timer = setDelay(220);

                SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame,
                                sizeof(ConfirmFrame));
                ConfirmFrame = 0;
        }

 
        if (((DebugData_Interval && checkDelay(DebugData_Timer)) || request_DebugData)
                        && txd_complete) {
                SendOutData('D', FC_ADDRESS, 1, (uint8_t *) &DebugOut, sizeof(DebugOut));
                DebugData_Timer = setDelay(DebugData_Interval);
                request_DebugData = FALSE;

        }
 
        if (((Data3D_Interval && checkDelay(Data3D_Timer)) || request_Data3D)
                        && txd_complete) {
                SendOutData('C', FC_ADDRESS, 1, (uint8_t *) &Data3D, sizeof(Data3D));
                Data3D.AngleNick = (int16_t) ((10 * angle[PITCH])
                                / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
                Data3D.AngleRoll = (int16_t) ((10 * angle[ROLL])
                                / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
                Data3D.Heading = (int16_t) ((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
                Data3D_Timer = setDelay(Data3D_Interval);

                request_Data3D = FALSE;
        }
 
        if (request_ExternalControl && txd_complete) {
                SendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl,

                                sizeof(externalControl));
                request_ExternalControl = FALSE;
        }
 
#ifdef USE_MK3MAG
        if((checkDelay(Compass_Timer)) && txd_complete) {
                ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
                ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
                ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];
                ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];
                ToMk3Mag.CalState = compassCalState;
                SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
                // the last state is 5 and should be send only once to avoid multiple flash writing

                if(compassCalState > 4) compassCalState = 0;
                Compass_Timer = setDelay(99);

Rev 1874	Rev 1887
Line 129...	Line 129...
129	"GyroPitch(PID) ",	129	"GyroPitch(PID) ",
130	"GyroRoll(PID) ",	130	"GyroRoll(PID) ",
131	"GyroYaw ", //5	131	"GyroYaw ", //5
132	"GYRO_RATE_FACTOR",	132	"GYRO_RATE_FACTOR",
133	"GYRO_RATE_FACTOR",	133	"GYRO_RATE_FACTOR",
134	"unused ",	134	"AttitudeControl ",
135	"AccPitch (angle)",	135	"AccPitch (angle)",
136	"AccRoll (angle) ", //10	136	"AccRoll (angle) ", //10
137	"UBat ",	137	"UBat ",
138	"Pitch Term ",	138	"Pitch Term ",
139	"Roll Term ",	139	"Roll Term ",
Line 212...	Line 212...
212	UCSR0B \|= (1 << RXCIE0);	212	UCSR0B \|= (1 << RXCIE0);
213	// enable TX-Interrupt	213	// enable TX-Interrupt
214	UCSR0B \|= (1 << TXCIE0);	214	UCSR0B \|= (1 << TXCIE0);
Line 215...	Line 215...
215		215
216	// initialize the debug timer	216	// initialize the debug timer
Line 217...	Line 217...
217	DebugData_Timer = SetDelay(DebugData_Interval);	217	DebugData_Timer = setDelay(DebugData_Interval);
218		218
219	// unlock rxd_buffer	219	// unlock rxd_buffer
220	rxd_buffer_locked = FALSE;	220	rxd_buffer_locked = FALSE;
Line 221...	Line 221...
221	pRxData = 0;	221	pRxData = 0;
222	RxDataLen = 0;	222	RxDataLen = 0;
Line 223...	Line 223...
223		223
224	// no bytes to send	224	// no bytes to send
225	txd_complete = TRUE;	225	txd_complete = TRUE;
Line 226...	Line 226...
226		226
227	#ifdef USE_MK3MAG	227	#ifdef USE_MK3MAG
228	Compass_Timer = SetDelay(220);	228	Compass_Timer = setDelay(220);
Line 703...	Line 703...
703	SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame,	703	SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame,
704	sizeof(ConfirmFrame));	704	sizeof(ConfirmFrame));
705	ConfirmFrame = 0;	705	ConfirmFrame = 0;
706	}	706	}
Line 707...	Line 707...
707		707
708	if (((DebugData_Interval && CheckDelay(DebugData_Timer)) \|\| request_DebugData)	708	if (((DebugData_Interval && checkDelay(DebugData_Timer)) \|\| request_DebugData)
709	&& txd_complete) {	709	&& txd_complete) {
710	SendOutData('D', FC_ADDRESS, 1, (uint8_t *) &DebugOut, sizeof(DebugOut));	710	SendOutData('D', FC_ADDRESS, 1, (uint8_t *) &DebugOut, sizeof(DebugOut));
711	DebugData_Timer = SetDelay(DebugData_Interval);	711	DebugData_Timer = setDelay(DebugData_Interval);
712	request_DebugData = FALSE;	712	request_DebugData = FALSE;
Line 713...	Line 713...
713	}	713	}
714		714
715	if (((Data3D_Interval && CheckDelay(Data3D_Timer)) \|\| request_Data3D)	715	if (((Data3D_Interval && checkDelay(Data3D_Timer)) \|\| request_Data3D)
716	&& txd_complete) {	716	&& txd_complete) {
717	SendOutData('C', FC_ADDRESS, 1, (uint8_t *) &Data3D, sizeof(Data3D));	717	SendOutData('C', FC_ADDRESS, 1, (uint8_t *) &Data3D, sizeof(Data3D));
718	Data3D.AngleNick = (int16_t) ((10 * angle[PITCH])	718	Data3D.AngleNick = (int16_t) ((10 * angle[PITCH])
719	/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°	719	/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
720	Data3D.AngleRoll = (int16_t) ((10 * angle[ROLL])	720	Data3D.AngleRoll = (int16_t) ((10 * angle[ROLL])
721	/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°	721	/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
722	Data3D.Heading = (int16_t) ((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°	722	Data3D.Heading = (int16_t) ((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
723	Data3D_Timer = SetDelay(Data3D_Interval);	723	Data3D_Timer = setDelay(Data3D_Interval);
Line 724...	Line 724...
724	request_Data3D = FALSE;	724	request_Data3D = FALSE;
725	}	725	}
726		726
727	if (request_ExternalControl && txd_complete) {	727	if (request_ExternalControl && txd_complete) {
728	SendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl,	728	SendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl,
Line 729...	Line 729...
729	sizeof(externalControl));	729	sizeof(externalControl));
730	request_ExternalControl = FALSE;	730	request_ExternalControl = FALSE;
731	}	731	}
732		732
733	#ifdef USE_MK3MAG	733	#ifdef USE_MK3MAG
734	if((CheckDelay(Compass_Timer)) && txd_complete) {	734	if((checkDelay(Compass_Timer)) && txd_complete) {
735	ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg	735	ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
736	ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg	736	ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
737	ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];	737	ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];
738	ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];	738	ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];
739	ToMk3Mag.CalState = compassCalState;	739	ToMk3Mag.CalState = compassCalState;
740	SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));	740	SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
741	// the last state is 5 and should be send only once to avoid multiple flash writing	741	// the last state is 5 and should be send only once to avoid multiple flash writing
Line 742...	Line 742...
742	if(compassCalState > 4) compassCalState = 0;	742	if(compassCalState > 4) compassCalState = 0;
743	Compass_Timer = SetDelay(99);	743	Compass_Timer = setDelay(99);

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/uart0.c/ - Rev 1874 → 1887