예제 #1
0
void CalibrateData::callbackAccelGyro(const sensor_msgs::ImuConstPtr & msg)
{
  Eigen::Vector3d accel_raw;
  Eigen::Vector3d accel_cal;
  Eigen::Vector3d gyro_raw;
  Eigen::Vector3d gyro_cal;

  accel_raw(0) = msg->linear_acceleration.x;
  accel_raw(1) = msg->linear_acceleration.y;
  accel_raw(2) = msg->linear_acceleration.z;
  gyro_raw(0) = msg->angular_velocity.x;
  gyro_raw(1) = msg->angular_velocity.y;
  gyro_raw(2) = msg->angular_velocity.z;

  accel_cal = calibrate(accel_raw,
                        accel_alignment_matrix_,
                        accel_sensitivity_matrix_,
                        accel_offset_vector_);
  gyro_cal = calibrate(gyro_raw,
                       gyro_alignment_matrix_,
                       gyro_sensitivity_matrix_,
                       gyro_offset_vector_);

  sensor_msgs::Imu imu_msg = *msg;
  imu_msg.linear_acceleration.x = accel_cal(0);
  imu_msg.linear_acceleration.y = accel_cal(1);
  imu_msg.linear_acceleration.z = accel_cal(2);
  imu_msg.angular_velocity.x = gyro_cal(0) * M_PI / 180.0;
  imu_msg.angular_velocity.y = gyro_cal(1) * M_PI / 180.0;
  imu_msg.angular_velocity.z = gyro_cal(2) * M_PI / 180.0;

  imu_pub_.publish(imu_msg);
}
예제 #2
0
void mpu6050_init() {
//	printf("rspt: %x\n", MCM_ETBCC&6);
//	SetIsr(NonMaskableInt_VECTORn, NMI_Handler);
//	EnableIsr(NonMaskableInt_VECTORn);

    i2c_init(I2C0, 400000);
    DELAY_MS(100);
    printf("init start\n");
    mpu6050_short_init();
    DELAY_MS(1000);
    gyro_cal();
    printf("init ends\n");

//	float Rx = 0, raw_accel_angle = 0, sum = 0;
//	for(int i=0; i<100; i++){
//		float Rx =  (((float) (adc_once(ADC0_SE17, ADC_10bit) - 525) * 3.3/ 1023))/ 0.8f;
//
//		if(Rx > 1.0){
//			Rx = 1.0;
//		}else if(Rx < -1.0){
//			Rx = -1.0;
//		}
//		raw_accel_angle = 90 - (acos(Rx) * 180 / 3.1415f - 90);
//		sum += raw_accel_angle;
//	}
//	angle[0] = sum/100;
}
예제 #3
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

}
예제 #4
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

}
예제 #5
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;
}
예제 #6
0
int main(void)
{

	clk_init();

	gpio_init();

#ifdef SERIAL
	serial_init();
#endif

	i2c_init();

	spi_init();

	pwm_init();

	pwm_set(MOTOR_FL, 0);	// FL
	pwm_set(MOTOR_FR, 0);
	pwm_set(MOTOR_BL, 0);	// BL
	pwm_set(MOTOR_BR, 0);	// FR

	time_init();


	if (RCC_GetCK_SYSSource() == 8)
	  {
          
	  }
	else
	  {
		  failloop(5);
	  }

	sixaxis_init();

	if (sixaxis_check())
	  {
#ifdef SERIAL
		  printf(" MPU found \n");
#endif
	  }
	else
	  {
#ifdef SERIAL
		  printf("ERROR: MPU NOT FOUND \n");
#endif
		  failloop(4);
	  }

	adc_init();

	rx_init();

	int count = 0;
	vbattfilt = 0.0;

	while (count < 64)
	  {
		  vbattfilt += adc_read(1);
		  count++;
	  }
       // for randomising MAC adddress of ble app - this will make the int = raw float value        
		random_seed =  *(int *)&vbattfilt ; 
		random_seed = random_seed&0xff;
      
	vbattfilt = vbattfilt / 64;

#ifdef SERIAL
	printf("Vbatt %2.2f \n", vbattfilt);
#ifdef NOMOTORS
	printf("NO MOTORS\n");
#warning "NO MOTORS"
#endif
#endif

#ifdef STOP_LOWBATTERY
// infinite loop
	if (vbattfilt < STOP_LOWBATTERY_TRESH)
		failloop(2);
#endif

// loads acc calibration and gyro dafaults
	loadcal();

	gyro_cal();

	rgb_init();
	
	imu_init();
	
	extern unsigned int liberror;
	if (liberror)
	  {
#ifdef SERIAL
		  printf("ERROR: I2C \n");
#endif
		  failloop(7);
	  }


	lastlooptime = gettime();
	extern int rxmode;
	extern int failsafe;

	float thrfilt;

//
//
//              MAIN LOOP
//
//


	checkrx();

	while (1)
	  {
		  // gettime() needs to be called at least once per second 
		  maintime = gettime();
		  looptime = ((uint32_t) (maintime - lastlooptime));
		  if (looptime <= 0)
			  looptime = 1;
		  looptime = looptime * 1e-6f;
		  if (looptime > 0.02f)	// max loop 20ms
		    {
			    failloop(3);
			    //endless loop                  
		    }
		  lastlooptime = maintime;

		  if (liberror > 20)
		    {
			    failloop(8);
			    // endless loop
		    }

		  sixaxis_read();

		  control();

// battery low logic
				
		float hyst;
		float battadc = adc_read(1);
vbatt = battadc;
		// average of all 4 motor thrusts
		// should be proportional with battery current			
		extern float thrsum; // from control.c
		// filter motorpwm so it has the same delay as the filtered voltage
		// ( or they can use a single filter)		
		lpf ( &thrfilt , thrsum , 0.9968f);	// 0.5 sec at 1.6ms loop time	
		
		lpf ( &vbattfilt , battadc , 0.9968f);		

#ifdef AUTO_VDROP_FACTOR

static float lastout[12];
static float lastin[12];
static float vcomp[12];
static float score[12];
static int current_index = 0;

int minindex = 0;
float min = score[0];

{
	int i = current_index;

	vcomp[i] = vbattfilt + (float) i *0.1f * thrfilt;
		
	if ( lastin[i] < 0.1f ) lastin[i] = vcomp[i];
	float temp;
	//	y(n) = x(n) - x(n-1) + R * y(n-1) 
	//  out = in - lastin + coeff*lastout
		// hpf
	 temp = vcomp[i] - lastin[i] + FILTERCALC( 1000*12 , 1000e3) *lastout[i];
		lastin[i] = vcomp[i];
		lastout[i] = temp;
	 lpf ( &score[i] , fabsf(temp) , FILTERCALC( 1000*12 , 10e6 ) );

	}
	current_index++;
	if ( current_index >= 12 ) current_index = 0;

	for ( int i = 0 ; i < 12; i++ )
	{
	 if ( score[i] < min )  
		{
			min = score[i];
			minindex = i;
		}
}

#undef VDROP_FACTOR
#define VDROP_FACTOR  minindex * 0.1f
#endif

		if ( lowbatt ) hyst = HYST;
		else hyst = 0.0f;

		vbatt_comp = vbattfilt + (float) VDROP_FACTOR * thrfilt;

		if ( vbatt_comp <(float) VBATTLOW + hyst ) lowbatt = 1;
		else lowbatt = 0;
		

// led flash logic              

		  if (rxmode != RX_MODE_BIND)
		    {
					// non bind                    
			    if (failsafe)
			      {
				      if (lowbatt)
					      ledflash(500000, 8);
				      else
					      ledflash(500000, 15);
			      }
			    else
			      {
				      if (lowbatt)
					      ledflash(500000, 8);
				      else
					{
						if (ledcommand)
						  {
							  if (!ledcommandtime)
								  ledcommandtime = gettime();
							  if (gettime() - ledcommandtime > 500000)
							    {
								    ledcommand = 0;
								    ledcommandtime = 0;
							    }
							  ledflash(100000, 8);
						  }
						else
						{
							if ( aux[LEDS_ON] )
							ledon( 255);
							else 
							ledoff( 255);
						}
					}
			      }
		    }
		  else
		    {		// bind mode
			    ledflash(100000 + 500000 * (lowbatt), 12);
		    }

// rgb strip logic   
#if (RGB_LED_NUMBER > 0)				
	extern void rgb_led_lvc( void);
	rgb_led_lvc( );
#endif
				
#ifdef BUZZER_ENABLE
	buzzer();
#endif

#ifdef FPV_ON
			static int fpv_init = 0;
			if ( rxmode == RX_MODE_NORMAL && ! fpv_init ) {
				fpv_init = gpio_init_fpv();
			}
			if ( fpv_init ) {
				if ( failsafe ) {
					GPIO_WriteBit( FPV_PIN_PORT, FPV_PIN, Bit_RESET );
				} else {
					GPIO_WriteBit( FPV_PIN_PORT, FPV_PIN, aux[ FPV_ON ] ? Bit_SET : Bit_RESET );
				}
			}
#endif

	checkrx();
				
					
	// loop time 1ms                
	while ((gettime() - maintime) < (1000 - 22) )
			delay(10);



	}			// end loop


}
예제 #7
0
int main(void)
{

	clk_init();

	gpio_init();

#ifdef SERIAL
	serial_init();
#endif

	i2c_init();

	spi_init();

	pwm_init();

	pwm_set(MOTOR_FL, 0);	// FL
	pwm_set(MOTOR_FR, 0);
	pwm_set(MOTOR_BL, 0);	// BL
	pwm_set(MOTOR_BR, 0);	// FR

	time_init();


#ifdef SERIAL
	printf("\n clock source:");
#endif
	if (RCC_GetCK_SYSSource() == 8)
	  {
#ifdef SERIAL
		  printf(" PLL \n");
#endif
	  }
	else
	  {
#ifdef SERIAL
		  if (RCC_GetCK_SYSSource() == 0)
			  printf(" HSI \n");
		  else
			  printf(" OTHER \n");
#endif
		  failloop(5);
	  }

	sixaxis_init();

	if (sixaxis_check())
	  {
#ifdef SERIAL
		  printf(" MPU found \n");
#endif
	  }
	else
	  {
#ifdef SERIAL
		  printf("ERROR: MPU NOT FOUND \n");
#endif
		  failloop(4);
	  }

	adc_init();

	rx_init();

	int count = 0;
	float vbattfilt = 0.0;

	while (count < 64)
	  {
		  vbattfilt += adc_read(1);
		  count++;
	  }
	vbattfilt = vbattfilt / 64;

#ifdef SERIAL
	printf("Vbatt %2.2f \n", vbattfilt);
#ifdef NOMOTORS
	printf("NO MOTORS\n");
#warning "NO MOTORS"
#endif
#endif

#ifdef STOP_LOWBATTERY
// infinite loop
	if (vbattfilt < STOP_LOWBATTERY_TRESH)
		failloop(2);
#endif

// loads acc calibration and gyro dafaults
	loadcal();

	gyro_cal();

	imu_init();
	
	extern unsigned int liberror;
	if (liberror)
	  {
#ifdef SERIAL
		  printf("ERROR: I2C \n");
#endif
		  failloop(7);
	  }


	lastlooptime = gettime();
	extern int rxmode;
	extern int failsafe;

	float thrfilt;

//
//
//              MAIN LOOP
//
//


	checkrx();

	while (1)
	  {
		  // gettime() needs to be called at least once per second 
		  maintime = gettime();
		  looptime = ((uint32_t) (maintime - lastlooptime));
		  if (looptime <= 0)
			  looptime = 1;
		  looptime = looptime * 1e-6f;
		  if (looptime > 0.02f)	// max loop 20ms
		    {
			    failloop(3);
			    //endless loop                  
		    }
		  lastlooptime = maintime;

		  if (liberror > 20)
		    {
			    failloop(8);
			    // endless loop
		    }

		  sixaxis_read();

		  control();

// battery low logic

		  float battadc = adc_read(1);

		  // average of all 4 motor thrusts
		  // should be proportional with battery current                  
		  extern float thrsum;	// from control.c
		  // filter motorpwm so it has the same delay as the filtered voltage
		  // ( or they can use a single filter)           
		  lpf(&thrfilt, thrsum, 0.9968);	// 0.5 sec at 1.6ms loop time   


		  lpf(&vbattfilt, battadc, 0.9968);

		  if (vbattfilt + VDROP_FACTOR * thrfilt < VBATTLOW)
			  lowbatt = 1;
		  else
			  lowbatt = 0;

// led flash logic              

		  if (rxmode != RX_MODE_BIND)
		    {		// non bind                    
			    if (failsafe)
			      {
				      if (lowbatt)
					      ledflash(500000, 8);
				      else
					      ledflash(500000, 15);
			      }
			    else
			      {
				      if (lowbatt)
					      ledflash(500000, 8);
				      else
					{
						if (ledcommand)
						  {
							  if (!ledcommandtime)
								  ledcommandtime = gettime();
							  if (gettime() - ledcommandtime > 500000)
							    {
								    ledcommand = 0;
								    ledcommandtime = 0;
							    }
							  ledflash(100000, 8);
						  }
						else
							ledon(255);
					}
			      }
		    }
		  else
		    {		// bind mode
			    ledflash(100000 + 500000 * (lowbatt), 12);
		    }


		  checkrx();
#ifdef DEBUG
		  elapsedtime = gettime() - maintime;
#endif
// loop time 1ms                
		  while ((gettime() - maintime) < 1000)
			  delay(10);



	  }			// end loop


}
예제 #8
0
파일: main.c 프로젝트: Gastaudr/h8mini-acro
int main(void)
{

	clk_init();
	
	delay(1000);
	
  gpio_init();

#ifdef SERIAL	
	serial_init();
#endif

	i2c_init();
	
	spi_init();
	
	pwm_init();

	pwm_set( MOTOR_FL , 0);   // FL
	pwm_set( MOTOR_FR , 0);	 
	pwm_set( MOTOR_BL , 0);   // BL
	pwm_set( MOTOR_BR , 0);   // FR

  time_init();
	

#ifdef SERIAL	
	printf( "\n clock source:" );
	#endif
	if (  RCC_GetCK_SYSSource() == 8)  
	{
		#ifdef SERIAL	
		printf( " PLL \n" );
		#endif
	}
	else 
	{
	#ifdef SERIAL	
	if (  RCC_GetCK_SYSSource() == 0)  printf( " HSI \n" );
	else  printf( " OTHER \n" );
	#endif
  failloop( 5 );
	}

	sixaxis_init();
	
	if ( sixaxis_check() ) 
	{
		#ifdef SERIAL	
		printf( " MPU found \n" );
		#endif
	}
	else 
	{
		#ifdef SERIAL	
		printf( "ERROR: MPU NOT FOUND \n" );	
		#endif
		failloop(4);
	}
	
	adc_init();
	
	rx_init();
	
int count = 0;
	
while ( count < 64 )
{
	vbattfilt += adc_read(1);
	count++;
}
 vbattfilt = vbattfilt/64;	

#ifdef SERIAL	
		printf( "Vbatt %2.2f \n", vbattfilt );
		#ifdef NOMOTORS
    printf( "NO MOTORS\n" );
		#warning "NO MOTORS"
		#endif
#endif
	
#ifdef STOP_LOWBATTERY
// infinite loop
if ( vbattfilt < (float) STOP_LOWBATTERY_TRESH) failloop(2);
#endif

	gyro_cal();

	
extern unsigned int liberror;
if ( liberror ) 
{
	  #ifdef SERIAL	
		printf( "ERROR: I2C \n" );	
		#endif
		failloop(7);
}


 static unsigned lastlooptime; 
 lastlooptime = gettime();
 extern int rxmode;
 extern int failsafe;

 float thrfilt;

//
//
// 		MAIN LOOP
//
//

#ifdef DEBUG
static float timefilt;
#endif

	while(1)
	{
		// gettime() needs to be called at least once per second 
		unsigned long time = gettime(); 
		looptime = ((uint32_t)( time - lastlooptime));
		if ( looptime <= 0 ) looptime = 1;
		looptime = looptime * 1e-6f;
		if ( looptime > 0.02f ) // max loop 20ms
		{
			failloop( 3);	
			//endless loop			
		}
	
		#ifdef DEBUG				
		totaltime += looptime;
		lpf ( &timefilt , looptime, 0.998 );
		#endif
		lastlooptime = time;
		
		if ( liberror > 20) 
		{
			failloop(8);
			// endless loop
		}
		
		checkrx();
		
		gyro_read();
		
		control();
		
// battery low logic
		static int lowbatt = 0;
		float hyst;
		float battadc = adc_read(1);

		// average of all 4 motor thrusts
		// should be proportional with battery current			
		extern float thrsum; // from control.c
		// filter motorpwm so it has the same delay as the filtered voltage
		// ( or they can use a single filter)		
		lpf ( &thrfilt , thrsum , 0.9968f);	// 0.5 sec at 1.6ms loop time	
		
		lpf ( &vbattfilt , battadc , 0.9968f);		

		if ( lowbatt ) hyst = HYST;
		else hyst = 0.0f;
		
		if ( vbattfilt + (float) VDROP_FACTOR * thrfilt <(float) VBATTLOW + hyst ) lowbatt = 1;
		else lowbatt = 0;
		
// led flash logic		
		if ( rxmode == 0)
		{// bind mode
		ledflash ( 100000+ 500000*(lowbatt) , 12);
		}else
		{// non bind
		if ( failsafe) 
		{
		if ( lowbatt )
				ledflash ( 500000 , 8);
		else
				ledflash ( 500000, 15);	
			
		}
			else
			{					
			if ( lowbatt) 
				 ledflash ( 500000 , 8);	
			else ledon( 255);	
			} 		
		}

// the delay is required or it becomes endless loop ( truncation in time routine)
while ( (gettime() - time) < 1000 ) delay(10); 		

		
	}// end loop
	

}