WebSVN - FlightCtrl - Diff - Rev 1738 and 1757 - /branches/V0.80g_ACC-HH_MartinR



                                TCCR2A &= ~(1<<COM2A0);// make a high pulse
                                RemainingPulse  = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms

 
                                ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
                                ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
                          //if (Parameter_UserParam7 < 50)  // MartinR: um per UserParameter den Nickausgleich abzuschalten
                          //{
                                if(EE_Parameter.ServoCompInvert & 0x01)
                                {       // inverting movement of servo
                                        ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
                                }
                                else
                                {       // non inverting movement of servo
                                        ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
                                }
                          //}
                                // limit servo value to its parameter range definition
                                if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
                                {
                                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;

                                        switch(ServoIndex) // map servo channels
                                        {
                                                case 1: // Nick Compensation Servo
                                                        ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
                                                        ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
                                                        if (Parameter_UserParam7 < 50)  // MartinR: um per UserParameter den Nickausgleich abzuschalten
                                                        {
                                                        if(EE_Parameter.ServoCompInvert & 0x01)
                                                        {       // inverting movement of servo
                                                                ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
                                                        }
                                                        else
                                                        {       // non inverting movement of servo
                                                                ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
                                                        }
                                                        }
                                                        // limit servo value to its parameter range definition
                                                        if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
                                                        {
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
                                                        }

                                                        ServoNickValue /= MULTIPLYER;
                                                        break;
                                         case 2: // Roll Compensation Servo
                                                        ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl  * MULTIPLYER) / 4; // lowpass offset
                                                        ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
                                                        if (Parameter_UserParam7 < 100)  // MartinR: um per UserParameter den Nickausgleich abzuschalten
                                                        {
                                                        if(EE_Parameter.ServoCompInvert & 0x02)
                                                        {       // inverting movement of servo
                                                                ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
                                                        }
                                                        else
                                                        {       // non inverting movement of servo
                                                                ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
                                                        }
                                                        }
                                                    // limit servo value to its parameter range definition
                                                        if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
                                                        {
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
                                                        }

Subversion Repositories FlightCtrl

(root)/branches/V0.80g_ACC-HH_MartinR_HR/timer0.c - Rev 1738 → 1757

Rev 1738		Rev 1757
Line 222...		Line 222...
222	TCCR2A &= ~(1<<COM2A0);// make a high pulse	222	TCCR2A &= ~(1<<COM2A0);// make a high pulse
223	RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms	223	RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
Line 224...		Line 224...
224		224
225	ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset	225	ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
-		226	ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
-		227	//if (Parameter_UserParam7 < 50) // MartinR: um per UserParameter den Nickausgleich abzuschalten
226	ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)	228	//{
227	if(EE_Parameter.ServoCompInvert & 0x01)	229	if(EE_Parameter.ServoCompInvert & 0x01)
228	{ // inverting movement of servo	230	{ // inverting movement of servo
229	ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );	231	ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
230	}	232	}
231	else	233	else
232	{ // non inverting movement of servo	234	{ // non inverting movement of servo
233	ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );	235	ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
-		236	}
234	}	237	//}
235	// limit servo value to its parameter range definition	238	// limit servo value to its parameter range definition
236	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )	239	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
237	{	240	{
238	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;	241	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
Line 285...		Line 288...
285	switch(ServoIndex) // map servo channels	288	switch(ServoIndex) // map servo channels
286	{	289	{
287	case 1: // Nick Compensation Servo	290	case 1: // Nick Compensation Servo
288	ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset	291	ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
289	ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)	292	ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
-		293	if (Parameter_UserParam7 < 50) // MartinR: um per UserParameter den Nickausgleich abzuschalten
-		294	{
290	if(EE_Parameter.ServoCompInvert & 0x01)	295	if(EE_Parameter.ServoCompInvert & 0x01)
291	{ // inverting movement of servo	296	{ // inverting movement of servo
292	ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );	297	ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
293	}	298	}
294	else	299	else
295	{ // non inverting movement of servo	300	{ // non inverting movement of servo
296	ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );	301	ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
297	}	302	}
-		303	}
298	// limit servo value to its parameter range definition	304	// limit servo value to its parameter range definition
299	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )	305	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
300	{	306	{
301	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;	307	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
302	}	308	}
Line 309...		Line 315...
309	ServoNickValue /= MULTIPLYER;	315	ServoNickValue /= MULTIPLYER;
310	break;	316	break;
311	case 2: // Roll Compensation Servo	317	case 2: // Roll Compensation Servo
312	ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset	318	ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset
313	ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)	319	ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
-		320	if (Parameter_UserParam7 < 100) // MartinR: um per UserParameter den Nickausgleich abzuschalten
-		321	{
314	if(EE_Parameter.ServoCompInvert & 0x02)	322	if(EE_Parameter.ServoCompInvert & 0x02)
315	{ // inverting movement of servo	323	{ // inverting movement of servo
316	ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );	324	ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
317	}	325	}
318	else	326	else
319	{ // non inverting movement of servo	327	{ // non inverting movement of servo
320	ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );	328	ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
321	}	329	}
-		330	}
322	// limit servo value to its parameter range definition	331	// limit servo value to its parameter range definition
323	if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )	332	if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
324	{	333	{
325	ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;	334	ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
326	}	335	}