void init_leds()
{
	DD_REG_GREEN |= BIT_GREEN;
	
	LED_ON(GREEN);
	
	// leave on for 2 seconds
	for (int i=0;i<200;i++)
	WAIT_10MS;
		
	LED_OFF(GREEN);
}

/*..........................................................................*/
void BSP_displyPhilStat(uint8_t n, char const Q_ROM *stat) {
    if (stat[0] == (char const)'e') {        /* is this Philosopher eating? */
        LED_ON(n);
    }
    else {                                /* this Philosopher is not eating */
        LED_OFF(n);
    }
    QS_BEGIN(PHILO_STAT, AO_Philo[n])  /* application-specific record begin */
        QS_U8(1, n);                                  /* Philosopher number */
        QS_STR(stat);                                 /* Philosopher status */
    QS_END()
}

//进行ahrs校准器的运行，
void ahrs_aligner_run(void) {

  RATES_ADD(gyro_sum,  imu.gyro);
  VECT3_ADD(accel_sum, imu.accel);
  VECT3_ADD(mag_sum,   imu.mag);

  ref_sensor_samples[samples_idx] = imu.accel.z;//该数组大小为60（PERIDIC FREQUENCY）
  samples_idx++;//samples_idx从0开始

#ifdef AHRS_ALIGNER_LED
  RunOnceEvery(50, {LED_TOGGLE(AHRS_ALIGNER_LED)});//如果定义了ahrs校准器的指示灯时会让该灯以固定频率闪烁
#endif

  if (samples_idx >= SAMPLES_NB) {
    int32_t avg_ref_sensor = accel_sum.z;
    if ( avg_ref_sensor >= 0)
      avg_ref_sensor += SAMPLES_NB / 2;
    else
      avg_ref_sensor -= SAMPLES_NB / 2;
    avg_ref_sensor /= SAMPLES_NB;
    //噪声的误差计算
    ahrs_aligner.noise = 0;
    int i;
    for (i=0; i<SAMPLES_NB; i++) {
      int32_t diff = ref_sensor_samples[i] - avg_ref_sensor;
      ahrs_aligner.noise += abs(diff);
    }
    //存储平均值(60次)到ahrs校准的lp_xxx
    RATES_SDIV(ahrs_aligner.lp_gyro,  gyro_sum,  SAMPLES_NB);
    VECT3_SDIV(ahrs_aligner.lp_accel, accel_sum, SAMPLES_NB);
    VECT3_SDIV(ahrs_aligner.lp_mag,   mag_sum,   SAMPLES_NB);
    //清零
    INT_RATES_ZERO(gyro_sum);
    INT_VECT3_ZERO(accel_sum);
    INT_VECT3_ZERO(mag_sum);
    samples_idx = 0;

    if (ahrs_aligner.noise < LOW_NOISE_THRESHOLD)
      ahrs_aligner.low_noise_cnt++;
    else
      if ( ahrs_aligner.low_noise_cnt > 0)
        ahrs_aligner.low_noise_cnt--;

    if (ahrs_aligner.low_noise_cnt > LOW_NOISE_TIME) {
      ahrs_aligner.status = AHRS_ALIGNER_LOCKED;//如果ahrs校准器的噪声（noise）值低于阈值的次数为5次，那么ahrs校准器将关闭
#ifdef AHRS_ALIGNER_LED
      LED_ON(AHRS_ALIGNER_LED);//ahrs校准器关闭的话，对应的led灯就会关闭
#endif
    }
  }

}

static inline bool_t cut_accel (struct Int32Vect3 i1, struct Int32Vect3 i2, int32_t threshold) {
  struct Int32Vect3 diff;
  VECT3_DIFF(diff, i1, i2);
  if (diff.x < -threshold || diff.x > threshold ||
      diff.y < -threshold || diff.y > threshold ||
      diff.z < -threshold || diff.z > threshold) {
    LED_ON(4);
    return TRUE;
  } else {
    LED_OFF(4);
    return FALSE;
  }
}

static void NORETURN led_process(void)
{
	static int i = 0;
	static bool turn = true;

	/* Periodically blink the led (toggle each 100 ms) */
	while (1)
	{
		if (turn)
		{
			LED_ON(i);
			if (i)
				LED_OFF(i - 1);

			if (i == 2)
			{
				turn = false;
				timer_delay(70);
			}
			else
				i++;
		}
		else
		{
			LED_OFF(i);
			if (i > 0)
				LED_ON(i-1);

			if (!i)
			{
				turn = true;
				timer_delay(70);
			}
			else
				i--;
		}
		timer_delay(300);
	}
}

int  CreateMediaRender(char * buf)
{
    void *msg;
    tls_os_status_t status;
    u8 uuid[17] = {0};
    u8 *mac = wpa_supplicant_get_mac();
    sprintf((char *)uuid, "%02x%02x%02x%02x%02x%02x-dmr",
            mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
    tls_dmr_init((char *)uuid, (char *)uuid);

    tls_gpio_cfg(SPI_CLK, TLS_GPIO_DIR_OUTPUT, TLS_GPIO_ATTR_FLOATING);
    tls_gpio_cfg(SPI_DATO, TLS_GPIO_DIR_OUTPUT, TLS_GPIO_ATTR_FLOATING);
    tls_gpio_cfg(SPI_CCS, TLS_GPIO_DIR_OUTPUT, TLS_GPIO_ATTR_PULLHIGH);
    tls_gpio_cfg(DAT_REQ, TLS_GPIO_DIR_INPUT, TLS_GPIO_ATTR_FLOATING);
    tls_gpio_cfg(LED, TLS_GPIO_DIR_OUTPUT, TLS_GPIO_ATTR_PULLHIGH);
    tls_gpio_cfg(RST, TLS_GPIO_DIR_OUTPUT, TLS_GPIO_ATTR_PULLHIGH);

//	tls_gpio_cfg(RUN_MODE, TLS_GPIO_DIR_INPUT, TLS_GPIO_ATTR_FLOATING);

    SPI_CS(1);
    LED_ON(0);

    msg = tls_mem_alloc(64 * sizeof(void *));
    if (!msg)
        return -1;
    status = tls_os_queue_create(&sd_down_mbox, msg, 64, 0);
    if (status != TLS_OS_SUCCESS) {
        tls_mem_free(msg);
        return -1;
    }

    tls_os_task_create(NULL, NULL,
                       sd_down_thread,
                       NULL,
                       (void *)sd_down_task_stk,
                       UPNP_SD_STK_SIZE * sizeof(u32),
                       DEMO_DMR_TASK_PRIO,//DEMO_DMR_TASK_PRIO,
                       0);

    tls_dmr_set_play_callback(httpdownloadmusic);
    tls_dmr_set_stop_callback(httpstopdownloadmusic);
    tls_dmr_set_seek_callback(httpstopdownloadmusic);
    tls_dmr_set_pause_callback(httpstopdownloadmusic);
    tls_dmr_set_play_progress_callback(get_grogress);

    tls_dmr_set_mute_callback(mute_callback);
    tls_dmr_set_volume_callback(volume_callback);
    tls_dmr_set_volumedb_callback(volumedb_callback);
    tls_dmr_set_loudness_callback(loudness_callback);
    return 0;
}

void config_file_update()
{
	int i, group;
	static u4_t key_last_update;
	const scan_code *kp, *lp;
	key_last_t *kl;
	key_last_t key[N_GROUPS];

	for (i=1; i<N_GROUPS; i++) { key[i].key = 0; key[i].lp = 0; }

	for (i=0; i<N_KEYS; i++) {
		kp = &front_pnl_key[i];
		if (kp->group == 0) continue;
		assert(kp->group < N_GROUPS);
		lp = &front_pnl_led[i];
		
		if (LED_ON(lp)) {	// state of key LED determines whether we consider it "pushed" or not
			key[kp->group].key = KEY(i);
			key[kp->group].lp = lp;
		}
	}

	// check for state change of keys in a group
	for (i=1; i<N_GROUPS; i++) {
		kl = &key_last[i];

		if (kl->key != key[i].key) {
			kl->key = key[i].key;
			kl->lp = key[i].lp;
			key_need_update = TRUE;
		}
	}
	
	// only update file periodically
	if (key_need_update && (time_diff(timer_ms(), key_last_update) > 10000)) {
		FILE *fp;
		
		key_need_update = FALSE;
		key_last_update = timer_ms();

		sprintf(dbuf, "%s/.5370.%s.keys", ROOT_DIR, conf_profile);
		scallz("fopen", fp = fopen(dbuf, "w"));
		for (i=1; i<N_GROUPS; i++) {
			if (key_last[i].key) {
				//printf("store \"%s\": key 0x%02x %s\n", conf_profile, key_last[i].key, (key_last[i].lp)->name);
				fprintf(fp, "rcl key 0x%02x %s\n", key_last[i].key, (key_last[i].lp)->name);
			}
		}
		fclose(fp);
	}
}

void baro_periodic( void ) {
  // Run some loops to get correct readings from the adc
  if (startup_cnt > 0) {
    --startup_cnt;
#ifdef BARO_LED
    LED_TOGGLE(BARO_LED);
    if (startup_cnt == 0) {
      LED_ON(BARO_LED);
    }
#endif
  }
  // Read the ADC (at 50/4 Hz, conversion time is 68 ms)
  RunOnceEvery(4,mcp355x_read());
}

void SM_CANreceiver()
{
	
	if(CanMessage_PduHandler0.msg_data_field[0]==0x7F)
	{
		LED_ON(LED2);
		LED_OFF(LED1);	
		
	}
	else if(CanMessage_PduHandler0.msg_data_field[0]==0x67)
		{
			LED_ON(LED1);
			LED_OFF(LED2);	
		}
	
		else
		{
			LED_OFF(LED2);
			LED_OFF(LED1);
		}			
	
	
}

int main(void)
{
	init();

	LED_ON(LED_RED);

	for (;;)
	{
		kprintf("Hello world!\n");
		timer_delay(500);
	}

	return 0;
}

static void NORETURN hp_process(void)
{
	while (1)
	{
		sig_wait(SIG_USER0);
		#if CONFIG_USE_LED
			LED_ON();
		#endif
		#if CONFIG_USE_HP_TIMER
			end = timer_hw_hpread();
		#endif
		sig_send(main_proc, SIG_USER0);
	}
}

void baro_periodic(void)
{
  // check i2c_done
  if (!i2c_idle(&i2c2)) { return; }

  switch (baro_board.status) {
    case LBS_UNINITIALIZED:
      baro_board_send_reset();
      baro_board.status = LBS_RESETED;
      break;
    case LBS_RESETED:
      baro_board_send_config_abs();
      baro_board.status = LBS_INITIALIZING_ABS;
      break;
    case LBS_INITIALIZING_ABS:
      baro_board_set_current_register(BARO_ABS_ADDR, 0x00);
      baro_board.status = LBS_INITIALIZING_ABS_1;
      break;
    case LBS_INITIALIZING_ABS_1:
      baro_board_send_config_diff();
      baro_board.status = LBS_INITIALIZING_DIFF;
      break;
    case LBS_INITIALIZING_DIFF:
      baro_board_set_current_register(BARO_DIFF_ADDR, 0x00);
      baro_board.status = LBS_INITIALIZING_DIFF_1;
      //    baro_board.status = LBS_UNINITIALIZED;
      break;
    case LBS_INITIALIZING_DIFF_1:
      baro_board.running = true;
      /* Falls through. */
    case LBS_READ_DIFF:
      baro_board_read_from_current_register(BARO_ABS_ADDR);
      baro_board.status = LBS_READING_ABS;
      break;
    case LBS_READ_ABS:
      baro_board_read_from_current_register(BARO_DIFF_ADDR);
      baro_board.status = LBS_READING_DIFF;
      break;
    default:
      break;
  }

#ifdef BARO_LED
  if (baro_board.running == TRUE) {
    LED_ON(BARO_LED);
  } else {
    LED_TOGGLE(BARO_LED);
  }
#endif
}

void mark(uint8_t t)
{
	volatile unsigned int i;
	adf7020_1_ook(1);
//	P1OUT &= ~BIT1;
	LED_ON();
	for(i = 0; i < t; i++)
	{                         // Turn On
		delay();
	}
	adf7020_1_ook(0);
	LED_OFF();                          // Turn Off
	delay();
}

/*..........................................................................*/
__ramfunc
void QK_onIdle(void) {

    /* toggle first LED on and off, see NOTE01 */
    QF_INT_DISABLE();
    LED_ON(3);  /* turn LED on  */
    LED_OFF(3); /* turn LED off */
    QF_INT_ENABLE();

#ifdef NDEBUG /* only if not debugging (idle mode hinders debugging) */
    AT91C_BASE_PMC->PMC_SCDR = 1;/* Power-Management: disable the CPU clock */
    /* NOTE: an interrupt starts the CPU clock again */
#endif
}

static void NORETURN led_process(void)
{
	int i;

	/* Periodically blink the led (toggle each 100 ms) */
	for (i = 0; ; i = !i)
	{
		if (i)
			LED_ON();
		else
			LED_OFF();
		timer_delay(100);
	}
}

/* Sets the actual actuator commands */
STATIC_INLINE void main_periodic(void)
{
  /* Inter-MCU watchdog */
  intermcu_periodic();

  /* Safety check and set FBW mode */
  fbw_safety_check();

#ifdef BOARD_PX4IO
  //due to a baud rate issue on PX4, for a few seconds the baud is 1500000 however this may result in package loss, causing the motors to spin at random
  //to prevent this situation:
  if (intermcu.stable_px4_baud != PPRZ_BAUD) {
    fbw_mode = FBW_MODE_FAILSAFE;
    fbw_motors_on = false;
    //signal to user whether fbw can be flashed:
#ifdef FBW_MODE_LED
    LED_OFF(FBW_MODE_LED); // causes really fast blinking
#endif
  }
#endif

  // TODO make module out of led blink?
#ifdef FBW_MODE_LED
  static uint16_t dv = 0;
  if (fbw_mode == FBW_MODE_FAILSAFE) {
    if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
  } else if (fbw_mode == FBW_MODE_MANUAL) {
    if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
  } else if (fbw_mode == FBW_MODE_AUTO) {
    LED_ON(FBW_MODE_LED);
  }
#endif // FWB_MODE_LED

  /* Set failsafe commands */
  if (fbw_mode == FBW_MODE_FAILSAFE) {
    fbw_motors_on = false;
    SetCommands(commands_failsafe);
  }

  /* If in auto copy autopilot motors on */
  if (fbw_mode == FBW_MODE_AUTO) {
    fbw_motors_on = autopilot_motors_on;
  }

  /* Set actuators */
  SetActuatorsFromCommands(commands, autopilot_mode);

  /* Periodic blinking */
  RunOnceEvery(10, LED_PERIODIC());
}

static inline void main_periodic( void ) {
  char ch;

  Uart1Transmit('a');
  Uart2Transmit('b');
  Uart3Transmit('c');
  Uart5Transmit('d');

  LED_OFF(1);
  LED_OFF(2);

  if (Uart1ChAvailable()) {
    ch = Uart1Getch();
    if (ch == 'a') {
      LED_ON(1);
    } else {
      LED_ON(2);
    }
  }

  if (Uart2ChAvailable()) {
    ch = Uart2Getch();
    if (ch == 'b') {
      LED_ON(1);
    } else {
      LED_ON(2);
    }
  }

  if (Uart3ChAvailable()) {
    ch = Uart3Getch();
    if (ch == 'c') {
      LED_ON(1);
    } else {
      LED_ON(2);
    }
  }

  if (Uart5ChAvailable()) {
    ch = Uart5Getch();
    if (ch == 'd') {
      LED_ON(1);
    } else {
      LED_ON(2);
    }
  }
}

static inline void main_periodic( void ) {
  char ch;

  uart_transmit(&uart1, 'a');
  uart_transmit(&uart2, 'b');
  uart_transmit(&uart3, 'c');
  uart_transmit(&uart5, 'd');

  LED_OFF(1);
  LED_OFF(2);

  if (uart_char_available(&uart1)) {
    ch = uart_getch(&uart1);
    if (ch == 'a') {
      LED_ON(1);
    } else {
      LED_ON(2);
    }
  }

  if (uart_char_available(&uart2)) {
    ch =  uart_getch(&uart2);
    if (ch == 'b') {
      LED_ON(1);
    } else {
      LED_ON(2);
    }
  }

  if (uart_char_available(&uart3)) {
    ch =  uart_getch(&uart3);
    if (ch == 'c') {
      LED_ON(1);
    } else {
      LED_ON(2);
    }
  }

  if (uart_char_available(&uart5)) {
    ch =  uart_getch(&uart5);
    if (ch == 'd') {
      LED_ON(1);
    } else {
      LED_ON(2);
    }
  }
}

/**
 * The entry point to the application.
 */
int main(void) {
  /* Hardware Setup - Don't leave MCLK floating */
  LPC_GPIO0->DIR |= (1 << 1);

  SystemInit();

  /* Start the SPI Bus first, that's really important */
  general_spi_init();

  /* Power monitoring - Turn off the battery measurement circuit */
  pwrmon_init();

  /* LED */
  LED_ON();

  /* Initialise the flash memory first so this gets off the SPI bus */
  flash_spi_init();
  flash_init();
  flash_setup();
  spi_shutdown();

  /* Optionally wipe the memory. This may take a few seconds... */
  wipe_mem();

  /* Initialise the memory writing code */
  init_write();

  /* Try to initialise the audio interface */
  if (wm8737_init() < 0) { /* If it fails */
    while (1); /* Wait here forever! */
  }

  /**
   * This delay of approximately 5 seconds is so we can
   * re-program the chip before it goes to sleep
   */
  uint32_t i = 1000*1000*3;
  while (i-- > 0);

  /* Initialise the radio stack */
  radio_init(radio_rx_callback);
  /* Initialise the time */
  time_init();

  /* Sleep forever, let the wakeup loop in sleeping.c handle everything */
  infinite_deep_sleep();

  return 0;
}

static inline void main_event_task(void)
{

  if (new_can_data) {
    if (rx_data[0] & 0x10) {
      LED_ON(2);
    } else {
      LED_OFF(2);
    }
  }

  if (new_can_data) {
    if (rx_data[0] & 0x20) {
      LED_ON(3);
    } else {
      LED_OFF(3);
    }
  }

  if (new_can_data) {
    if (rx_data[0] & 0x40) {
      LED_ON(4);
    } else {
      LED_OFF(4);
    }
  }

  if (new_can_data) {
    if (rx_data[0] & 0x80) {
      LED_ON(5);
    } else {
      LED_OFF(5);
    }
  }

}