Ejemplo n.º 1
0
u32 getDiffTickFromNow(u32 ttick)
{
	getSysTick();

	if(ttick <= nowSysTick)
		return (nowSysTick - ttick); //getDiffTick(ttick, ticknow);
	else
		return (0xFFFFFFFF-ttick+nowSysTick);
}
Ejemplo n.º 2
0
u32 getDiffTickFromNow(u32 ttick)
{
	u32 ticknow =  getSysTick();

	if(ttick <= ticknow)
		return (ticknow - ttick); //getDiffTick(ttick, ticknow);
	else
		return (0xFFFFFFFF-ttick+ticknow);
}
Ejemplo n.º 3
0
static uint8_t tick(void) {
    float time = getSysTick() / 10000.0f;
    char text[128];
    snprintf(text, 127, "Device Time: %.1fs", time);

    struct zint_symbol zs;
    memset(&zs, 0, sizeof(zs));
    zs.input_mode = DATA_MODE;
    zs.option_1 = 3;
    /* zs.option_2 = 2; */
    int qr_res = qr_code(&zs, (unsigned char *)text, strlen(text));
    if (qr_res) {
        /* printf("Error: %i\n", qr_res); */
    }

    int zoom = MAX(1, MIN(LED_WIDTH / (zs.width + (BORDER * 2)), LED_HEIGHT / (zs.rows + (BORDER * 2))));
    static int old_width = 0;
    if (old_width != zs.width) {
        old_width = zs.width;
        lcdFillRGB(255, 255, 255);
    }

    int x0 = (LED_WIDTH - zoom * zs.width) / 2;
    int y0 = (LED_HEIGHT - zoom * zs.rows) / 2;
    for(int y = 0; y < zs.width; y++) {
        for(int x = 0; x < zs.rows; x++) {
            for(int y1 = y * zoom; y1 < (y + 1) * zoom; y1++) {
                for(int x1 = x * zoom; x1 < (x + 1) * zoom; x1++) {
                    if (module_is_set(&zs, x, y)) {
                        setLedXY(x0 + x1, y0 + y1, 0, 0, 0);
                    } else {
                        setLedXY(x0 + x1, y0 + y1, 255, 255, 255);
                    }
                }
            }
        }
    }
        
    time++;
    return 0;
}
Ejemplo n.º 4
0
void UpdateButton(struct Button *butt){
  
  if(butt->edge==BUTTON_RISING){
    butt->edge=BUTTON_PRESSED;
  }else if (butt->edge==BUTTON_FALLING){
    butt->edge=BUTTON_UNPRESSED;
  }
  
  if (GPIO_ReadInputDataBit(butt->port, butt->pin) == RESET)
  {
    if ((butt->integrator) > 0)
      (butt->integrator)--;
  }
  else if ((butt->integrator) < MAXIMUM)
    (butt->integrator)++;
  
  /* Step 2: Update the output state based on the integrator.  Note that the
  output will only change states if the integrator has reached a limit, either
  
  0 or MAXIMUM. */
  
  
  
  if (butt->integrator == 0){
    if(butt->output==1){
      butt->edge = BUTTON_FALLING;
    }
    butt->output = 0;
  }
  else if ((butt->integrator) >= MAXIMUM){
    if(butt->edge==BUTTON_UNPRESSED){
      butt->startTime=getSysTick();
      butt->edge=BUTTON_RISING;
    }
    butt->output = 1;
    butt->integrator = MAXIMUM;  /* defensive code if integrator got corrupted */
  }
}
Ejemplo n.º 5
0
void testLoop()
{
	u8 errorCount = 0,test_current_level;
	u32 tempSysTick;
	u16 tCurrentAdc,minCurrent,maxCurrent;

	//gTestMode = TEST_AA_BATTERY;
Start:
	if(gTestMode == TEST_AAA_BATTERY)
	{
		gAdc_Current_Level_1_Min = ADC_CURRENT_LEVEL_1_MIN_AAA;
		gAdc_Current_Level_1_Max = ADC_CURRENT_LEVEL_1_MAX_AAA;
		gAdc_Current_Level_2_Min = ADC_CURRENT_LEVEL_2_MIN_AAA;
		gAdc_Current_Level_2_Max = ADC_CURRENT_LEVEL_2_MAX_AAA;
		gAdc_Current_Level_3_Min = ADC_CURRENT_LEVEL_3_MIN_AAA;
		gAdc_Current_Level_3_Max = ADC_CURRENT_LEVEL_3_MAX_AAA;
		gAdc_Start_Battery_Charging = ADC_START_BATTERY_CHARGING;	
	}
	else
	{
		gAdc_Current_Level_1_Min = ADC_CURRENT_LEVEL_1_MIN;
		gAdc_Current_Level_1_Max = ADC_CURRENT_LEVEL_1_MAX;
		gAdc_Current_Level_2_Min = ADC_CURRENT_LEVEL_2_MIN;
		gAdc_Current_Level_2_Max = ADC_CURRENT_LEVEL_2_MAX;
		gAdc_Current_Level_3_Min = ADC_CURRENT_LEVEL_3_MIN;
		gAdc_Current_Level_3_Max = ADC_CURRENT_LEVEL_3_MAX;
		gAdc_Start_Battery_Charging = ADC_START_BATTERY_CHARGING;
	}
	
	
	
	//test_pos_now = TEST_CHANNEL_2;


	do{

			switch(test_pos_now)
			{
				case TEST_CHANNEL_1:
						CLOSE_CHANNEL_1();break;
				case TEST_CHANNEL_2:
						CLOSE_CHANNEL_2();break;
				case TEST_CHANNEL_3:
						CLOSE_CHANNEL_3();break;
				case TEST_CHANNEL_4:
						CLOSE_CHANNEL_4();break;
				default:
						dumpHandler();
						goto endpos;
						break;
			}



			//wait for ZERO_STATE---->DETECT_STATE
			//while(getAverage(test_pos_now) > ADC_START_BATTERY_DETECT)
			//	ClrWdt();
			getSysTick();
			tempSysTick = nowSysTick;
			do
			{
				tCurrentAdc = getAverage(test_pos_now);
				if(tCurrentAdc > ADC_NO_CURRENT)
				{
					if(tCurrentAdc > gAdc_Current_Level_1_Min)
					{
						stepNow = 4;
						goto battery_detect;
					}
					else
						break;
				}
				#if 1
				getSysTick();
				if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >=	12)
				{
					dumpHandler();
					goto endpos;
				}
				#endif
				ClrWdt();
				
			}while(1);

			getSysTick();
			tempSysTick = nowSysTick;
			while(getAverage(test_pos_now) > ADC_NO_CURRENT)
			{
				getSysTick();
				if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >=	MAX_TIME_BATTERY_DEAD)
				{
					dumpHandler();
					goto endpos;
				}
				ClrWdt();
			}
			stepNow++;
			//ok now battery state is BATTERY_DETECT
			//time wait for battery detect
			tempSysTick = nowSysTick;
			while(getAverage(test_pos_now) < gAdc_Start_Battery_Charging)
			{
				getSysTick();
				if(nowSysTick < tempSysTick  || (nowSysTick-tempSysTick) >=	MAX_TIME_TO_BATTERY_DETECT)
				{
					dumpHandler();
					goto endpos;
				}
			}
			stepNow++;
			if((nowSysTick-tempSysTick) < MIN_TIME_TO_BATTERY_DETECT)
			{
				dumpHandler();
				goto endpos;
			}
			stepNow++;
			
			//BATTERY_DETECT_STATE
		battery_detect:
			getSysTick();
			tempSysTick = nowSysTick;

			delay_ms(10);
			do
			{
				tCurrentAdc = getAverage(test_pos_now);
				if(tCurrentAdc < gAdc_Current_Level_1_Min|| tCurrentAdc > gAdc_Current_Level_1_Max)	// CURRENT_LEVEL_1
				{
					errorCount++;
					if(errorCount > 3)
					{
						dumpHandler();
						goto endpos;
					}
				}

				getSysTick();
				if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >=	MAX_TIME_DURING_BATTERY_DETECT)
				{
					dumpHandler();
					goto endpos;
				}
				//delay_ms(10);
				ClrWdt();
			}while(tCurrentAdc > gAdc_Current_Level_1_Min);
			stepNow++;
			if((nowSysTick-tempSysTick) < MIN_TIME_DURING_BATTERY_DETECT)
			{
				dumpHandler();
				goto endpos;
			}
			stepNow++;
			//wait for no current
			tempSysTick = nowSysTick;
			do{
				tCurrentAdc = getAverage(test_pos_now);
				ClrWdt();
				getSysTick();
				if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >=4)
				{
					dumpHandler();
					goto endpos;
				}
			}while(tCurrentAdc > ADC_NO_CURRENT);
			stepNow++;
			//ok, now battery state is BATTERY_NORMAL_CHARGING
			//time wait for battery normal charging
			for(test_current_level = 1; test_current_level <=4; test_current_level++)
			{
			errorCount = 0;
			switch(test_current_level)
			{
				case 1:
						minCurrent = gAdc_Current_Level_3_Min;
						maxCurrent = gAdc_Current_Level_3_Max;
						break;
				case 2:
						minCurrent = gAdc_Current_Level_2_Min;
						maxCurrent = gAdc_Current_Level_2_Max;
						break;
				case 3:
				case 4:
						minCurrent = gAdc_Current_Level_1_Min;
						maxCurrent = gAdc_Current_Level_1_Max;
						break;
				default:
						dumpHandler();
						goto endpos;
						break;
			}
			getSysTick();
			tempSysTick = nowSysTick;
			while(getAverage(test_pos_now) < gAdc_Start_Battery_Charging)
			{
				getSysTick();
				if(nowSysTick < tempSysTick  || (nowSysTick-tempSysTick) >=	MAX_TIME_TO_BATTERY_NORMAL_CHARGING)
				{
					dumpHandler();
					goto endpos;
				}
			}
			stepNow++;
			if((nowSysTick-tempSysTick) < MIN_TIME_TO_BATTERY_NORMAL_CHARGING_SPEC)
			{
				dumpHandler();
				goto endpos;
			}
			stepNow++;
			//BATTERY_NORMAL_CHARGING
			getSysTick();
			tempSysTick = nowSysTick;
			delay_ms(10);
			do{
				tCurrentAdc = getAverage(test_pos_now);
				if(tCurrentAdc < minCurrent || tCurrentAdc > maxCurrent)
				{
					errorCount++;
					if(errorCount > 5)
					{
						dumpHandler();
						goto endpos;
					}
				}
				getSysTick();
				if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >=	MAX_TIME_DURING_BATTERY_NORMAL_CHARGING)
				{
					dumpHandler();
					goto endpos;
				}
				ClrWdt();
			}while(tCurrentAdc > minCurrent);

			if((nowSysTick-tempSysTick) < MIN_TIME_DURING_BATTERY_NORMAL_CHARGING)
			{
				dumpHandler();
				goto endpos;
			}

			tempSysTick = nowSysTick;
			do{
				tCurrentAdc = getAverage(test_pos_now);
				ClrWdt();
				getSysTick();
				if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >=4)
				{
					dumpHandler();
					goto endpos;
				}
			}while(tCurrentAdc > ADC_NO_CURRENT);

			#ifdef ENABLE_FAST_TEST_MODE
			if(test_current_level == 2)
			{
				if(gTestMode == TEST_AA_BATTERY)
				{
					if(test_pos_now != TEST_CHANNEL_1)
						break;
				}
				else if(gTestMode == TEST_AAA_BATTERY)
				{
					if(test_pos_now != TEST_CHANNEL_1)
						break;
				}
				#endif
			}
		}

			
			switch(test_pos_now)
			{
				case TEST_CHANNEL_1:
						OPEN_CHANNEL_1();
						LED3_G_ON();
						break;
				case TEST_CHANNEL_2:
						OPEN_CHANNEL_2();
						LED4_G_ON();
						break;
				case TEST_CHANNEL_3:
						OPEN_CHANNEL_3();
						LED1_G_ON();
						break;
				case TEST_CHANNEL_4:
						OPEN_CHANNEL_4();
						LED2_G_ON();
						break;
				default:
						dumpHandler();
						goto endpos;
						break;
			}
			
		endpos:
			delay_ms(20);
			test_pos_now++;
	}while(test_pos_now<= TEST_CHANNEL_4);

	if(isError)
	{
		while(1)
		{
			NOP();
			ClrWdt();
		}
	}
getSysTick();
tempSysTick = nowSysTick;
while(GET_BATTERY_STATUS() == gTestMode)
{
	ClrWdt();
	getSysTick();
	if((nowSysTick-tempSysTick) >= 200)
		LED_ALL_OFF();
	
}
LED_ALL_OFF();


//CLOSE_CHANNEL_1();
//CLOSE_CHANNEL_2();
//CLOSE_CHANNEL_3();
//CLOSE_CHANNEL_4();
while(1)
		{
			NOP();
			ClrWdt();
		}

gTestMode = GET_BATTERY_STATUS();

	errorCount = 0;
	test_pos_now = TEST_CHANNEL_1;
	goto Start;
}
Ejemplo n.º 6
0
Archivo: main.c Proyecto: fishr/Origin
/**
* @brief   Main program
* @param  None
* @retval None
*/
int main(void)
{
  SysTick_Config(SystemCoreClock / 1000);
  
  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);

  GPIO_InitTypeDef  GPIO_InitStructure;

  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  GPIO_Init(GPIOE, &GPIO_InitStructure);
  
  GPIO_ResetBits(GPIOE, GPIO_Pin_2);  //start with gps off to make sure it activates when wanted
  
  GPIO_Start();
  ADC_Start();
  Flash_Start();
  
  unsigned long tickey = getSysTick()+1000;
  
  GPIO_ResetBits(GPIOA, GPIO_Pin_10); //LCD Reset must be held 10us
  GPIO_SetBits(GPIOG, GPIO_Pin_3);  //flash deselect
  GPIO_SetBits(GPIOC, GPIO_Pin_8);  //flash #hold off, we have dedicated pins
  GPIO_SetBits(GPIOC, GPIO_Pin_1);  //osc enable
  GPIO_ResetBits(GPIOC, GPIO_Pin_11); //xbee reset
  GPIO_SetBits(GPIOE, GPIO_Pin_6); //buck enable
  while(getSysTick()<tickey);
  GPIO_SetBits(GPIOE, GPIO_Pin_2); //gps on/off
  GPIO_SetBits(GPIOC, GPIO_Pin_11); //xbee reset
  GPIO_SetBits(GPIOA, GPIO_Pin_10);  //LCD unreset
  UART4_Start();
  UART5_Start();
  MPU_Start();
  
  //========================BUTTONS====================
  InitButton(&button1, GPIOE, GPIO_Pin_4);
#ifdef BOARD_V1
  InitButton(&button2, GPIOE, GPIO_Pin_5);
#else
  InitButton(&button2, GPIOA, GPIO_Pin_9);
#endif
  
  //=======================END BUTTONS==================
  
  
  /* LCD Configuration */
  LCD_Config();
  /* Enable The LCD */
  LTDC_Cmd(ENABLE);
  LCD_SetLayer(LCD_FOREGROUND_LAYER);
  GUI_ClearBackground();
  int count = 0;
  delay(20000);
  
#ifndef ORIGIN    
  GUI_InitNode(1, 72,  83, 0xe8ec);
  GUI_InitNode(2, 86,  72, 0xfd20);
  GUI_InitNode(3, 'R',  'F', 0x001f);
#endif
  
  int screencount = 0;
  
#ifdef INSIDE
  origin_state.lati=KRESGE_LAT;
  origin_state.longi=KRESGE_LONG;
  origin_state.gpslock=1;
#endif
  
  unsigned long tickey2 = getSysTick()+2000;  //2 second counter
  unsigned long tickey3 = getSysTick()+4000;  //4 second delay to check gps state
  
  /* Infinite loop */
  while (1)
  {
    UpdateButton(&button1);
    UpdateButton(&button2);
    
    if( buttonRisingEdge(&button1)){//right
      GPIO_ToggleBits(GPIOC, GPIO_Pin_3);//yellow
      //UART_Transmit(&huart4, gps_init_msg, cmdData1Len, 500);
      origin_state.pingnum+=1;
      origin_state.pingactive=1;
      origin_state.whodunnit = origin_state.id;
      origin_state.pingclearedby = 0;
    }
    
    if(buttonRisingEdge(&button2)){//left
      //UART_Transmit(&huart4, gps_get_time_msg, cmdData2Len, 500);
      GPIO_ToggleBits(GPIOA, GPIO_Pin_2); //green
      
      if(origin_state.pingactive&&(origin_state.whodunnit != origin_state.id)){
        origin_state.pingactive=0;
      }
    }
    
    if(origin_state.gpson>2 &&(getSysTick()>tickey3)){
      GPIO_ResetBits(GPIOE, GPIO_Pin_2);
      delay(20000);
      GPIO_SetBits(GPIOE, GPIO_Pin_2);
      delay(20000);      
      char setme[80];
      sprintf(setme, "%s%c%c", gps_init_msg, 0x0D, 0x0A);
      UART_Transmit(UART4, setme, sizeof(setme)/sizeof(setme[0]), 5000);
      origin_state.gpson=0;
      tickey3+=4000;
    }
    
    if(getReset()){
      NVIC_SystemReset();
    }
    
#ifdef ORIGIN
//    long actHeading=0;
//    inv_get_sensor_type_heading(&actHeading, &headingAcc, &headingTime);
//    degrees=((double)actHeading)/((double)65536.0);
//    origin_state.heading=degrees;
    
     long actHeading[3] = {0,0,0};
inv_get_sensor_type_euler(actHeading, &headingAcc, &headingTime);
degrees=((double)actHeading[2])/((double)65536.0);
//origin_state.heading=degrees;

    long tempyraiture;
    mpu_get_temperature(&tempyraiture, NULL);

//    short garbage[3];
//    mpu_get_compass_reg(garbage, NULL);
//    double compass_angle = atan2(-garbage[0], -garbage[1])*180/3.1415;
//    //origin_state.heading = .9*degrees + .1*compass_angle;
//    origin_state.heading = compass_angle;
#endif
    
    if(getSysTick()>tickey2){
      tickey2 +=2000;
      sendMessage();
    }
    
      processGPS();
      processXbee();
    
    if(getSysTick()>tickey){
      tickey +=53;
      
      GPIO_ToggleBits(GPIOC, GPIO_Pin_3); 
      
#ifndef ORIGIN
      GUI_UpdateNode(1, degrees*3.1415/180.0+3.14*1.25, screencount, (screencount>10), 0);
      GUI_UpdateNode(2, degrees*3.1415/180.0+3.14, screencount, (screencount>30), 0);
      GUI_UpdateNode(3, degrees*3.1415/180.0+0, screencount, (screencount>50), 0);
#else
      GUI_UpdateNodes();
#endif
      
      
      GUI_UpdateArrow(-degrees*3.1415/180.0);
      GUI_UpdateBattery(getBatteryStatus());
      GUI_DrawTime();
      if (count > 50){
        GUI_UpdateBottomButton(1, 0xe8ec);
      } else {
        GUI_UpdateBottomButton(0, 0);
      }
      GUI_Redraw();
      
      screencount += 1;
#ifndef ORIGIN
      degrees += 3.6;
      if (screencount%100 == 0){
        screencount  = 0;
        degrees = 0;
      }
#else
      if (screencount%100 == 0){
        screencount  = 0;
      }
#endif
    }

    
    //Sensors_I2C_ReadRegister((unsigned char)0x68, (unsigned char)MPU_WHOAMI, 1, inImu);    
    
    //==================================IMU================================
    unsigned long sensor_timestamp;
    int new_data = 0;
    
    get_tick_count(&timestamp);
    
#ifdef COMPASS_ENABLED
    /* We're not using a data ready interrupt for the compass, so we'll
    * make our compass reads timer-based instead.
    */
    if ((timestamp > hal.next_compass_ms) && !hal.lp_accel_mode &&
        hal.new_gyro && (hal.sensors & COMPASS_ON)) {
          hal.next_compass_ms = timestamp + COMPASS_READ_MS;
          new_compass = 1;
        }
#endif
    /* Temperature data doesn't need to be read with every gyro sample.
    * Let's make them timer-based like the compass reads.
    */
    if (timestamp > hal.next_temp_ms) {
      hal.next_temp_ms = timestamp + TEMP_READ_MS;
      new_temp = 1;
    }
    
    if (hal.motion_int_mode) {
      /* Enable motion interrupt. */
      mpu_lp_motion_interrupt(500, 1, 5);
      /* Notify the MPL that contiguity was broken. */
      inv_accel_was_turned_off();
      inv_gyro_was_turned_off();
      inv_compass_was_turned_off();
      inv_quaternion_sensor_was_turned_off();
      /* Wait for the MPU interrupt. */
      while (!hal.new_gyro) {}
      /* Restore the previous sensor configuration. */
      mpu_lp_motion_interrupt(0, 0, 0);
      hal.motion_int_mode = 0;
    }
    
    if (!hal.sensors || !hal.new_gyro) {
      continue;
    }    
    
    if (hal.new_gyro && hal.lp_accel_mode) {
      short accel_short[3];
      long accel[3];
      mpu_get_accel_reg(accel_short, &sensor_timestamp);
      accel[0] = (long)accel_short[0];
      accel[1] = (long)accel_short[1];
      accel[2] = (long)accel_short[2];
      inv_build_accel(accel, 0, sensor_timestamp);
      new_data = 1;
      hal.new_gyro = 0;
    } else if (hal.new_gyro && hal.dmp_on) {
      short gyro[3], accel_short[3], sensors;
      unsigned char more;
      long accel[3], quat[4], temperature;
      /* This function gets new data from the FIFO when the DMP is in
      * use. The FIFO can contain any combination of gyro, accel,
      * quaternion, and gesture data. The sensors parameter tells the
      * caller which data fields were actually populated with new data.
      * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
      * the FIFO isn't being filled with accel data.
      * The driver parses the gesture data to determine if a gesture
      * event has occurred; on an event, the application will be notified
      * via a callback (assuming that a callback function was properly
      * registered). The more parameter is non-zero if there are
      * leftover packets in the FIFO.
      */
      dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);
      if (!more)
        hal.new_gyro = 0;
      if (sensors & INV_XYZ_GYRO) {
        /* Push the new data to the MPL. */
        inv_build_gyro(gyro, sensor_timestamp);
        new_data = 1;
        if (new_temp) {
          new_temp = 0;
          /* Temperature only used for gyro temp comp. */
          mpu_get_temperature(&temperature, &sensor_timestamp);
          inv_build_temp(temperature, sensor_timestamp);
        }
      }
      if (sensors & INV_XYZ_ACCEL) {
        accel[0] = (long)accel_short[0];
        accel[1] = (long)accel_short[1];
        accel[2] = (long)accel_short[2];
        inv_build_accel(accel, 0, sensor_timestamp);
        new_data = 1;
      }
      if (sensors & INV_WXYZ_QUAT) {
        inv_build_quat(quat, 0, sensor_timestamp);
        new_data = 1;
      }
    } else if (hal.new_gyro) {
      short gyro[3], accel_short[3];
      unsigned char sensors, more;
      long accel[3], temperature;
      /* This function gets new data from the FIFO. The FIFO can contain
      * gyro, accel, both, or neither. The sensors parameter tells the
      * caller which data fields were actually populated with new data.
      * For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't
      * being filled with accel data. The more parameter is non-zero if
      * there are leftover packets in the FIFO. The HAL can use this
      * information to increase the frequency at which this function is
      * called.
      */
      hal.new_gyro = 0;
      mpu_read_fifo(gyro, accel_short, &sensor_timestamp,
                    &sensors, &more);
      if (more)
        hal.new_gyro = 1;
      if (sensors & INV_XYZ_GYRO) {
        /* Push the new data to the MPL. */
        inv_build_gyro(gyro, sensor_timestamp);
        new_data = 1;
        if (new_temp) {
          new_temp = 0;
          /* Temperature only used for gyro temp comp. */
          mpu_get_temperature(&temperature, &sensor_timestamp);
          inv_build_temp(temperature, sensor_timestamp);
        }
      }
      if (sensors & INV_XYZ_ACCEL) {
        accel[0] = (long)accel_short[0];
        accel[1] = (long)accel_short[1];
        accel[2] = (long)accel_short[2];
        inv_build_accel(accel, 0, sensor_timestamp);
        new_data = 1;
      }
    }
#ifdef COMPASS_ENABLED
    if (new_compass) {
      short compass_short[3];
      long compass[3];
      new_compass = 0;
      /* For any MPU device with an AKM on the auxiliary I2C bus, the raw
      * magnetometer registers are copied to special gyro registers.
      */
      if (!mpu_get_compass_reg(compass_short, &sensor_timestamp)) {
        compass[0] = (long)compass_short[0];
        compass[1] = (long)compass_short[1];
        compass[2] = (long)compass_short[2];
        /* NOTE: If using a third-party compass calibration library,
        * pass in the compass data in uT * 2^16 and set the second
        * parameter to INV_CALIBRATED | acc, where acc is the
        * accuracy from 0 to 3.
        */
        inv_build_compass(compass, 0, sensor_timestamp);
      }
      new_data = 1;
    }
#endif
    if (new_data) {
      inv_execute_on_data();
      /* This function reads bias-compensated sensor data and sensor
      * fusion outputs from the MPL. The outputs are formatted as seen
      * in eMPL_outputs.c. This function only needs to be called at the
      * rate requested by the host.
      */
      read_from_mpl();
    }
    
    //========================================IMU==================================
  }
}
Ejemplo n.º 7
0
void FastCharge(u8 batNum)
{
	u16 tempV,tempT;
	static u8 dropCount=0;	


	if(batNum <3)		
		tempT = getAverage(CHANNEL_TEMP_1);
	else
		tempT = getAverage(CHANNEL_TEMP_2);

	tempV = getVbatAdc(batNum);
	send(tempV);
	if(getDiffTickFromNow(gChargingTimeTick[batNum-1]) > BAT_START_DV_CHECK)  //hod-off time, in this period, we do NOT detect -dv
	{

		tempV = ((gBatVoltArray[batNum-1][0]<<2)+tempV)/5;
		send(tempV);
		//if(getDiffTickFromNow(gChargingIntervalTick[batNum-1]) > BAT_DV_CHECK_INTERVAL)
		{
				sendStr(enter);
			gChargingIntervalTick[batNum-1] = getSysTick();
			if(gChargingIntervalTick[batNum-1] == 0)
			{
				GIE = 0;
				shortTick = 1;
				GIE = 1;
				gChargingIntervalTick[batNum-1] = 1;
			}
			
			if(tempV >= CHARGING_FAST_MAX_VOLT || getDiffTickFromNow(gChargingTimeTick[batNum-1]) > BAT_CHARGING_FAST_MAX_TIME || tempT < ADC_TEMP_MAX)
			{
				gBatStateBuf[batNum] &= ~CHARGE_STATE_FAST;
				gBatStateBuf[batNum] |= CHARGE_STATE_TRICK;
				gBatStateBuf[batNum] |= CHARGE_STATE_FULL;
				gChargingTimeTick[batNum-1] = 0;
				gChargingIntervalTick[batNum-1] = 0;
				if(tempV >= CHARGING_FAST_MAX_VOLT)
				{
					sendStr(stopMaxVolt);
					sendStr(enter);
					LED_ON(3);
				}
				else
				{
					sendStr(stopMaxTime);
					sendStr(enter);
					LED_ON(4);
				}
				return;
			}
			if(tempV > gBatVoltArray[batNum-1][0])
			{
				gBatVoltArray[batNum-1][0] = tempV;
				dropCount = 0;
			}
			else
			{
				if((gBatVoltArray[batNum-1][0] - tempV) > ADC_DV_VOLT)
				{
					dropCount++;
					if(dropCount >5)
					{
					sendStr(stopdV);
					sendStr(enter);
					gBatStateBuf[batNum] &= ~CHARGE_STATE_FAST;
					gBatStateBuf[batNum] |= CHARGE_STATE_TRICK;
					gBatStateBuf[batNum] |= CHARGE_STATE_FULL;
					gChargingTimeTick[batNum-1] = 0;
					gChargingIntervalTick[batNum-1] = 0;
					LED_ON(1);
					}
				}
			}
		}
	}
	else
	{
			sendStr(enter);
		gBatVoltArray[batNum-1][0] = tempV;
		
		if(tempT < ADC_TEMP_MAX )
		{
			sendStr(stopMaxTemp);
			sendStr(enter);
			gBatStateBuf[batNum] &= ~CHARGE_STATE_FAST;
			gBatStateBuf[batNum] |= CHARGE_STATE_TRICK;
			gBatStateBuf[batNum] |= CHARGE_STATE_FULL;
			gChargingTimeTick[batNum-1] = 0;
			gChargingIntervalTick[batNum-1] = 0;
			LED_ON(4);
		}
	}

}
Ejemplo n.º 8
0
void chargeHandler(void)
{
	u16 tempV;
	
	//close all pwm
	//PB &= 0xF0;

	if(gBatNumNow ==0)
		return;

	if(ChargingTimeTick == 0)   
	{
		switch(gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] & 0x38)	
		{
			case CHARGE_STATE_FAST:
					skipCount = FAST_SKIP_COUNT; break; //BatFastCharge(ticknow%4);break;
		//	case CHARGE_STATE_SUP:
		//			skipCount = SUP_SKIP_COUNT;break;// BatSupCharge(ticknow%4);break;
			case CHARGE_STATE_PRE:
					skipCount = PRE_SKIP_COUNT;break;//BatPreCharge(ticknow%4);break;
			case CHARGE_STATE_TRICK:
					skipCount = TRI_SKIP_COUNT;break;//BatTrickCharge(ticknow%4);break;
			default:
					break;
		}

		ChargingTimeTick = getSysTick();
		if(ChargingTimeTick == 0)
		{
			GIE =0 ;
			shortTick = 1;
			GIE =1;
			ChargingTimeTick =1;
		}
		if(gIsChargingBatPos !=0)    //0 pos is a dummy
		{
			//电池类型错误    充电错误
			if(gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] & ((BAT_TYPE_ERROR) |(CHARGE_STATE_ERROR)))
			{
				tempV = getVbatAdc(gBatNowBuf[gIsChargingBatPos]);
				if(tempV < BAT_MIN_VOLT_OPEN)
				{
					ChargingTimeTick = 0;
					removeBat(gIsChargingBatPos);
				}	
				return;	
			}
	
			if(gChargeSkipCount[gBatNowBuf[gIsChargingBatPos]-1] >=skipCount)   //ok, it's pulse now
			{
				PB &= 0xF0;   //close all pwm
				PB |= (1<<(gBatNowBuf[gIsChargingBatPos]-1));
				gChargeSkipCount[gBatNowBuf[gIsChargingBatPos]-1] = 0;
			}
			else
				gChargeSkipCount[gBatNowBuf[gIsChargingBatPos] - 1]++;
		}
	}
	else
	{
		 if(gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] & BAT_DETECT_BIT)   //电池检测
		{
			if(getDiffTickFromNow(ChargingTimeTick)>BAT_CHARGING_DETECT_TIME)
			{
				tempV = getVbatAdc(gBatNowBuf[gIsChargingBatPos]);
				sendStr(f**k);
				send(tempV);
				sendStr(enter);
				if(tempV<BAT_MIN_VOLT_OPEN)   //电池被拔出
				{
					ChargingTimeTick = 0;
					removeBat(gIsChargingBatPos);
				}
				else if(tempV>BAT_MAX_VOLT_CLOSE || gChargeCurrent <3/*|| (gChargeCurrent<<3) <= ( tempV-gBatVoltArray[gBatNowBuf[gIsChargingBatPos]-1][0])*/)  //
				{
					gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] &= ~(BAT_DETECT_BIT |CHARGE_STATE_ALL);
					gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] |= BAT_TYPE_ERROR;
					gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] |= HAS_BATTERY;
					PB &= 0xF0;   //close current pwm channel
					ChargingTimeTick = 0;
				}
				else   //电池检测ok
				{
					//gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] &= ~ BAT_DETECT_BIT;
					gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] = 0;
					gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] |= HAS_BATTERY;
					if(gBatNowBuf[gIsChargingBatPos]<3)
						tempV = getAverage(CHANNEL_TEMP_1);
					else
						tempV = getAverage(CHANNEL_TEMP_2);
					if(tempV < ADC_TEMP_MAX || tempV > ADC_TEMP_MIN)
						gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] |= CHARGE_STATE_TRICK;
					else
					{
						if(gBatVoltArray[gBatNowBuf[gIsChargingBatPos]-1][0] < CHARGING_PRE_END_VOLT)
							gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] |= CHARGE_STATE_PRE;
						else if(gBatVoltArray[gBatNowBuf[gIsChargingBatPos]-1][0] < CHARGING_FAST_END_VOLT)
							gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] |= CHARGE_STATE_FAST;
						else
							gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] |= (CHARGE_STATE_TRICK | CHARGE_STATE_FULL);
					}
						
				}
  					
  			}
		}
		 else if(getDiffTickFromNow(ChargingTimeTick)  > BAT_CHARGING_PULSE_TIME)   //change to next channel
		{
			tempV = getVbatAdc(gBatNowBuf[gIsChargingBatPos]);
			if(PB & (1<<(gBatNowBuf[gIsChargingBatPos]-1)))
			{
				sendF(getSysTick());
				send(tempV);
				send(gChargeCurrent);
			}
			
			PB &= 0xF0;   //close current pwm channel

			if((gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] & 0x38) == CHARGE_STATE_FAST)
				FastCharge(gBatNowBuf[gIsChargingBatPos]);
			
			ChargingTimeTick = 0;
			if(gIsChargingBatPos >= gBatNumNow)
			{
				if(gBatNumNow%2)
				{
					gIsChargingBatPos =0;
				}
				else
					gIsChargingBatPos =1;
			}
			else
				gIsChargingBatPos++;
		}
		else if(gIsChargingBatPos !=0 && (getDiffTickFromNow(ChargingTimeTick) > BAT_CHARGING_TEST_TIME))
		{
			tempV = getVbatAdc(gBatNowBuf[gIsChargingBatPos]);
			//if((gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] & 0x38) == CHARGE_STATE_TRICK)
			//{
			//	if(getDiffTickFromNow(ChargingTimeTick) > BAT_CHARGING_TRICK_TIME)
			//		PB &=0xF0;
			//}
			/*
			if(gChargeCurrent<44)    // <1800mA
			{
				LED_OFF(3);LED_OFF(4);LED_ON(1);
			}
			else if(gChargeCurrent<54)  //<2200mA
			{
				LED_OFF(1);LED_OFF(3);LED_ON(4);
			}
			else
			{
				LED_OFF(1);LED_OFF(4);LED_ON(3);
			}
			*/
			if(tempV < BAT_MIN_VOLT_OPEN || ((PB & (1<<(gBatNowBuf[gIsChargingBatPos]-1))) &&gChargeCurrent  < 3) /*|| tempV > BAT_MAX_VOLT_CLOSE*/)
			{
					sendStr(stopError);
					sendStr(enter);
					ChargingTimeTick = 0;
					removeBat(gIsChargingBatPos);
			}
			else
			{
				if(gChargingTimeTick[gBatNowBuf[gIsChargingBatPos]-1] == 0)
				{
					gChargingTimeTick[gBatNowBuf[gIsChargingBatPos]-1] = getSysTick();
					if(gChargingTimeTick[gBatNowBuf[gIsChargingBatPos]-1] == 0)
					{
						GIE =0;
						shortTick=1;
						GIE= 1;
						gChargingTimeTick[gBatNowBuf[gIsChargingBatPos]-1] = 1;
					}
				}
				switch(gBatStateBuf[gBatNowBuf[gIsChargingBatPos]] & 0x38)
				{
				//	case CHARGE_STATE_FAST:
				//			FastCharge(gBatNowBuf[gIsChargingBatPos]);break;
				//	case CHARGE_STATE_SUP:
				//			 SupCharge(gBatNowBuf[gIsChargingBatPos]);break;
					case CHARGE_STATE_PRE:
							PreCharge(gBatNowBuf[gIsChargingBatPos]);break;
					case CHARGE_STATE_TRICK:
							TrickCharge(gBatNowBuf[gIsChargingBatPos]);break;
					default:
							break;
				}
			}
		}
	}
}
Ejemplo n.º 9
0
unsigned long getPressDuration(struct Button *butt){
  if(butt->output==0)
    return 0;
  return (getSysTick()-butt->startTime);
}