C++ LED_TOGGLE函数代码示例

void dl_parse_msg( void ) {
    uint8_t msg_id = IdOfMsg(dl_buffer);
    if (msg_id == DL_SET_ACTUATOR) {
        uint8_t servo_no = DL_SET_ACTUATOR_no(dl_buffer);
        uint16_t servo_value = DL_SET_ACTUATOR_value(dl_buffer);
        LED_TOGGLE(2);
        if (servo_no < SERVOS_NB)
            SetServo(servo_no, servo_value);
    }
#ifdef DlSetting
    else if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
        uint8_t i = DL_SETTING_index(dl_buffer);
        float val = DL_SETTING_value(dl_buffer);
        DlSetting(i, val);
        LED_TOGGLE(2);
        for (int j=0 ; j<8 ; j++) {
            SetServo(j,actuators[j]);
        }
        DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
    } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
        uint8_t i = DL_GET_SETTING_index(dl_buffer);
        float val = settings_get_value(i);
        DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
    }
#endif
}

void parse_mavpilot_msg( void )
{
  if (intermcu_data.msg_class == MSG_INTERMCU_ID)
  {
    if (intermcu_data.msg_id == MSG_INTERMCU_COMMAND_ID)
    {
#if COMMANDS_NB > 8
#error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED"
#endif

      for (int i=0; i< COMMANDS_NB; i++)
      {
        ap_state->commands[i] = ((pprz_t)MSG_INTERMCU_COMMAND(intermcu_data.msg_buf, i));
      }

#ifdef LINK_MCU_LED
      LED_TOGGLE(LINK_MCU_LED);
#endif
      inter_mcu_received_ap = TRUE;
    }
    else if (intermcu_data.msg_id == MSG_INTERMCU_RADIO_ID)
    {
#if RADIO_CONTROL_NB_CHANNEL > 10
#error "INTERMCU UART CAN ONLY SEND 10 RADIO CHANNELS OR THE UART WILL BE OVERFILLED"
#endif

      for (int i=0; i< RADIO_CONTROL_NB_CHANNEL; i++)
      {
        fbw_state->channels[i] = ((pprz_t)MSG_INTERMCU_RADIO(intermcu_data.msg_buf, i));
      }
    }
    else if (intermcu_data.msg_id == MSG_INTERMCU_TRIM_ID)
    {
      ap_state->command_roll_trim  = ((pprz_t) MSG_INTERMCU_TRIM_ROLL(intermcu_data.msg_buf));
      ap_state->command_pitch_trim = ((pprz_t) MSG_INTERMCU_TRIM_PITCH(intermcu_data.msg_buf));
    }
    else if (intermcu_data.msg_id == MSG_INTERMCU_FBW_ID)
    {
      fbw_state->ppm_cpt = MSG_INTERMCU_FBW_MOD(intermcu_data.msg_buf);
      fbw_state->status = MSG_INTERMCU_FBW_STAT(intermcu_data.msg_buf);
      fbw_state->nb_err = MSG_INTERMCU_FBW_ERR(intermcu_data.msg_buf);
      fbw_state->vsupply = MSG_INTERMCU_FBW_VOLT(intermcu_data.msg_buf);
      fbw_state->current = MSG_INTERMCU_FBW_CURRENT(intermcu_data.msg_buf);

#ifdef LINK_MCU_LED
      LED_TOGGLE(LINK_MCU_LED);
#endif
      inter_mcu_received_fbw = TRUE;
    }
  }
}

/* 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());
}

void led_update(enum led_status ls)
{
	static bool is_pwm = false;
	static bool step_sign = false;
	static const int pwm_max = 100;
	static const int pwm_one_step = 10;
	static int pwm_last = 0;

	if (!is_pwm && ls == LST_NORMAL) {
		LED_ENABLE_PWM_MODE();
		is_pwm = true;
	}
	else if (is_pwm && ls != LST_NORMAL) {
		LED_DISABLE_PWM_MODE();
		is_pwm = false;
	}

	if (ls == LST_FAIL) {
		LED_ON();
	}
	else if (ls == LST_NORMAL) {
		pwm_last += (step_sign) ? -pwm_one_step : pwm_one_step;
		if (0 > pwm_last || pwm_last > pwm_max) {
			step_sign = !step_sign;
			pwm_last += (step_sign) ? -pwm_one_step : pwm_one_step;
		}

		LED_PWM(pwm_last);
	}
	else { /* INIT */
		LED_TOGGLE();
	}
}

static inline void main_periodic_task( void ) {
  LED_TOGGLE(1);
  //  DOWNLINK_SEND_TAKEOFF(&cpu_time_sec);
  usb_serial_transmit( 'A' );
  usb_serial_transmit( '\n' );

}

void parse_ins_msg(void)
{
  struct link_device *dev = InsLinkDevice;
  while (dev->char_available(dev->periph)) {
    uint8_t ch = dev->get_byte(dev->periph);

    if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
      if (CHIMU_DATA.m_MsgID == 0x03) {
        new_ins_attitude = 1;
        RunOnceEvery(25, LED_TOGGLE(3));
        if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
          CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
        }

        struct FloatEulers att = {
          CHIMU_DATA.m_attitude.euler.phi,
          CHIMU_DATA.m_attitude.euler.theta,
          CHIMU_DATA.m_attitude.euler.psi
        };
        stateSetNedToBodyEulers_f(&att);
        ahrs_chimu.is_aligned = TRUE;
#if CHIMU_DOWNLINK_IMMEDIATE
        DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi,
                                 &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
#endif

      }
    }
  }
}

int main()
{
	LED_OUTPUT();

	usi_init_master();
	volatile TWRESULT status = usi_start_master(0x08, 1);
	if (status != TWST_OK)
	{
		while(1)
		{
			LED_TOGGLE();
			_delay_ms(100);
		}
	}

	LED_HIGH();
	
	for (uint8_t i = 0; i < 25; ++i)
	{
		volatile uint8_t data = usi_read_master(0);
	}
	usi_read_master(1);

	usi_stop();
	LED_LOW();

	while(1);
	return 0;
}

void buttons_leds_timer()
{
	static uint8_t led_timer = 0;
	
	if (led_timer > 0)
		led_timer--;
		
	switch (led_state)
	{
		case led_on:
			led_timer = 0;
			LED_ON();
			break;
			
		case led_blink_slow:
		case led_blink_fast:
			if (led_timer == 0)
			{
				led_timer = led_state == led_blink_fast ? HM_TIMER_TICKS_FROM_MS(150) : HM_TIMER_TICKS_FROM_MS(300);
				LED_TOGGLE();
			}				
			break;
		
		default:
			led_timer = 0;
			LED_OFF();
	}
	
}

void mpu9250_calibrate_gyro_offset(imu_calibrated_offset_t *imu_offset, uint16_t count)
{

	imu_unscaled_data_t mpu9250_cache_unscaled_data;
	imu_data_t mpu9250_cache_average_data;

	mpu9250_cache_average_data.gyro[0] = 0.0;
	mpu9250_cache_average_data.gyro[1] = 0.0;
	mpu9250_cache_average_data.gyro[2] = 0.0;
	uint16_t i = 0;

	for (i = 0; i < count; i++) {

		mpu9250_read_accel_temp_gyro(&mpu9250_cache_unscaled_data);
		mpu9250_cache_average_data.gyro[0] += ((float)mpu9250_cache_unscaled_data.gyro[0]) / (float)count;
		mpu9250_cache_average_data.gyro[1] += ((float)mpu9250_cache_unscaled_data.gyro[1]) / (float)count;
		mpu9250_cache_average_data.gyro[2] += ((float)mpu9250_cache_unscaled_data.gyro[2]) / (float)count;

		mpu9250_delay(100);
		LED_TOGGLE(LED2);
	}

	imu_offset->gyro[0] = (int16_t)mpu9250_cache_average_data.gyro[0];
	imu_offset->gyro[1] = (int16_t)mpu9250_cache_average_data.gyro[1];
	imu_offset->gyro[2] = (int16_t)mpu9250_cache_average_data.gyro[2];

}

/* Process that reads from the USB port and writes to the UART port */
static void NORETURN usb_serial_process(void)
{
	iptr_t type = proc_currentUserData();

	KFile *in_fd = (type == USB_TO_SERIAL) ? &usb_port.fd : &ser_port.fd;
	KFile *out_fd = (type == USB_TO_SERIAL) ? &ser_port.fd : &usb_port.fd;

	while (1)
	{
		int c;

		c = kfile_getc(in_fd);
		if (UNLIKELY(c == EOF))
		{
			kfile_clearerr(in_fd);
			continue;
		}
		kfile_putc(c, out_fd);

		/*
		 * Toggle the STAT LED when some data passes through the
		 * usb-seral link
		 */
		LED_TOGGLE();
	}
}

static inline void led_toggle(void)
{

#ifdef BOARD_LISA_L
  LED_TOGGLE(7);
#endif
}

// FIXME : nb_tick rollover ???
//
// 97 days at 512hz
// 12 hours at 100khz
//
static inline void sys_tick_irq_handler(void) {

  /* set match register for next interrupt */
  T0MR0 += sys_time.resolution_cpu_ticks - 1;

  sys_time.nb_tick++;
  sys_time.nb_sec_rem += sys_time.resolution_cpu_ticks;
  if (sys_time.nb_sec_rem >= sys_time.cpu_ticks_per_sec) {
    sys_time.nb_sec_rem -= sys_time.cpu_ticks_per_sec;
    sys_time.nb_sec++;
#ifdef SYS_TIME_LED
    LED_TOGGLE(SYS_TIME_LED);
#endif
  }
  for (unsigned int i=0; i<SYS_TIME_NB_TIMER; i++) {
    if (sys_time.timer[i].in_use &&
        sys_time.nb_tick >= sys_time.timer[i].end_time) {
      sys_time.timer[i].end_time += sys_time.timer[i].duration;
      sys_time.timer[i].elapsed = TRUE;
      if (sys_time.timer[i].cb) {
        sys_time.timer[i].cb(i);
      }
    }
  }
}

void tmr_test(void)
{
    // Create a timer object
    tmr_t tmr;

    // Initialise PIO
    BIT_SET_HI(PORT_LED_O, BIT_LED_O);
    BIT_SET_HI(DDR_LED_O,  BIT_LED_O);

    // Initialise module
    pit_init();

    // Enable global interrupts
    sei();

    // Start timer with a 1s timeout
    tmr_start(&tmr, TMR_MS_TO_TICKS(1000));

    for(;;)
    {
        // Wait until timer has expired
        while(!tmr_has_expired(&tmr))
        {
            ;
        }

        // Restart timer
        tmr_restart(&tmr);

        // Toggle LED
        LED_TOGGLE();
    }
}

/*!
 * @brief Main function
 */
int main(void)
{
    volatile uint32_t i;
    uint32_t sysFreq;

    /* Structure for OSC configuration */
    osc_config_t oscConfig;
    oscConfig.freq = BOARD_XTAL0_CLK_HZ;
    oscConfig.capLoad = 0U;
    oscConfig.workMode = kOSC_ModeOscLowPower;
    oscConfig.oscerConfig.enableMode = kOSC_ErClkEnable;

    BOARD_InitPins();
    CLOCK_InitOsc0(&oscConfig);

    CLOCK_SetXtal0Freq(BOARD_XTAL0_CLK_HZ);

    /* Set clock divider to safe value to switch mode */
    CLOCK_SetSimSafeDivs();

#if (defined(FSL_FEATURE_MCG_HAS_PLL_INTERNAL_MODE) && FSL_FEATURE_MCG_HAS_PLL_INTERNAL_MODE)
    /* Calculate frdiv */
    if (!APP_GetAvailableFrdiv())
    {
        while (1)
        {
        }
    }
#endif /* FSL_FEATURE_MCG_HAS_PLL_INTERNAL_MODE || FSL_FEATURE_MCG_USE_PLLREFSEL */

    /* Configure pll */
    if (!APP_GetAvailablePllConfig(&g_pllConfig))
    {
        while (1)
        {
        }
    }

    APP_BootToPeeExample();

    /* Change clock PEE -> PBE -> BLPE */
    APP_ChangePeeToBlpeExample();

    /* Change clock BLPE -> PBE -> PEE */
    APP_ChangeBlpeToPeeExample();

    /* Get System clock to blink a LED */
    sysFreq = CLOCK_GetFreq(kCLOCK_CoreSysClk) / 20U;
    /* Enable a LED */
    LED_INIT();
    /* Blink a LED */
    while (1)
    {
        for (i = 0; i < sysFreq; i++)
        {
            __NOP();
        }
        LED_TOGGLE();
    }
}

void parse_ins_msg( void )
{
  while (InsLink(ChAvailable()))
  {
    uint8_t ch = InsLink(Getch());
    
    if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
    {
      if(CHIMU_DATA.m_MsgID==0x03)
      {
	new_ins_attitude = 1;
	RunOnceEvery(25, LED_TOGGLE(3) );
	if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
	{
	  CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
	}
	
	EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
	EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta);
      }
      else if(CHIMU_DATA.m_MsgID==0x02)
      {
	
	RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
	
      }
    }
  }
}