Ejemplo n.º 1
0
void control(void)
{

	// hi rates
	float ratemulti;
	float ratemultiyaw;
	float maxangle;
	float anglerate;

	if (aux[RATES])
	  {
		  ratemulti = HIRATEMULTI;
		  ratemultiyaw = HIRATEMULTIYAW;
		  maxangle = MAX_ANGLE_HI;
		  anglerate = LEVEL_MAX_RATE_HI;
	  }
	else
	  {
		  ratemulti = 1.0f;
		  ratemultiyaw = 1.0f;
		  maxangle = MAX_ANGLE_LO;
		  anglerate = LEVEL_MAX_RATE_LO;
	  }


	for (int i = 0; i < 3; i++)
	  {
		#ifdef STOCK_TX_AUTOCENTER
		rxcopy[i] = rx[i] - autocenter[i];
		#else
		rxcopy[i] = rx[i];
		#endif
	  }

		
  flip_sequencer();
	


	if ( (!aux[STARTFLIP])&&auxchange[STARTFLIP] )
	{
		start_flip();
		auxchange[STARTFLIP]= 0;		
	}	

	if (auxchange[HEADLESSMODE])
	  {
		  yawangle = 0;
	  }

	if ((aux[HEADLESSMODE]) )
	  {
		yawangle = yawangle + gyro[2] * looptime;
			
		while (yawangle < -3.14159265f)
    yawangle += 6.28318531f;

    while (yawangle >  3.14159265f)
    yawangle -= 6.28318531f;
		
		float temp = rxcopy[0];
		rxcopy[0] = rxcopy[0] * fastcos( yawangle) - rxcopy[1] * fastsin(yawangle );
		rxcopy[1] = rxcopy[1] * fastcos( yawangle) + temp * fastsin(yawangle ) ;
	  }
	else
	  {
		  yawangle = 0;
	  }

// check for acc calibration

	int command = gestures2();

	if (command)
	  {
		  if (command == 3)
		    {
			    gyro_cal();	// for flashing lights
			    acc_cal();
			    savecal();
			    // reset loop time 
			    extern unsigned lastlooptime;
			    lastlooptime = gettime();
		    }
		  else
		    {
			    ledcommand = 1;
			    if (command == 2)
			      {
				      aux[CH_AUX1] = 1;

			      }
			    if (command == 1)
			      {
				      aux[CH_AUX1] = 0;
			      }
		    }
	  }

	if ( controls_override)
	{
		for ( int i = 0 ; i < 3 ; i++)
		{
			rxcopy[i] = rx_override[i];
		}
		 ratemulti = 1.0f;
		 maxangle = MAX_ANGLE_HI;
		 anglerate = LEVEL_MAX_RATE_HI;
	}
		
	imu_calc();

	pid_precalc();

	if ((aux[LEVELMODE]||level_override)&&!acro_override)
	  {			// level mode

		  angleerror[0] = rxcopy[0] * maxangle - attitude[0] + (float) TRIM_ROLL;
		  angleerror[1] = rxcopy[1] * maxangle - attitude[1] + (float) TRIM_PITCH;

		  error[0] = apid(0) * anglerate * DEGTORAD - gyro[0];
		  error[1] = apid(1) * anglerate * DEGTORAD - gyro[1];


	  }
	else
	  {			// rate mode

		  error[0] = rxcopy[0] * MAX_RATE * DEGTORAD * ratemulti - gyro[0];
		  error[1] = rxcopy[1] * MAX_RATE * DEGTORAD * ratemulti - gyro[1];

		  // reduce angle Iterm towards zero
		  extern float aierror[3];
		  for (int i = 0; i <= 2; i++)
			  aierror[i] *= 0.8f;


	  }


	error[2] = rxcopy[2] * MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[2];

	pid(0);
	pid(1);
	pid(2);

// map throttle so under 10% it is zero 
	float throttle = mapf(rx[3], 0, 1, -0.1, 1);
	if (throttle < 0)
		throttle = 0;
	if (throttle > 1.0f)
		throttle = 1.0f;


// turn motors off if throttle is off and pitch / roll sticks are centered
	if (failsafe || (throttle < 0.001f && ( !ENABLESTIX || !onground_long || aux[LEVELMODE] || level_override || (fabsf(rx[0]) < (float) ENABLESTIX_TRESHOLD && fabsf(rx[1]) < (float) ENABLESTIX_TRESHOLD))))
	  {			// motors off
		
		onground = 1;
			
		if ( onground_long )
		{
			if ( gettime() - onground_long > 1000000)
			{
				onground_long = 0;
			}
		}	

		#ifdef MOTOR_BEEPS
		extern void motorbeep( void);
		motorbeep();
		#endif
		
		  thrsum = 0;
		  for (int i = 0; i <= 3; i++)
		    {
			    pwm_set(i, 0);
		    }

		  // reset the overthrottle filter
		  lpf(&overthrottlefilt, 0.0f, 0.72f);	// 50hz 1khz sample rate

#ifdef MOTOR_FILTER
		  // reset the motor filter
		  for (int i = 0; i <= 3; i++)
		    {
			    motorfilter(0, i);
		    }
#endif


#ifdef 	THROTTLE_TRANSIENT_COMPENSATION
		  // reset hpf filter;
		  throttlehpf(0);
#endif

#ifdef STOCK_TX_AUTOCENTER
      for( int i = 0 ; i <3;i++)
				{
					if ( rx[i] == lastrx[i] )
						{
						  consecutive[i]++;
							
						}
					else consecutive[i] = 0;
					lastrx[i] = rx[i];
					if ( consecutive[i] > 1000 && fabsf( rx[i]) < 0.1f )
						{
							autocenter[i] = rx[i];
						}
				}
#endif				
	  }
	else
	  {
// motors on - normal flight

		onground_long = gettime();
			
#ifdef 	THROTTLE_TRANSIENT_COMPENSATION
		  throttle += 7.0f * throttlehpf(throttle);
		  if (throttle < 0)
			  throttle = 0;
		  if (throttle > 1.0f)
			  throttle = 1.0f;
#endif


		  // throttle angle compensation
#ifdef AUTO_THROTTLE
		  if (aux[LEVELMODE])
		    {

			    float autothrottle = fastcos(attitude[0] * DEGTORAD) * fastcos(attitude[1] * DEGTORAD);
			    float old_throttle = throttle;
			    if (autothrottle <= 0.5f)
				    autothrottle = 0.5f;
			    throttle = throttle / autothrottle;
			    // limit to 90%
			    if (old_throttle < 0.9f)
				    if (throttle > 0.9f)
					    throttle = 0.9f;

			    if (throttle > 1.0f)
				    throttle = 1.0f;

		    }
#endif

#ifdef LVC_PREVENT_RESET
extern float vbatt;
if (vbatt < (float) LVC_PREVENT_RESET_VOLTAGE) throttle = 0;
#endif
		  onground = 0;
		  float mix[4];

			if ( controls_override)
			{// change throttle in flip mode
				throttle = rx_override[3];
			}
				
#ifdef INVERT_YAW_PID
		  pidoutput[2] = -pidoutput[2];
#endif

		  mix[MOTOR_FR] = throttle - pidoutput[0] - pidoutput[1] + pidoutput[2];	// FR
		  mix[MOTOR_FL] = throttle + pidoutput[0] - pidoutput[1] - pidoutput[2];	// FL   
		  mix[MOTOR_BR] = throttle - pidoutput[0] + pidoutput[1] - pidoutput[2];	// BR
		  mix[MOTOR_BL] = throttle + pidoutput[0] + pidoutput[1] + pidoutput[2];	// BL   


#ifdef INVERT_YAW_PID
// we invert again cause it's used by the pid internally (for limit)
		  pidoutput[2] = -pidoutput[2];
#endif


#ifdef MIX_LOWER_THROTTLE

// limit reduction to this amount ( 0.0 - 1.0)
// 0.0 = no action 
// 0.5 = reduce up to 1/2 throttle      
//1.0 = reduce all the way to zero 
#define MIX_THROTTLE_REDUCTION_MAX 0.5

		  float overthrottle = 0;

		  for (int i = 0; i < 4; i++)
		    {
			    if (mix[i] > overthrottle)
				    overthrottle = mix[i];
		    }

		  overthrottle -= 1.0f;

		  if (overthrottle > (float)MIX_THROTTLE_REDUCTION_MAX)
			  overthrottle = (float)MIX_THROTTLE_REDUCTION_MAX;

#ifdef MIX_THROTTLE_FILTER_LPF
		  if (overthrottle > overthrottlefilt)
			  lpf(&overthrottlefilt, overthrottle, 0.82);	// 20hz 1khz sample rate
		  else
			  lpf(&overthrottlefilt, overthrottle, 0.72);	// 50hz 1khz sample rate
#else
		  if (overthrottle > overthrottlefilt)
			  overthrottlefilt += 0.005f;
		  else
			  overthrottlefilt -= 0.01f;
#endif

		  if (overthrottlefilt > 0.5f)
			  overthrottlefilt = 0.5;
		  if (overthrottlefilt < -0.1f)
			  overthrottlefilt = -0.1;


		  overthrottle = overthrottlefilt;

		  if (overthrottle < 0.0f)
			  overthrottle = 0.0f;

		  if (overthrottle > 0)
		    {		// exceeding max motor thrust

			    // prevent too much throttle reduction
			    if (overthrottle > (float)MIX_THROTTLE_REDUCTION_MAX)
				    overthrottle = (float)MIX_THROTTLE_REDUCTION_MAX;
			    // reduce by a percentage only, so we get an inbetween performance
			    overthrottle *= ((float)MIX_THROTTLE_REDUCTION_PERCENT / 100.0f);

			    for (int i = 0; i < 4; i++)
			      {
				      mix[i] -= overthrottle;
			      }
		    }
#endif


#ifdef MOTOR_FILTER

		  for (int i = 0; i < 4; i++)
		    {
			    mix[i] = motorfilter(mix[i], i);
		    }
#endif




#ifdef CLIP_FF
		  float clip_ff(float motorin, int number);

		  for (int i = 0; i < 4; i++)
		    {
			    mix[i] = clip_ff(mix[i], i);
		    }
#endif

		  for (int i = 0; i < 4; i++)
		    {
			    float test = motormap(mix[i]);
					#ifdef MOTORS_TO_THROTTLE
					test = throttle;
					// flash leds in valid throttle range
					ledcommand = 1;
					// for battery estimation
					mix[i] = throttle;
					#warning "MOTORS TEST MODE"
					#endif
					
					#ifdef MOTOR_MIN_ENABLE
					if (test < (float) MOTOR_MIN_VALUE)
					{
						test = (float) MOTOR_MIN_VALUE;
					}
					#endif
					
					#ifdef MOTOR_MAX_ENABLE
					if (test > (float) MOTOR_MAX_VALUE)
					{
						test = (float) MOTOR_MAX_VALUE;
					}
					#endif
					
					#ifndef NOMOTORS
					//normal mode
					pwm_set( i , test );				
					#else
					#warning "NO MOTORS"
					#endif
		    }

		  thrsum = 0;
		  for (int i = 0; i < 4; i++)
		    {
			    if (mix[i] < 0)
				    mix[i] = 0;
			    if (mix[i] > 1)
				    mix[i] = 1;
			    thrsum += mix[i];
		    }
		  thrsum = thrsum / 4;

	  }			// end motors on

}
Ejemplo n.º 2
0
void control( void)
{

    // hi rates
    float ratemulti;
    float ratemultiyaw;
    float maxangle;
    float anglerate;

    if ( aux[RATES] )
    {
        ratemulti = HIRATEMULTI;
        ratemultiyaw = HIRATEMULTIYAW;
        maxangle = MAX_ANGLE_HI;
        anglerate = LEVEL_MAX_RATE_HI;
    }
    else
    {
        ratemulti = 1.0f;
        ratemultiyaw = 1.0f;
        maxangle = MAX_ANGLE_LO;
        anglerate = LEVEL_MAX_RATE_LO;
    }


    for ( int i = 0 ; i < 3; i++)
    {
        rxcopy[i]=   rx[i];
    }


    if ( auxchange[HEADLESSMODE] )
    {
        yawangle = 0;
    }

    if ( (aux[HEADLESSMODE] )&&!aux[LEVELMODE] )
    {

        yawangle = yawangle + gyro[2]*looptime;

        float temp = rxcopy[0];
        rxcopy[0] = rxcopy[0] * cosf( yawangle) - rxcopy[1] * sinf(yawangle );
        rxcopy[1] = rxcopy[1] * cosf( yawangle) + temp * sinf(yawangle ) ;
    }
    else
    {
        yawangle = 0;
    }

// check for acc calibration

    int command = gestures2();

    if ( command )
    {
        if ( command == 3 )
        {
            gyro_cal(); // for flashing lights
            acc_cal();
            savecal();
            // reset loop time
            extern unsigned lastlooptime;
            lastlooptime = gettime();
        }
        else
        {
            ledcommand = 1;
            if ( command == 2 )
            {
                aux[CH_AUX1]= 1;

            }
            if ( command == 1 )
            {
                aux[CH_AUX1]= 0;
            }
        }
    }

    imu_calc();

    pid_precalc();

    if ( aux[LEVELMODE] )
    {   // level mode

        angleerror[0] = rxcopy[0] * maxangle - attitude[0];
        angleerror[1] = rxcopy[1] * maxangle - attitude[1];

        error[0] = apid(0) * anglerate * DEGTORAD  - gyro[0];
        error[1] = apid(1) * anglerate * DEGTORAD  - gyro[1];


    }
    else
    {   // rate mode

        error[0] = rxcopy[0] * MAX_RATE * DEGTORAD * ratemulti - gyro[0];
        error[1] = rxcopy[1] * MAX_RATE * DEGTORAD * ratemulti - gyro[1];

        // reduce angle Iterm towards zero
        extern float aierror[3];
        for ( int i = 0 ; i <= 2 ; i++) aierror[i] *= 0.8f;


    }


    error[2] = rxcopy[2] * MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[2];

    pid(0);
    pid(1);
    pid(2);

// map throttle so under 10% it is zero
    float	throttle = mapf(rx[3], 0 , 1 , -0.1 , 1 );
    if ( throttle < 0   ) throttle = 0;
    if ( throttle > 1.0f ) throttle = 1.0f;


// turn motors off if throttle is off and pitch / roll sticks are centered
    if ( failsafe || (throttle < 0.001f && (!ENABLESTIX||  (fabs(rx[0]) < 0.5f && fabs(rx[1]) < 0.5f ) ) ) )

    {   // motors off

        onground = 1;
        thrsum = 0;
        for ( int i = 0 ; i <= 3 ; i++)
        {
            pwm_set( i , 0 );
        }

        // reset the overthrottle filter
        lpf( &overthrottlefilt, 0.0f , 0.72f); // 50hz 1khz sample rate

#ifdef MOTOR_FILTER
        // reset the motor filter
        for ( int i = 0 ; i <= 3 ; i++)
        {
            motorfilter( 0 , i);
        }
#endif


#ifdef 	THROTTLE_TRANSIENT_COMPENSATION
        // reset hpf filter;
        throttlehpf( 0 );
#endif

    }
    else
    {


#ifdef 	THROTTLE_TRANSIENT_COMPENSATION
        throttle += 7.0f*throttlehpf( throttle );
        if ( throttle < 0   ) throttle = 0;
        if ( throttle > 1.0f ) throttle = 1.0f;
#endif


        // throttle angle compensation
#ifdef AUTO_THROTTLE
        if ( aux[LEVELMODE]||AUTO_THROTTLE_ACRO_MODE )
        {

            float autothrottle = fastcos( attitude[0] * DEGTORAD ) * fastcos( attitude[1] * DEGTORAD );
            float old_throttle = throttle;
            if ( autothrottle <= 0.5f ) autothrottle = 0.5f;
            throttle = throttle / autothrottle;
            // limit to 90%
            if ( old_throttle < 0.9f ) if ( throttle > 0.9f ) throttle = 0.9f;

            if ( throttle > 1.0f ) throttle = 1.0f;

        }
#endif
        onground = 0;
        float mix[4];


        mix[MOTOR_FR] = throttle - pidoutput[0] - pidoutput[1] + pidoutput[2];		// FR
        mix[MOTOR_FL] = throttle + pidoutput[0] - pidoutput[1] - pidoutput[2];		// FL
        mix[MOTOR_BR] = throttle - pidoutput[0] + pidoutput[1] - pidoutput[2];		// BR
        mix[MOTOR_BL] = throttle + pidoutput[0] + pidoutput[1] + pidoutput[2];		// BL


#ifdef MIX_LOWER_THROTTLE

// limit reduction to this amount ( 0.0 - 1.0)
// 0.0 = no action
// 0.5 = reduce up to 1/2 throttle
//1.0 = reduce all the way to zero
#define MIX_THROTTLE_REDUCTION_MAX 0.5

        float overthrottle = 0;

        for ( int i = 0 ; i < 3 ; i++)
        {
            if ( mix[i] > overthrottle ) overthrottle =  mix[i];
        }

        overthrottle -=1.0f;

        if ( overthrottle > (float) MIX_THROTTLE_REDUCTION_MAX ) overthrottle = (float) MIX_THROTTLE_REDUCTION_MAX;

#ifdef MIX_THROTTLE_FILTER_LPF
        if (overthrottle > overthrottlefilt)  lpf ( &overthrottlefilt, overthrottle , 0.82); // 20hz 1khz sample rate
        else lpf ( &overthrottlefilt, overthrottle , 0.72); // 50hz 1khz sample rate
#else
        if (overthrottle > overthrottlefilt) overthrottlefilt += 0.005f;
        else overthrottlefilt -= 0.01f;
#endif

        if (overthrottlefilt > 0.5f) overthrottlefilt = 0.5;
        if (overthrottlefilt < -0.1f) overthrottlefilt = -0.1;


        overthrottle = overthrottlefilt;

        if ( overthrottle < 0.0f ) overthrottle = 0.0f;

        if ( overthrottle > 0 )
        {   // exceeding max motor thrust

            // prevent too much throttle reduction
            if ( overthrottle > (float) MIX_THROTTLE_REDUCTION_MAX ) overthrottle = (float) MIX_THROTTLE_REDUCTION_MAX;
            // reduce by a percentage only, so we get an inbetween performance
            overthrottle *= ((float) MIX_THROTTLE_REDUCTION_PERCENT / 100.0f);

            for ( int i = 0 ; i < 3 ; i++)
            {
                mix[i] -=overthrottle;
            }
        }
#endif


#ifdef MOTOR_FILTER

        for ( int i = 0 ; i <= 3 ; i++)
        {
            mix[i] = motorfilter(  mix[i] , i);
        }
#endif




#ifdef CLIP_FF
        float clip_ff( float motorin ,int number);

        for ( int i = 0 ; i <= 3 ; i++)
        {
            mix[i] = clip_ff(  mix[i] , i);
        }
#endif

        for ( int i = 0 ; i <= 3 ; i++)
        {
            float test = motormap( mix[i] );
#ifndef NOMOTORS
            pwm_set( i , ( test )  );
#else
#warning "NO MOTORS"
#endif
        }

        thrsum = 0;
        for ( int i = 0 ; i <= 3 ; i++)
        {
            if ( mix[i] < 0 ) mix[i] = 0;
            if ( mix[i] > 1 ) mix[i] = 1;
            thrsum+= mix[i];
        }
        thrsum = thrsum / 4;

    }// end motors on

}
Ejemplo n.º 3
0
void control(void)
{

	// hi rates
	float ratemulti;
	float ratemultiyaw;
	float maxangle;
	float anglerate;


#ifdef TOGGLE_IN
if ( auxchange[TOGGLE_IN] && !aux[TOGGLE_IN] )
{
   ledcommand = 1;
aux[TOGGLE_OUT]=!aux[TOGGLE_OUT];    
}
#endif
	if (aux[RATES])
	  {
		  ratemulti = HIRATEMULTI;
		  ratemultiyaw = HIRATEMULTIYAW;
		  maxangle = MAX_ANGLE_HI;
		  anglerate = LEVEL_MAX_RATE_HI;
	  }
	else
	  {
		  ratemulti = 1.0f;
		  ratemultiyaw = 1.0f;
		  maxangle = MAX_ANGLE_LO;
		  anglerate = LEVEL_MAX_RATE_LO;
	  }


	for (int i = 0; i < 3; i++)
	  {
		#ifdef STOCK_TX_AUTOCENTER
		rxcopy[i] = rx[i] - autocenter[i];
		#else
		rxcopy[i] = rx[i];
		#endif
	  }

		
  flip_sequencer();
	


	if ( (!aux[STARTFLIP])&&auxchange[STARTFLIP] )
	{
		start_flip();
		auxchange[STARTFLIP]= 0;		
	}	

	if (auxchange[HEADLESSMODE])
	  {
		  yawangle = 0;
	  }

	if ((aux[HEADLESSMODE]) )
	  {
		yawangle = yawangle + gyro[2] * looptime;
			
		while (yawangle < -3.14159265f)
    yawangle += 6.28318531f;

    while (yawangle >  3.14159265f)
    yawangle -= 6.28318531f;
		
		float temp = rxcopy[0];
		rxcopy[0] = rxcopy[0] * fastcos( yawangle) - rxcopy[1] * fastsin(yawangle );
		rxcopy[1] = rxcopy[1] * fastcos( yawangle) + temp * fastsin(yawangle ) ;
	  }
	else
	  {
		  yawangle = 0;
	  }

// check for acc calibration

	int command = gestures2();

	if (command)
	  {
		  if (command == 3)
		    {
			    gyro_cal();	// for flashing lights
			    acc_cal();
			    savecal();
			    // reset loop time 
			    extern unsigned lastlooptime;
			    lastlooptime = gettime();
		    }
		  else
		    {
			    ledcommand = 1;
			    if (command == 2)
			      {
				      aux[CH_AUX1] = 1;

			      }
			    if (command == 1)
			      {
				      aux[CH_AUX1] = 0;
			      }
		    }
	  }

	if ( controls_override)
	{
		for ( int i = 0 ; i < 3 ; i++)
		{
			rxcopy[i] = rx_override[i];
		}
		 ratemulti = 1.0f;
		 maxangle = MAX_ANGLE_HI;
		 anglerate = LEVEL_MAX_RATE_HI;
	}
		

	pid_precalc();

	if ((aux[LEVELMODE]||level_override)&&!acro_override)
	  {	// level mode
			
		extern	void stick_vector( float );
		extern float errorvect[];	

			stick_vector( maxangle);
			float yawerror[3];
			extern float GEstG[3];

			float yawrate = rxcopy[2] * (float) MAX_RATEYAW * DEGTORAD * ratemultiyaw* ( 1/ 2048.0f);
			
		yawerror[0] = GEstG[1]  * yawrate;
		yawerror[1] = - GEstG[0]  * yawrate;
		yawerror[2] = GEstG[2]  * yawrate;

		angleerror[0] = errorvect[0] * RADTODEG *1.1f;
		angleerror[1] = errorvect[1] * RADTODEG *1.1f;
	
		for ( int i = 0 ; i <2; i++)
			{
			error[i] = apid(i) * anglerate * DEGTORAD + yawerror[i] - gyro[i];
			}

			error[2] = yawerror[2]  - gyro[2];

	  }
	else
	  {			// rate mode

		  error[0] = rxcopy[0] * MAX_RATE * DEGTORAD * ratemulti - gyro[0];
		  error[1] = rxcopy[1] * MAX_RATE * DEGTORAD * ratemulti - gyro[1];
			
			error[2] = rxcopy[2] * MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[2];
			
		  // reduce angle Iterm towards zero
		  extern float aierror[3];
			
		  aierror[0] = 0.0f;
			aierror[1] = 0.0f;


	  }



	pid(0);
	pid(1);
	pid(2);

// map throttle so under 10% it is zero 
	float throttle = mapf(rx[3], 0, 1, -0.1, 1);
	if (throttle < 0)
		throttle = 0;
	if (throttle > 1.0f)
		throttle = 1.0f;


#ifdef AIRMODE_HOLD_SWITCH
	if (failsafe || aux[AIRMODE_HOLD_SWITCH] || throttle < 0.001f && !onground_long)
	{
		onground_long = 0;
#else
// turn motors off if throttle is off and pitch / roll sticks are centered
	if (failsafe || (throttle < 0.001f && ( !ENABLESTIX || !onground_long || aux[LEVELMODE] || level_override || (fabsf(rx[0]) < (float) ENABLESTIX_TRESHOLD && fabsf(rx[1]) < (float) ENABLESTIX_TRESHOLD))))
	  {			// motors off
#endif

		onground = 1;
			
		if ( onground_long )
		{
			if ( gettime() - onground_long > 1000000)
			{
				onground_long = 0;
			}
		}	

		#ifdef MOTOR_BEEPS
		extern void motorbeep( void);
		motorbeep();
		#endif
		
		  thrsum = 0;
		  for (int i = 0; i <= 3; i++)
		    {
			    pwm_set(i, 0);
		    }

		  // reset the overthrottle filter
		  lpf(&overthrottlefilt, 0.0f, 0.72f);	// 50hz 1khz sample rate
			lpf(&underthrottlefilt, 0.0f, 0.72f);	// 50hz 1khz sample rate
#ifdef MOTOR_FILTER
		  // reset the motor filter
		  for (int i = 0; i <= 3; i++)
		    {
			    motorfilter(0, i);
		    }
#endif


#ifdef 	THROTTLE_TRANSIENT_COMPENSATION
		  // reset hpf filter;
		  throttlehpf(0);
#endif

#ifdef STOCK_TX_AUTOCENTER
      for( int i = 0 ; i <3;i++)
				{
					if ( rx[i] == lastrx[i] )
						{
						  consecutive[i]++;
							
						}
					else consecutive[i] = 0;
					lastrx[i] = rx[i];
					if ( consecutive[i] > 1000 && fabsf( rx[i]) < 0.1f )
						{
							autocenter[i] = rx[i];
						}
				}
#endif				
// end motors off / failsafe / onground
	  }
	else
	  {
// motors on - normal flight

		onground_long = gettime();
			
#ifdef 	THROTTLE_TRANSIENT_COMPENSATION
		  throttle += 7.0f * throttlehpf(throttle);
		  if (throttle < 0)
			  throttle = 0;
		  if (throttle > 1.0f)
			  throttle = 1.0f;
#endif


		  // throttle angle compensation
#ifdef AUTO_THROTTLE
		  if (aux[LEVELMODE])
		    {

			    float autothrottle = fastcos(attitude[0] * DEGTORAD) * fastcos(attitude[1] * DEGTORAD);
			    float old_throttle = throttle;
			    if (autothrottle <= 0.5f)
				    autothrottle = 0.5f;
			    throttle = throttle / autothrottle;
			    // limit to 90%
			    if (old_throttle < 0.9f)
				    if (throttle > 0.9f)
					    throttle = 0.9f;

			    if (throttle > 1.0f)
				    throttle = 1.0f;

		    }
#endif

#ifdef LVC_PREVENT_RESET
extern float vbatt;
if (vbatt < (float) LVC_PREVENT_RESET_VOLTAGE) throttle = 0;
#endif
		  onground = 0;
		  float mix[4];

			if ( controls_override)
			{// change throttle in flip mode
				throttle = rx_override[3];
			}
				
#ifdef INVERT_YAW_PID
		  pidoutput[2] = -pidoutput[2];
#endif

		  mix[MOTOR_FR] = throttle - pidoutput[0] - pidoutput[1] + pidoutput[2];	// FR
		  mix[MOTOR_FL] = throttle + pidoutput[0] - pidoutput[1] - pidoutput[2];	// FL   
		  mix[MOTOR_BR] = throttle - pidoutput[0] + pidoutput[1] - pidoutput[2];	// BR
		  mix[MOTOR_BL] = throttle + pidoutput[0] + pidoutput[1] + pidoutput[2];	// BL   


#ifdef INVERT_YAW_PID
// we invert again cause it's used by the pid internally (for limit)
		  pidoutput[2] = -pidoutput[2];
#endif

#if ( defined MIX_LOWER_THROTTLE || defined MIX_INCREASE_THROTTLE)

//#define MIX_INCREASE_THROTTLE

// options for mix throttle lowering if enabled
// 0 - 100 range ( 100 = full reduction / 0 = no reduction )
#ifndef MIX_THROTTLE_REDUCTION_PERCENT
#define MIX_THROTTLE_REDUCTION_PERCENT 100
#endif
// lpf (exponential) shape if on, othewise linear
//#define MIX_THROTTLE_FILTER_LPF

// limit reduction and increase to this amount ( 0.0 - 1.0)
// 0.0 = no action 
// 0.5 = reduce up to 1/2 throttle      
//1.0 = reduce all the way to zero 
#ifndef MIX_THROTTLE_REDUCTION_MAX
#define MIX_THROTTLE_REDUCTION_MAX 0.5
#endif

#ifndef MIX_THROTTLE_INCREASE_MAX
#define MIX_THROTTLE_INCREASE_MAX 0.2
#endif

#ifndef MIX_MOTOR_MAX
#define MIX_MOTOR_MAX 1.0f
#endif


		  float overthrottle = 0;
			float underthrottle = 0.001f;
		
		  for (int i = 0; i < 4; i++)
		    {
			    if (mix[i] > overthrottle)
				    overthrottle = mix[i];
					if (mix[i] < underthrottle)
						underthrottle = mix[i];
		    }

		  overthrottle -= MIX_MOTOR_MAX ;

		  if (overthrottle > (float)MIX_THROTTLE_REDUCTION_MAX)
			  overthrottle = (float)MIX_THROTTLE_REDUCTION_MAX;

#ifdef MIX_THROTTLE_FILTER_LPF
		  if (overthrottle > overthrottlefilt)
			  lpf(&overthrottlefilt, overthrottle, 0.82);	// 20hz 1khz sample rate
		  else
			  lpf(&overthrottlefilt, overthrottle, 0.72);	// 50hz 1khz sample rate
#else
		  if (overthrottle > overthrottlefilt)
			  overthrottlefilt += 0.005f;
		  else
			  overthrottlefilt -= 0.01f;
#endif
			
			// over			
		  if (overthrottlefilt > (float)MIX_THROTTLE_REDUCTION_MAX)
			  overthrottlefilt = (float)MIX_THROTTLE_REDUCTION_MAX;
		  if (overthrottlefilt < -0.1f)
			  overthrottlefilt = -0.1;

		  overthrottle = overthrottlefilt;
			
		  if (overthrottle < 0.0f)
			  overthrottle = -0.0001f;
		
			// reduce by a percentage only, so we get an inbetween performance
			overthrottle *= ((float)MIX_THROTTLE_REDUCTION_PERCENT / 100.0f);

#ifndef MIX_LOWER_THROTTLE
	// disable if not enabled
	overthrottle = -0.0001f;
#endif		
		
			
#ifdef MIX_INCREASE_THROTTLE
// under			
			
		  if (underthrottle < -(float)MIX_THROTTLE_INCREASE_MAX)
			  underthrottle = -(float)MIX_THROTTLE_INCREASE_MAX;
			
#ifdef MIX_THROTTLE_FILTER_LPF
		  if (underthrottle < underthrottlefilt)
			  lpf(&underthrottlefilt, underthrottle, 0.82);	// 20hz 1khz sample rate
		  else
			  lpf(&underthrottlefilt, underthrottle, 0.72);	// 50hz 1khz sample rate
#else
		  if (underthrottle < underthrottlefilt)
			  underthrottlefilt -= 0.005f;
		  else
			  underthrottlefilt += 0.01f;
#endif
// under
			if (underthrottlefilt < - (float)MIX_THROTTLE_REDUCTION_MAX)
			  underthrottlefilt = - (float)MIX_THROTTLE_REDUCTION_MAX;
		  if (underthrottlefilt > 0.1f)
			  underthrottlefilt = 0.1;

			underthrottle = underthrottlefilt;
					
			if (underthrottle > 0.0f)
			  underthrottle = 0.0001f;

			underthrottle *= ((float)MIX_THROTTLE_REDUCTION_PERCENT / 100.0f);
			
#endif			
	
			
		  if (overthrottle > 0 || underthrottle < 0 )
		    {		// exceeding max motor thrust
					float temp = overthrottle + underthrottle;
			    for (int i = 0; i < 4; i++)
			      {
				      mix[i] -= temp;
			      }
		    }
// end MIX_LOWER_THROTTLE
#endif	


#ifdef MOTOR_FILTER

		  for (int i = 0; i < 4; i++)
		    {
			    mix[i] = motorfilter(mix[i], i);
		    }
#endif




#ifdef CLIP_FF
		  float clip_ff(float motorin, int number);

		  for (int i = 0; i < 4; i++)
		    {
			    mix[i] = clip_ff(mix[i], i);
		    }
#endif

		  for (int i = 0; i < 4; i++)
		    {
			    float test = motormap(mix[i]);
					#ifdef MOTORS_TO_THROTTLE
					test = throttle;
					// flash leds in valid throttle range
					ledcommand = 1;
					// for battery estimation
					mix[i] = throttle;
					#warning "MOTORS TEST MODE"
					#endif
					
					#ifdef MOTOR_MIN_ENABLE
					if (test < (float) MOTOR_MIN_VALUE)
					{
						test = (float) MOTOR_MIN_VALUE;
					}
					#endif
					
					#ifdef MOTOR_MAX_ENABLE
					if (test > (float) MOTOR_MAX_VALUE)
					{
						test = (float) MOTOR_MAX_VALUE;
					}
					#endif
					
					#ifndef NOMOTORS
					//normal mode
					pwm_set( i , test );				
					#else
					#warning "NO MOTORS"
					#endif
		    }

		  thrsum = 0;
		  for (int i = 0; i < 4; i++)
		    {
			    if (mix[i] < 0)
				    mix[i] = 0;
			    if (mix[i] > 1)
				    mix[i] = 1;
			    thrsum += mix[i];
		    }
		  thrsum = thrsum / 4;

	  }			// end motors on

		
	imu_calc();
	
}

/////////////////////////////
/////////////////////////////





#ifdef MOTOR_CURVE_6MM_490HZ
// the old map for 490Hz
float motormap(float input)
{
	// this is a thrust to pwm function
	//  float 0 to 1 input and output
	// output can go negative slightly
	// measured eachine motors and prop, stock battery
	// a*x^2 + b*x + c
	// a = 0.262 , b = 0.771 , c = -0.0258

	if (input > 1)
		input = 1;
	if (input < 0)
		input = 0;

	input = input * input * 0.262f + input * (0.771f);
	input += -0.0258f;

	return input;
}
#endif

// 8k pwm is where the motor thrust is relatively linear for the H8 6mm motors
// it's due to the motor inductance cancelling the nonlinearities.
#ifdef MOTOR_CURVE_NONE
float motormap(float input)
{
	return input;
}
#endif


#ifdef MOTOR_CURVE_85MM_8KHZ
// Hubsan 8.5mm 8khz pwm motor map
// new curve
float motormap(float input)
{
//      Hubsan 8.5mm motors and props 

	if (input > 1)
		input = 1;
	if (input < 0)
		input = 0;

	input = input * input * 0.683f + input * (0.262f);
	input += 0.06f;

	return input;
}
#endif



#ifdef MOTOR_CURVE_85MM_8KHZ_OLD
// Hubsan 8.5mm 8khz pwm motor map
float motormap(float input)
{
//      Hubsan 8.5mm motors and props 

	if (input > 1)
		input = 1;
	if (input < 0)
		input = 0;

	input = input * input * 0.789f + input * (0.172f);
	input += 0.04f;

	return input;
}
#endif


#ifdef MOTOR_CURVE_85MM_32KHZ
// Hubsan 8.5mm 8khz pwm motor map
float motormap(float input)
{
//      Hubsan 8.5mm motors and props 

	if (input > 1)
		input = 1;
	if (input < 0)
		input = 0;

	input = input * input * 0.197f + input * (0.74f);
	input += 0.067f;

	return input;
}
#endif

float hann_lastsample[4];
float hann_lastsample2[4];

// hanning 3 sample filter
float motorfilter(float motorin, int number)
{
	float ans = motorin * 0.25f + hann_lastsample[number] * 0.5f + hann_lastsample2[number] * 0.25f;

	hann_lastsample2[number] = hann_lastsample[number];
	hann_lastsample[number] = motorin;

	return ans;
}
Ejemplo n.º 4
0
void control( void)
{

	// hi rates
	float ratemulti;
	float ratemultiyaw;

	if ( aux[RATES] ) 
	{
		ratemulti = HIRATEMULTI;
		ratemultiyaw = HIRATEMULTIYAW;
	}
	else 
	{
		ratemulti = 1.0;
		ratemultiyaw = 1.0;
	}

	
	yawangle = yawangle + gyro[YAW]*looptime;

	if ( auxchange[HEADLESSMODE] )
	{
		yawangle = 0;
	}
	
	if ( aux[HEADLESSMODE] ) 
	{
		float temp = rx[ROLL];
		rx[ROLL] = rx[ROLL] * cosf( yawangle) - rx[PITCH] * sinf(yawangle );
		rx[PITCH] = rx[PITCH] * cosf( yawangle) + temp * sinf(yawangle ) ;
	}
	
	
	error[ROLL] = rx[ROLL] * (float) MAX_RATE * DEGTORAD * ratemulti - gyro[ROLL];
	error[PITCH] = rx[PITCH] * (float) MAX_RATE * DEGTORAD * ratemulti - gyro[PITCH];
	error[YAW] = rx[YAW] * (float) MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[YAW];
	
pid_precalc();

	pid(ROLL);
	pid(PITCH);
	pid(YAW);


	
// map throttle so under 10% it is zero	
float	throttle = mapf(rx[3], 0 , 1 , -0.1 , 1 );
if ( throttle < 0   ) throttle = 0;

// turn motors off if throttle is off and pitch / roll sticks are centered
	if ( failsafe || (throttle < 0.001f && (!ENABLESTIX||  (fabs(rx[ROLL]) < 0.5f && fabs(rx[PITCH]) < 0.5f ) ) ) ) 

	{ // motors off
		for ( int i = 0 ; i <= 3 ; i++)
		{
			pwm_set( i , 0 );	
		}	
		onground = 1;
		thrsum = 0;
		#ifdef MOTOR_FILTER		
		// reset the motor filter
		for ( int i = 0 ; i <= 3 ; i++)
					{		
					motorfilter( 0 , i);
					}	
		#endif
	}
	else
	{
		onground = 0;
		float mix[4];	
		
//		pidoutput[2] += motorchange;
#ifdef INVERT_YAW_PID
pidoutput[2] = -pidoutput[2];			
#endif
		
		mix[MOTOR_FR] = throttle - pidoutput[ROLL] - pidoutput[PITCH] + pidoutput[YAW];		// FR
		mix[MOTOR_FL] = throttle + pidoutput[ROLL] - pidoutput[PITCH] - pidoutput[YAW];		// FL	
		mix[MOTOR_BR] = throttle - pidoutput[ROLL] + pidoutput[PITCH] - pidoutput[YAW];		// BR
		mix[MOTOR_BL] = throttle + pidoutput[ROLL] + pidoutput[PITCH] + pidoutput[YAW];		// BL	
		
#ifdef INVERT_YAW_PID
// we invert again cause it's used by the pid internally (for limit)
pidoutput[2] = -pidoutput[2];			
#endif
			
		
		
#ifdef MOTOR_FILTER		
for ( int i = 0 ; i <= 3 ; i++)
			{
			mix[i] = motorfilter(  mix[i] , i);
			}	
#endif
			
		for ( int i = 0 ; i <= 3 ; i++)
		{
		#ifndef NOMOTORS
		pwm_set( i ,motormap( mix[i] ) );
		#else
		tempx[i] = motormap( mix[i] );
		#endif
		}	
	
		
		thrsum = 0;
		for ( int i = 0 ; i <= 3 ; i++)
		{
			if ( mix[i] < 0 ) mix[i] = 0;
			if ( mix[i] > 1 ) mix[i] = 1;
			thrsum+= mix[i];
		}	
		thrsum = thrsum / 4;
		
	}// end motors on
	
}
Ejemplo n.º 5
0
void control( void)
{

	// hi rates
	float ratemulti;
	float ratemultiyaw;

	
	if ( aux[RATES] ) 
	{
		ratemulti = HIRATEMULTI;
		ratemultiyaw = HIRATEMULTIYAW;
	}
	else 
	{
		ratemulti = 1.0;
		ratemultiyaw = 1.0;
	}

	
	yawangle = yawangle + gyro[YAW]*looptime;

	if ( auxchange[HEADLESSMODE] )
	{
		yawangle = 0;
	}
	
	if ( aux[HEADLESSMODE] ) 
	{
		float temp = rx[ROLL];
		rx[ROLL] = rx[ROLL] * cosf( yawangle) - rx[PITCH] * sinf(yawangle );
		rx[PITCH] = rx[PITCH] * cosf( yawangle) + temp * sinf(yawangle ) ;
	}

	if ( aux[INVERTEDMODE] ) 
	{
		bridge_sequencer(REVERSE);	// reverse
	}
	else
	{
		bridge_sequencer(FORWARD);	// forward
	}


// pwmdir controls hardware directly so we make a copy here
	currentdir = pwmdir;

	
float rxtemp[3];

for (int i = 0 ; i < 3 ; i++)
{
	rxtemp[i] = rx[i];
}

if (currentdir == REVERSE)
		{	
			// invert pitch in reverse mode 
		//rxtemp[ROLL] = - rx[ROLL];
		rxtemp[PITCH] = - rx[PITCH];
		rxtemp[YAW]	= - rx[YAW];	
				
		}
		else
		{
			// normal thrust mode
			//rxtemp[ROLL] = - rx[ROLL];
			//rxtemp[PITCH] = - rx[PITCH];
			//rxtemp[YAW]	= - rx[YAW];	 	
			
		}

	error[ROLL] = rxtemp[ROLL] * (float)MAX_RATE * (float)DEGTORAD * ratemulti - gyro[ROLL];
	error[PITCH] = rxtemp[PITCH] * (float)MAX_RATE *(float) DEGTORAD * ratemulti - gyro[PITCH];
	error[YAW] = ( - 1) * rxtemp[YAW] * (float)MAX_RATEYAW * (float)DEGTORAD * ratemultiyaw - gyro[YAW];
	

pid_precalc();
	
	pid(ROLL);
	pid(PITCH);
	pid(YAW);


	
// map throttle so under 10% it is zero	
float	throttle = mapf(rx[3], 0 , 1 , -0.1 , 1 );
if ( throttle < 0   ) throttle = 0;

// turn motors off if throttle is off and pitch / roll sticks are centered
	if ( failsafe || (throttle < 0.001f && (!ENABLESTIX||  (fabsf(rx[ROLL]) < 0.5f && fabsf(rx[PITCH]) < 0.5f ) ) ) ) 

	{ // motors off
		for ( int i = 0 ; i <= 3 ; i++)
		{
			pwm_set( i , 0 );	
		}	
		onground = 1;
		thrsum = 0;
		#ifdef MOTOR_FILTER		
		// reset the motor filter
		for ( int i = 0 ; i <= 3 ; i++)
					{		
					motorfilter( 0 , i);
					}	
		#endif
	}
	else
	{
		onground = 0;
		if ( stage == BRIDGE_WAIT ) onground = 1;
		float mix[4];	

		if (currentdir == REVERSE)
		{
			// inverted flight
			pidoutput[ROLL] = -pidoutput[ROLL];
			pidoutput[PITCH] = -pidoutput[PITCH];
			//pidoutput[YAW] = -pidoutput[YAW];	
		}
		else
		{
			pidoutput[ROLL] = -pidoutput[ROLL];
			//pidoutput[PITCH] = -pidoutput[PITCH];
			//pidoutput[YAW] = -pidoutput[YAW];	
		}


//		pidoutput[2] += motorchange;
#ifdef INVERT_YAW_PID
pidoutput[2] = -pidoutput[2];			
#endif
		
		mix[MOTOR_FR] = throttle - pidoutput[ROLL] - pidoutput[PITCH] + pidoutput[YAW];		// FR
		mix[MOTOR_FL] = throttle + pidoutput[ROLL] - pidoutput[PITCH] - pidoutput[YAW];		// FL	
		mix[MOTOR_BR] = throttle - pidoutput[ROLL] + pidoutput[PITCH] - pidoutput[YAW];		// BR
		mix[MOTOR_BL] = throttle + pidoutput[ROLL] + pidoutput[PITCH] + pidoutput[YAW];		// BL	
			
#ifdef INVERT_YAW_PID
// we invert again cause it's used by the pid internally (for limit)
pidoutput[2] = -pidoutput[2];			
#endif
			
		
		
#ifdef MOTOR_FILTER		
for ( int i = 0 ; i <= 3 ; i++)
			{
			mix[i] = motorfilter(  mix[i] , i);
			}	
#endif
		
		for ( int i = 0 ; i <= 3 ; i++)
		{
		#ifndef NOMOTORS
		pwm_set( i ,motormap( mix[i] ) );		
		#endif
		}	
		
		
		thrsum = 0;
		for ( int i = 0 ; i <= 3 ; i++)
		{
			if ( mix[i] < 0 ) mix[i] = 0;
			if ( mix[i] > 1 ) mix[i] = 1;
			thrsum+= mix[i];
		}	
		thrsum = thrsum / 4;
		
	}// end motors on
	
}