void 
Menu::Walker::on_key_down(uint8_t nr)
{
  // Access the current menu item
  Menu::item_list_P menu = m_stack[m_top];
  Menu::item_vec_P list = (Menu::item_vec_P) pgm_read_word(&menu->list);
  Menu::item_P item = (Menu::item_P) pgm_read_word(&list[m_ix]);
  Menu::type_t type = (Menu::type_t) pgm_read_byte(&item->type);
  
  // React to key event
  switch (nr) {
  case NO_KEY: 
    break;
  case SELECT_KEY: 
  case RIGHT_KEY:
    switch (type) {
    case Menu::ZERO_OR_MANY:
      // Select zero-or-many variable or toggle current item
      {
	if (!m_selected) {
	  m_selected = true;
	  m_bv = 0;
	  break;
	}
	Menu::zero_or_many_P var = (Menu::zero_or_many_P) item;
	uint16_t* vp = (uint16_t*) pgm_read_word(&var->value);
	list = (Menu::item_vec_P) pgm_read_word(&var->list);	  
	item = (Menu::item_P) pgm_read_word(&list[m_bv]);
	uint16_t value = *vp;
	if ((value & _BV(m_bv)) == 0)
	  *vp = (value | _BV(m_bv));
	else 
	  *vp = (value & ~_BV(m_bv));
      }
      break;
    case Menu::ITEM_LIST:
      // Walk into sub-menu
      {
	m_stack[++m_top] = (Menu::item_list_P) item;
	m_ix = 0;
      }
      break;
    case Menu::ACTION:
      // Execute action and fall back to menu root
      {
	Menu::action_P action = (Menu::action_P) item;
	Menu::Action* obj = (Menu::Action*) pgm_read_word(&action->obj);
	bool res = obj->run(item);
	m_top = 0;
	m_ix = 0;
	if (!res) return;
      }
      break;
    default:
      // Enter item modification mode
      m_selected = !m_selected;
      m_bv = 0;
    }
    break;
  case LEFT_KEY: 
    // Exit item modification mode or walk back
    if (m_selected) {
      m_selected = false;
    } 
    else if (m_top > 0) {
      m_top -= 1;
      m_ix = 0;
    }
    break;
  case DOWN_KEY: 
    // Step to the next menu item or value in item modification mode
    if (!m_selected) {
      m_ix += 1;
      item = (Menu::item_P) pgm_read_word(&list[m_ix]);
      if (item == 0) m_ix -= 1;
    }
    else {
      switch (type) {
      case Menu::ONE_OF:
	// Step to the next enumeration value
	{
	  Menu::one_of_P evar = (Menu::one_of_P) item;
	  uint16_t* vp = (uint16_t*) pgm_read_word(&evar->value);
	  uint16_t value = *vp + 1;
	  list = (Menu::item_vec_P) pgm_read_word(&evar->list);	  
	  item = (Menu::item_P) pgm_read_word(&list[value]);
	  if (item == 0) break;
	  *vp = value;
	}
	break;
      case Menu::ZERO_OR_MANY:
	// Step to the next item
	{
	  Menu::zero_or_many_P bitset = (Menu::zero_or_many_P) item;
	  list = (Menu::item_vec_P) pgm_read_word(&bitset->list);	  
	  item = (Menu::item_P) pgm_read_word(&list[m_bv + 1]);
	  if (item == 0) break;
	  m_bv += 1;
	}
	break;
//.........这里部分代码省略.........

uint8_t keymap_fn_layer(uint8_t index)
{
    return pgm_read_byte(&fn_layer[index]);
}

void TinyXML::processChar(uint8_t ch)
{
    uint16_t chToParse;
    boolean bMatch=false;
    while (!bMatch)
    {
        chToParse = pgm_read_word(&(pTable[currentState].charToParse));
        switch ( chToParse )
        {
        case whiteSpace:
            if (ch == ' ' || ch == '\t' || ch == '\n' | ch == '\r') bMatch=true;
            break;
        case alpha:
            if (isAlpha(ch))  bMatch=true;
            break;
        case alphanum:
            if (isAlpha(ch) || isNumeric(ch) || (ch == ':') || (ch == '_') || (ch == '-'))  bMatch=true;
            break;
        case quote:
            if (ch == '"' || ch == '\'')
            {
                matchQuote = ch;
                bMatch=true;
            }
            break;
        case matchingquote:
            if (ch == matchQuote) bMatch=true;
            break;
        case anychar:
            bMatch=true;
            break;
        default:
            if (ch == chToParse) bMatch=true;
            break;
        }
        if (!bMatch)
        {
#if DEBUG > 3
            Serial.print("Non-matching state:");
            Serial.print(currentState,DEC);
            Serial.print(" ch:");
            Serial.print(ch,HEX);
            Serial.print(" match criteria:");
            Serial.print(chToParse,HEX);
            Serial.print(" new state:");
            Serial.println(currentState+1,DEC);
#endif
            currentState++;
        }
    } // as every table enry must end in anychar we must get out of here

#if DEBUG > 2
    Serial.print("Matching state:");
    Serial.print(currentState,DEC);
    Serial.print(" ch:");
    Serial.print(ch,HEX);
    Serial.print(" match criteria:");
    Serial.print(chToParse,HEX);
    Serial.print(" tagBufferPtr:");
    Serial.print(tagBufferPtr,DEC);
    Serial.print(" new state:");
    Serial.println(pgm_read_byte(&(pTable[currentState].nextState)),DEC);
#endif
    action(ch, pgm_read_byte(&(pTable[currentState].actionNumber)));
    action(ch, pgm_read_byte(&(pTable[currentState].actionNumber2)));

    currentState=pgm_read_byte(&(pTable[currentState].nextState));
}

void USB_sendstring_P(PGM_P s)	/// send string from PROGMEM
{
	uint8_t b;
	while ( (b = pgm_read_byte(s++)) ) RingBuffer_Insert(&USARTtoUSB_Buffer, b);
}

//converts temp from ADC reading to temp in F
uint16_t convert_temp(uint16_t sample)
{
    uint16_t ret;
    ret = pgm_read_byte(&temps_vals[sample]);
    return ret; 
}

void radio_set_param(radio_attribute_t attr, radio_param_t parm)
{
    switch (attr)
    {
        case phyCurrentChannel:
            if (((int)parm.channel >= TRX_MIN_CHANNEL) &&
                ((int)parm.channel <= TRX_MAX_CHANNEL))
            {
                trx_bit_write(SR_CHANNEL, parm.channel);
                radiostatus.channel = parm.channel;
            }
            else
            {
                radio_error(SET_PARM_FAILED);
            }
            break;

        case phyTransmitPower:
            if (parm.tx_pwr >= -17 && parm.tx_pwr <= 3)
            {
                /** @todo move this into a radio-specific header file */
                static const uint8_t pwrtable[] =
                {
                    0x0F, 0x0F, 0x0F, 0x0F, 0x0F, /* -17...-13 dBm */
                    0x0E, 0x0E, 0x0E,             /* -12...-10 dBm */
                    0x0D, 0x0D,                   /* -9...-8 dBm */
                    0x0C, 0x0C,                   /* -7...-6 dBm */
                    0x0B,                         /* -5 dBm */
                    0x0A,                         /* -4 dBm */
                    0x09,                         /* -3 dBm */
                    0x08,                         /* -2 dBm */
                    0x07,                         /* -1 dBm */
                    0x06,                         /* 0 dBm */
                    0x04,                         /* 1 dBm */
                    0x02,                         /* 2 dBm */
                    0x00                          /* 3 dBm */
                };
                radiostatus.tx_pwr = parm.tx_pwr;
                uint8_t idx = parm.tx_pwr + 17;
                uint8_t pwrval = pgm_read_byte(pwrtable[idx]);
                trx_bit_write(SR_TX_PWR, pwrval);
            }
            else
            {
                radio_error(SET_PARM_FAILED);
            }
            break;

        case phyCCAMode:
            if (parm.cca_mode <= 3)
            {
                radiostatus.cca_mode = parm.cca_mode;
                trx_bit_write(SR_CCA_MODE, radiostatus.cca_mode);
            }
            else
            {
                radio_error(SET_PARM_FAILED);
            }
            break;

        case phyIdleState:
            radiostatus.idle_state = parm.idle_state;
            radio_set_state(parm.idle_state);
            break;

        case phyChannelsSupported:
            break;

        case phyPanId:
            trx_set_panid(parm.pan_id);
            break;

        case phyShortAddr:
            trx_set_shortaddr(parm.short_addr);
            break;

        case phyLongAddr:
        {
            uint8_t regno, *ap;
            for (regno = RG_IEEE_ADDR_0, ap = (uint8_t *)parm.long_addr;
                 regno <= RG_IEEE_ADDR_7;
                 regno++, ap++)
                trx_reg_write(regno, *ap);
            break;
        }

        case phyDataRate:
            trx_set_datarate(parm.data_rate);
            break;

        default:
            radio_error(SET_PARM_FAILED);
            break;
    }
}

uint8_t REDFLY::cmd(uint8_t *dst, uint8_t dst_size, PGM_P p1, char *v1, PGM_P p2, uint8_t *v2, uint16_t v2_size)
{
  uint8_t c, i;
  uint32_t ms;
  uint32_t timeout;
  uint8_t buf[8]; //ERRORx

  if(read_state == 2) //currently receiving data?
  {
    return 0xFF;
  }
  else if(available()) //check for new data
  {
    uint8_t sock=INVALID_SOCKET;
    uint16_t len=0;
    socketRead(&sock, &len, 0, 0, 0, 0);
    if(len != 0) //rx data found
    {
      return 0xFF;
    }
  }

  //send p1 command
  c = pgm_read_byte(p1++);
  while(c != 0)
  {
    write(c);
    c = pgm_read_byte(p1++);
  }

  //send v1 parameter 1
  if(v1)
  {
    while(*v1)
    {
      write(*v1++);
    }
  }

  //send p2 parameter 2
  if(p2)
  {
    c = pgm_read_byte(p2++);
    while(c != 0)
    {
      if(c == 0xDB) //0xDB -> 0xDB 0xDD
      {
        write(0xDB);
        write(0xDD);
        c = pgm_read_byte(p2++);
      }
      else if((c == 0x0D) && (pgm_read_byte(p2) == 0x0A)) //\r\n -> 0xDB 0xDC
      {
        write(0xDB);
        write(0xDC);
        p2++;
        c = pgm_read_byte(p2++);
      }
      else
      {
        write(c);
        c = pgm_read_byte(p2++);
      }
    }
  }

  //send v2 parameter 3
  if(v2_size)
  {
    while(v2_size)
    {
      if(v2[0] == 0xDB) //0xDB -> 0xDB 0xDD
      {
        write(0xDB);
        write(0xDD);
        v2++; v2_size--;
      }
      else if((v2[0] == 0x0D) && (v2[1] == 0x0A) && (v2_size >= 2)) //\r\n -> 0xDB 0xDC
      {
        write(0xDB);
        write(0xDC);
        v2+=2; v2_size-=2;
      }
      else
      {
        write(*v2++);
        v2_size--;
      }
    }
  }

  //flush rx and tx buffer
  flush_nowait();

  //send end characters of command
  write('\r');
  write('\n');

  //read response
  timeout = 10000; //default timeout: 10s
//.........这里部分代码省略.........

void printPSTR1 ( PGM_P s) 
{
  char c;
  while ((c=pgm_read_byte(s++)) != 0)
    Serial.print (c);
} 

uint8_t gamma_correct(uint8_t input) { return pgm_read_byte(&gamma[input]); }

/** 
 *	This function is called by the library when in device mode, and must be overridden (see library "USB Descriptors"
 *  documentation) by the application code so that the address and size of a requested descriptor can be given
 *  to the USB library. When the device receives a Get Descriptor request on the control endpoint, this function
 *  is called so that the descriptor details can be passed back and the appropriate descriptor sent back to the
 *  USB host.
 */
uint16_t CALLBACK_USB_GetDescriptor(const uint16_t wValue,
                                    const uint8_t wIndex,
                                    void** const DescriptorAddress)
{
	const uint8_t  DescriptorType   = (wValue >> 8);
	const uint8_t  DescriptorNumber = (wValue & 0xFF);

	void*    Address = NULL;
	uint16_t Size    = NO_DESCRIPTOR;

	switch (DescriptorType)
	{
		case DTYPE_Device: 
			Address = (void*)&DeviceDescriptor;
			Size    = sizeof(USB_Descriptor_Device_t);
			break;

		case DTYPE_Configuration: 
			if (g_bAttackMode == 0)
			{
				Address = (void*)&ConfigurationDescriptorForNormal;
				Size    = sizeof(USB_Descriptor_Configuration_Normal_t);
			}
			else
			{
				Address = (void*)&ConfigurationDescriptorForAttack;
				Size    = sizeof(USB_Descriptor_Configuration_Attack_t);
			}
			break;

		case DTYPE_String: 
			switch (DescriptorNumber)
			{
				case 0x00: 
					Address = (void*)&LanguageString;
					Size    = pgm_read_byte(&LanguageString.Header.Size);
					break;
				case 0x01: 
					Address = (void*)&ManufacturerString;
					Size    = pgm_read_byte(&ManufacturerString.Header.Size);
					break;
				case 0x02: 
					Address = (void*)&ProductString;
					Size    = pgm_read_byte(&ProductString.Header.Size);
					break;
			}
			break;

		case DTYPE_HID:
			if (g_bAttackMode == 0)
			{
				Address = (void*)&ConfigurationDescriptorForNormal.HID_KeyboardHID;
			}
			else
			{
				Address = (void*)&ConfigurationDescriptorForAttack.HID_KeyboardHID;
			}
			Size    = sizeof(USB_HID_Descriptor_t);
			break;

		case DTYPE_Report:
			if (wIndex == USER_IFNUM)
			{
				Address = (void*)&VendorReport;
				Size    = sizeof(VendorReport);
			}
			else
			{
				Address = (void*)&KeyboardReport;
				Size    = sizeof(KeyboardReport);
			}
			break;
	}
	
	*DescriptorAddress = Address;
	return Size;
}

/**
 * This function is called whenever we receive a setup request via USB.
 * \param data[8] eight bytes of data we received
 * \return number of bytes to use, or 0xff if usbFunctionWrite() should be
 * called
 */
usbMsgLen_t usbFunctionSetup(uint8_t data[8]) {

    delegateInterfaceReadyUsb();

    usbRequest_t *rq = (void *)data;
    DBG1(0xCC, data, 8);

    if ((rq->bmRequestType & USBRQ_TYPE_MASK) == USBRQ_TYPE_CLASS) {
        // class request type
        if (rq->bRequest == USBRQ_HID_GET_REPORT) {
            if (rq->wValue.word == HID_REPORT_KEBOARD)
            {
                // wValue: ReportType (highbyte), ReportID (lowbyte)
                usbMsgPtr = (usbMsgPtr_t)&reportKeyboard;
                return sizeof(reportKeyboard);
            }else if(rq->wValue.word == HID_REPORT_BOOT){

            	if(rq->wLength.word == LED2_GET_REPORT_LENGTH_RAINBOW){	// ready for rainbow color setting;
            		readyForRainbowColor = 1;
            	}
//                // boot
//            	static uchar    optionsBuffer[0x84];
//
//            	memset(optionsBuffer, 0, 0x83);
//            	optionsBuffer[0]       = REPORT_ID_BOOT;	// LED options
//            	/* LED options */
//            	optionsBuffer[0x83] = 0x84;
//            	usbMsgPtr = (usbMsgPtr_t)optionsBuffer;

//            	return sizeof(optionsBuffer);
            }else if(rq->wValue.word == HID_REPORT_OPTION){
            	// length : rq->wLength.word 필요한 리포트를 length로 구분한다.

        		uint8_t k, j, i = 0;
        		static uchar    led2Buffer[CUSTOM_MACRO_SIZE_MAX];
            	if(rq->wLength.word == LED2_GET_REPORT_LENGTH_INFO){
            		// report led2 info
            		getLed2((led2_info_t *)led2Buffer);
            		usbMsgPtr = (usbMsgPtr_t)led2Buffer;
            		return LED2_GET_REPORT_LENGTH_INFO; 	//sizeof(led2Buffer);
            	}else if(rq->wLength.word >= LED2_GET_REPORT_LENGTH_KEYMAP_LAYER1 && rq->wLength.word <= LED2_GET_REPORT_LENGTH_KEYMAP_LAYER4){
            		// keymap
					for(k = 0; k < ROWS; ++k){
						for (j = 0; j < COLUMNS; ++j)
						{
							led2Buffer[i++] = pgm_read_byte(KEYMAP_ADDRESS+(ROWS * COLUMNS * (rq->wLength.word - LED2_GET_REPORT_LENGTH_KEYMAP_LAYER1))+(k * COLUMNS + j));
						}
					}
//					DBG1(0x89, led2Buffer, 120);
					usbMsgPtr = (usbMsgPtr_t)led2Buffer;
					return LED2_GET_REPORT_LENGTH_KEYMAP;
            	}else if(rq->wLength.word >= LED2_GET_REPORT_LENGTH_MACRO1 && rq->wLength.word <= LED2_GET_REPORT_LENGTH_MACRO12){
            		// cst macro
            		for(k = 0; k < CUSTOM_MACRO_SIZE_MAX; ++k){
            			led2Buffer[i++] = pgm_read_byte(CUSTOM_MACRO_ADDRESS+(CUSTOM_MACRO_SIZE_MAX * (rq->wLength.word - LED2_GET_REPORT_LENGTH_MACRO1))+(k));
            		}
            		usbMsgPtr = (usbMsgPtr_t)led2Buffer;
					return CUSTOM_MACRO_SIZE_MAX;
            	}else {
            		return rq->wLength.word;
            	}
            }
        } else if (rq->bRequest == USBRQ_HID_SET_REPORT) {
        	DBG1(0xAA, (uchar *)&rq->wValue.word, 2);
        	// 02 03 : Report Type: 0x03,  ReportID: 0x02
        	// 01 03 : Report Type: 0x03,  ReportID: 0x01
            // Report Type: 0x02(Out)/ReportID: 0x00(none) && Interface: 0(keyboard)
            if (rq->wValue.word == HID_REPORT_KEBOARD && rq->wIndex.word == 0)
            {
                // We expect one byte reports
                expectReport = 1;
                return USB_NO_MSG; // Call usbFunctionWrite with data
            }else if(rq->wValue.word == HID_REPORT_BOOT){
                // boot
//                isStart = 1;
                if(readyForRainbowColor==1){
                	data[1] = LED2_INDEX_COLOR_RAINBOW_INIT;
                	setLed2((uint8_t *)data);
                	expectReport = 4;
                }else{
                	expectReport = 3;
                }
                readyForRainbowColor = 0;
                return USB_NO_MSG; // Call usbFunctionWrite with data
            }else if(rq->wValue.word == HID_REPORT_OPTION){
            	// options
                expectReport = 2;

                return USB_NO_MSG; // Call usbFunctionWrite with data
            }
        } else if (rq->bRequest == USBRQ_HID_GET_IDLE) {
            usbMsgPtr = idleRate;
            return 1;
        } else if (rq->bRequest == USBRQ_HID_SET_IDLE) {
//.........这里部分代码省略.........

void backlight_set(uint8_t level)
{
#ifdef SOFTPWM_LED_ENABLE
    softpwm_enable();
#endif
#ifdef BREATHING_LED_ENABLE
    switch (level) {
        case 1:
        case 2:
        case 3:
#ifdef SOFTPWM_LED_ENABLE
            softpwm_led_enable_all();
#ifdef FADING_LED_ENABLE
            fading_led_disable_all();
#endif
            breathing_led_disable_all();
#else
            backlight_enable();
            breathing_led_disable();
#endif
            backlight_brightness = pgm_read_byte(&backlight_table[level]);
            backlight_set_raw(backlight_brightness);
            break;
        case 4:
        case 5:
        case 6:
#ifdef SOFTPWM_LED_ENABLE
            softpwm_led_enable_all();
#ifdef FADING_LED_ENABLE
            fading_led_disable_all();
#endif
            breathing_led_enable_all();
#else
            backlight_enable();
            breathing_led_enable();
#endif
            breathing_led_set_duration(6 - level);
            break;
#ifdef SOFTPWM_LED_ENABLE
#ifdef FADING_LED_ENABLE
        case 7:
            softpwm_led_enable_all();
            fading_led_enable_all();
            breathing_led_disable_all();
            fading_led_set_direction_all(FADING_LED_FADE_IN);
            fading_led_set_duration(3);
            break;
        case 8:
            softpwm_led_enable_all();
            fading_led_enable_all();
            breathing_led_disable_all();
            fading_led_set_direction_all(FADING_LED_FADE_OUT);
            fading_led_set_duration(3);
            break;
#endif
#endif
        case 0:
        default:
#ifdef SOFTPWM_LED_ENABLE
#ifdef FADING_LED_ENABLE
            fading_led_disable_all();
#endif
            breathing_led_disable_all();
            backlight_brightness = 0;
            backlight_set_raw(backlight_brightness);
            softpwm_led_disable_all();
#else
            breathing_led_disable();
            backlight_disable();
#endif
            break;
    }
#else
    if (level > 0) {
        backlight_enable();
        backlight_set_raw(pgm_read_byte(&backlight_table[level]));
    }
    else {
        backlight_disable();
    }
#endif
}

//initialise spi & enc28j60
void enc28j60_init(void)
{
  unsigned char i;
  unsigned int timeout=0;
  
  //set bank to invalid value -> bank set will update in any case:
  enc28j60_current_bank = 0xFF;
  
  //set up port directions:
  ENC28J60_DDR |= (1<<ENC28J60_PIN_CS);
  
  //deselect enc28j60:
  ENC28J60_CS_HI();
  
  //SPI init
  // initialize I/O
  PORTB |= (1<<ENC28J60_PIN_SCK); //sck = hi
  
  //spi = output
  DDRB |= (1<<ENC28J60_PIN_SS)|(1<<ENC28J60_PIN_CS)|(1<<ENC28J60_PIN_MOSI)|(1<<ENC28J60_PIN_SCK); //SS,MOSI,SCK = OUT
  DDRB &= ~(1<<ENC28J60_PIN_MISO); //MISO = IN
  
  //SPI init:
  // - master mode
  // - positive clock phase
  // - msb first
  // - maximum SPI speed (fosc/2)
  // - enable spi
  SPCR = (0<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(0<<SPR1)|(0<<SPR0);
  SPSR = (1<<SPI2X);
  
  delay(200000);
  enc28j60_spi_write_word (ENC28J60_SOFT_RESET, ENC28J60_SOFT_RESET);
  // check CLKRDY bit to see if reset is complete
  delay(200000);
  
  //wait for ready flag
  while((!(enc28j60_read_address(ENC28J60_REG_ESTAT) & 0x01)) && (timeout<65000)){timeout++;};
  if(timeout>=65000){timeout=0;}
  
  //set up rx pointer:
  enc28j60_next_packet_ptr = ENC28J60_RX_BUFFER_START;
  //enc28j60_next_packet_ptr = 0x0602;
  
  //copy config from progmem to enc28j60:
  for(i=0; i<2*22; i+=2){
    enc28j60_write_address(pgm_read_byte(&enc28j60_config[i+0]),pgm_read_byte(&enc28j60_config[i+1]));
  }
  
  //setup phy:
  enc28j60_write_phy(ENC28J60_PHY_PHCON2, (1<<ENC28J60_BIT_HDLDIS)); //=no loopback of transmitted frames
  
  //enable interrups
  enc28j60_write_address(ENC28J60_REG_EIE, (1<<ENC28J60_EIE_INTIE)|(1<<ENC28J60_EIE_PKTIE));
  
  //enable rx
  //enc28j60_write_address(ENC28J60_REG_ECON1, (1<<ENC28J60_BIT_RXEN));
  enc28j60_spi_write_word(ENC28J60_OP_BFS|ENC28J60_REG_ECON1, (1<<ENC28J60_BIT_RXEN));
  
  //set up leds: LEDB: link status, LEDA: RX&TX activity, stretch 40ms, stretch enable
  enc28j60_write_phy(ENC28J60_PHY_PHLCON, 0x374A); //cave: Table3-3: reset value is 0x3422, do not modify the reserved "3"!! 
  //RevA Datasheet page 9: write as '0000', see RevB Datasheet: write 0011!
  //read silicon revision
  //was geht hier vor?? Trotzdem die Bank 2 ist (0x40), gibt er beim Lesen 
  //der RevID keine neue Bank aus, trotzdem kommt der Wert 05.
  //Vielleicht ist es gar keine Rev05??
  enc28j60_revision = enc28j60_read_address(ENC28J60_REG_ECON1);   //um die aktuelle Banknummer zu sehen
  enc28j60_revision = enc28j60_read_address(ENC28J60_REG_EREVID);
}

/**
 * %Print a string in flash memory to the serial port.
 *
 * \param[in] str Pointer to string stored in flash memory.
 */
void SerialPrint_P(PGM_P str)
{
  for (uint8_t c; (c = pgm_read_byte(str)); str++) Serial.print(c);
}

  0b10111110, 0b11100000, 0b11111110, 0b11110110, 0b11101110, 0b111110,
  0b10011100, 0b1111010,  0b10011110, 0b10001110};

/*******************************************************************************
*  Implementation
******************************************************************************/
void lad_init()
{
  SETBITMASK(LAD_OUT_DIR, 0b11111111);
  SETBITMASK(LAD_PINS_DIR, LAD_PINS_MASK);
}

void lad_display_dec(uint8_t number)
{
  uint8_t MSDigit = number / 10;
  number -= MSDigit * 10;
  uint8_t LSDigit = number;

  lad_ledsArray[LAD_INDEX_LSD] = pgm_read_byte(&lad_decDigits[LSDigit]);
  lad_ledsArray[LAD_INDEX_MSD] = pgm_read_byte(&lad_decDigits[MSDigit]);
}

void lad_display_hex(uint8_t number)
{
  uint8_t MSDigit = number >> 4;
  number -= MSDigit << 4;
  uint8_t LSDigit = number;

  lad_ledsArray[LAD_INDEX_LSD] = pgm_read_byte(&lad_hexDigits[LSDigit]);
  lad_ledsArray[LAD_INDEX_MSD] = pgm_read_byte(&lad_hexDigits[MSDigit]);
}

uint8_t text_decode (uint8_t *buffer, uint8_t *cmd_argument)
{

 uint8_t  found,ch,ch2,cmd_no,j,k,index,cmd_complete;
 uint8_t  state = WAIT_CMD_STATE, quote  = 0;
 static uint16_t i=0; // Keep pointer for next command decode
 cmd_complete = 0;
 cmd_no 	= 0;
 index 		= 0;
 
 cmd_argument  = buffer;
 
 while (*(buffer+i))	  		//End with NULL
 {
// Check with delimiter

 	 j = 0;
	 found = 0;
	
	switch (state)
	{
	  case WAIT_CMD_STATE:   // If found first character in command table  then go to get_cmd state
 		j = 0;
		while ((ch = pgm_read_byte(&text_cmd[j][0])))     //Check first charaters in cmd table
		{ 
		  if (*(buffer+i) == ch)
		  {
			state = GET_CMD_STATE;   // Found delimiter
			*(cmd_argument) = ch;
			index = 1;
			break; 
		  }
		 j++;
		}  
	    break;
	  case GET_CMD_STATE:
	    ch = *(buffer+i);
        j = 0; 
		found = 0;
	  	while ((ch2 = pgm_read_byte(&delimiter[j])))   //Check delimiter End with NULL
		{  
		   if ( ch == ch2)
		   {
              found = 1;
              break;
		   } 	 
           j++;	
		}
		if (found)
		{
        	*(cmd_argument+index) = 0x00;
		     if (ch == '=')
			 {
// Check command number 
  				for (k = 0; k < CMD_NUM ; k++)
				{
					if (!(strcasecmp_P(cmd_argument,&text_cmd[k][0])))  // Found then keep command
					{
						cmd_no  = k+1;   // Start from 0 
						break;
					}
				}
// clear index
  				index = 0;
			    state = WAIT_ARG_STATE;	
			 }	
		}
		else
		{
			*(cmd_argument+index) = ch;
			index++;		
		}
		break;
		
	  case WAIT_ARG_STATE:
		ch = *(buffer+i);
        j = 0; 
		found = 0;
	  	while ((ch2 = pgm_read_byte(&delimiter[j])))   //Check delimiter End with NULL
		{  
		   if ( ch == ch2)
		   {
              found = 1;
              break;
		   } 	 
           j++;	
		}
				
		if ( ch == '"')   // Start quote
		{
			quote ^= 1;   // Toggle quote	
			state = GET_ARG_STATE;   
 			index = 0;
		}
		
		if (!found)    // Not delimiter start get Argument
		{
			state = GET_ARG_STATE;   
			*(cmd_argument) = ch;
			index = 1;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
                                if (prec > 7) prec = 7;
                                vtype = prec;
                                ndigs = 0;
                        }
                        exp = __ftoa_engine (va_arg(ap,double), (char *)buf, vtype, ndigs);
                        vtype = buf[0];
    
                        sign = 0;
                        if ((vtype & FTOA_MINUS) && !(vtype & FTOA_NAN))
                                sign = '-';
                        else if (flags & FL_PLUS)
                                sign = '+';
                        else if (flags & FL_SPACE)
                                sign = ' ';

                        if (vtype & (FTOA_NAN | FTOA_INF)) {
                                const char *p;
                                ndigs = sign ? 4 : 3;
                                if (width > ndigs) {
                                        width -= ndigs;
                                        if (!(flags & FL_LPAD)) {
                                                do {
                                                        write(' ');
                                                } while (--width);
                                        }
                                } else {
                                        width = 0;
                                }
                                if (sign)
                                        write(sign);
                                p = PSTR("inf");
                                if (vtype & FTOA_NAN)
                                        p = PSTR("nan");
                                while ( (ndigs = pgm_read_byte((const prog_char *)p)) != 0) {
                                        if (flags & FL_FLTUPP)
                                                ndigs += 'I' - 'i';
                                        write(ndigs);
                                        p++;
                                }
                                goto tail;
                        }

                        /* Output format adjustment, number of decimal digits in buf[] */
                        if (flags & FL_FLTFIX) {
                                ndigs += exp;
                                if ((vtype & FTOA_CARRY) && buf[1] == '1')
                                        ndigs -= 1;
                                if ((signed char)ndigs < 1)
                                        ndigs = 1;
                                else if (ndigs > 8)
                                        ndigs = 8;
                        } else if (!(flags & FL_FLTEXP)) {              /* 'g(G)' format */
                                if (exp <= prec && exp >= -4)
                                        flags |= FL_FLTFIX;
                                while (prec && buf[1+prec] == '0')
                                        prec--;
                                if (flags & FL_FLTFIX) {
                                        ndigs = prec + 1;               /* number of digits in buf */
                                        prec = prec > exp
                                                ? prec - exp : 0;       /* fractional part length  */
                                }
                        }
    
                        /* Conversion result length, width := free space length */
                        if (flags & FL_FLTFIX)
                                n = (exp>0 ? exp+1 : 1);

//.........这里部分代码省略.........
    // initialize limits flags
    limit.roll_pitch = false;
    limit.yaw = false;
    limit.throttle_lower = false;
    limit.throttle_upper = false;

    // Throttle is 0 to 1000 only
    int16_t thr_in_min = rel_pwm_to_thr_range(_min_throttle);
    if (_throttle_control_input <= thr_in_min) {
        _throttle_control_input = thr_in_min;
        limit.throttle_lower = true;
    }
    if (_throttle_control_input >= _max_throttle) {
        _throttle_control_input = _max_throttle;
        limit.throttle_upper = true;
    }

    // tricopters limit throttle to 80%
    // To-Do: implement improved stability patch and remove this limit
    if (_throttle_control_input > 800) {
        _throttle_control_input = 800;
        limit.throttle_upper = true;
    }

    roll_pwm = calc_roll_pwm();
    pitch_pwm = calc_pitch_pwm();
    throttle_radio_output = calc_throttle_radio_output();
    yaw_radio_output = calc_yaw_radio_output();

    // if we are not sending a throttle output, we cut the motors
    if( is_zero(_throttle_control_input) ) {
        // range check spin_when_armed
        if (_spin_when_armed_ramped < 0) {
            _spin_when_armed_ramped = 0;
        }
        if (_spin_when_armed_ramped > _min_throttle) {
            _spin_when_armed_ramped = _min_throttle;
        }
        motor_out[AP_MOTORS_MOT_1] = _throttle_radio_min + _spin_when_armed_ramped;
        motor_out[AP_MOTORS_MOT_2] = _throttle_radio_min + _spin_when_armed_ramped;
        motor_out[AP_MOTORS_MOT_4] = _throttle_radio_min + _spin_when_armed_ramped;

    }else{
        int16_t roll_out            = (float)(roll_pwm * 0.866f);
        int16_t pitch_out           = pitch_pwm / 2;

        // check if throttle is below limit
        if (_throttle_control_input <= _min_throttle) {
            limit.throttle_lower = true;
            _throttle_control_input = _min_throttle;
            throttle_radio_output = calc_throttle_radio_output();
        }

        // TODO: set limits.roll_pitch and limits.yaw

        //left front
        motor_out[AP_MOTORS_MOT_2] = throttle_radio_output + roll_out + pitch_out;
        //right front
        motor_out[AP_MOTORS_MOT_1] = throttle_radio_output - roll_out + pitch_out;
        // rear
        motor_out[AP_MOTORS_MOT_4] = throttle_radio_output - pitch_pwm;

        // Tridge's stability patch
        if(motor_out[AP_MOTORS_MOT_1] > out_max) {
            motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_1] - out_max);
            motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_1] - out_max);
            motor_out[AP_MOTORS_MOT_1] = out_max;
        }

        if(motor_out[AP_MOTORS_MOT_2] > out_max) {
            motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_2] - out_max);
            motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_2] - out_max);
            motor_out[AP_MOTORS_MOT_2] = out_max;
        }

        if(motor_out[AP_MOTORS_MOT_4] > out_max) {
            motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_4] - out_max);
            motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_4] - out_max);
            motor_out[AP_MOTORS_MOT_4] = out_max;
        }

        // adjust for thrust curve and voltage scaling
        motor_out[AP_MOTORS_MOT_1] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_1], out_min, out_max);
        motor_out[AP_MOTORS_MOT_2] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_2], out_min, out_max);
        motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, out_max);

        // ensure motors don't drop below a minimum value and stop
        motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1],    out_min);
        motor_out[AP_MOTORS_MOT_2] = max(motor_out[AP_MOTORS_MOT_2],    out_min);
        motor_out[AP_MOTORS_MOT_4] = max(motor_out[AP_MOTORS_MOT_4],    out_min);
    }

    // send output to each motor
    hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), motor_out[AP_MOTORS_MOT_1]);
    hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), motor_out[AP_MOTORS_MOT_2]);
    hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), motor_out[AP_MOTORS_MOT_4]);

    // send out to yaw command to tail servo
    hal.rcout->write(AP_MOTORS_CH_TRI_YAW, yaw_radio_output);
}

void usb_serial_write_P(const char* data)
{
  while(pgm_read_byte(data) != 0)
    usb_serial_putchar(pgm_read_byte(data++));
}

void Tone::begin(uint8_t tonePin)
{
  if (_tone_pin_count < AVAILABLE_TONE_PINS)
  {
    _pin = tonePin;
    _timer = pgm_read_byte(tone_pin_to_timer_PGM + _tone_pin_count);
    _tone_pin_count++;

    // Set timer specific stuff
    // All timers in CTC mode
    // 8 bit timers will require changing prescalar values,
    // whereas 16 bit timers are set to either ck/1 or ck/64 prescalar
    switch (_timer)
    {
#if !defined(__AVR_ATmega8__)
      case 0:
        // 8 bit timer
        TCCR0A = 0;
        TCCR0B = 0;
        bitWrite(TCCR0A, WGM01, 1);
        bitWrite(TCCR0B, CS00, 1);
        timer0_pin_port = portOutputRegister(digitalPinToPort(_pin));
        timer0_pin_mask = digitalPinToBitMask(_pin);
        break;
#endif

      case 1:
        // 16 bit timer
        TCCR1A = 0;
        TCCR1B = 0;
        bitWrite(TCCR1B, WGM12, 1);
        bitWrite(TCCR1B, CS10, 1);
        timer1_pin_port = portOutputRegister(digitalPinToPort(_pin));
        timer1_pin_mask = digitalPinToBitMask(_pin);
        break;
      case 2:
        // 8 bit timer
        TCCR2A = 0;
        TCCR2B = 0;
        bitWrite(TCCR2A, WGM21, 1);
        bitWrite(TCCR2B, CS20, 1);
        timer2_pin_port = portOutputRegister(digitalPinToPort(_pin));
        timer2_pin_mask = digitalPinToBitMask(_pin);
        break;

#if defined(__AVR_ATmega1280__)
      case 3:
        // 16 bit timer
        TCCR3A = 0;
        TCCR3B = 0;
        bitWrite(TCCR3B, WGM32, 1);
        bitWrite(TCCR3B, CS30, 1);
        timer3_pin_port = portOutputRegister(digitalPinToPort(_pin));
        timer3_pin_mask = digitalPinToBitMask(_pin);
        break;
      case 4:
        // 16 bit timer
        TCCR4A = 0;
        TCCR4B = 0;
        bitWrite(TCCR4B, WGM42, 1);
        bitWrite(TCCR4B, CS40, 1);
        timer4_pin_port = portOutputRegister(digitalPinToPort(_pin));
        timer4_pin_mask = digitalPinToBitMask(_pin);
        break;
      case 5:
        // 16 bit timer
        TCCR5A = 0;
        TCCR5B = 0;
        bitWrite(TCCR5B, WGM52, 1);
        bitWrite(TCCR5B, CS50, 1);
        timer5_pin_port = portOutputRegister(digitalPinToPort(_pin));
        timer5_pin_mask = digitalPinToBitMask(_pin);
        break;
#endif
    }
  }
  else
  {
    // disabled
    _timer = -1;
  }
}