Beispiel #1
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


}
Beispiel #2
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


}
Beispiel #3
0
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
	

}