int handleData(coap_rw_buffer_t* scratch,
               const coap_packet_t* inpkt,
               coap_packet_t* outpkt,
               uint8_t id_hi, uint8_t id_lo)
{
    int temperature = getTemperature(scratch, inpkt, outpkt, id_hi, id_lo);

    printf("Received temperature %d\n", temperature);

    addValue(temperature);

    int average_temp = getSum() / getCount();

    printf("Average temperature %d\n", average_temp);

    if (average_temp < YELLOW_TEMP * 100) {
        setLed(LEDGREEN_PIN);
    } else {
        if (average_temp < RED_TEMP * 100) {
            setLed(LEDYELLOW_PIN);
        } else {
            setLed(LEDRED_PIN);
        }
    }

    return 0;
}

FUNC(int, OS_APPL_CODE) main(void)
{
#if NUMBER_OF_CORES > 1
    StatusType rv;

    switch(GetCoreID()){
      case OS_CORE_ID_MASTER :
        initLed();
        setLed(0, led_state1);
        /* Wakeup core 1 */
        StartCore(OS_CORE_ID_1, &rv);
        if(rv == E_OK)
          StartOS(OSDEFAULTAPPMODE);
        break;
      case OS_CORE_ID_1 :
        /* Core 1 : leds were already initialized by the core 0 at this step */
        setLed(1, led_state2);
        StartOS(OSDEFAULTAPPMODE);
        break;
      default :
        /* Should not happen */
        break;
    }
#else
    initLed();
    setLed(0, led_state1);
    StartOS(OSDEFAULTAPPMODE);
#endif

    return 0;
}

  /*
   * re-program firmware: this entire function and all subroutines must run from RAM
   * (don't make this static!)
   */
  __attribute__ ((section (".coderam")))
  void flashFirmware(uint8_t* source, uint32_t size){
    __disable_irq(); // Disable ALL interrupts. Can only be executed in Privileged modes.
    setLed(RED);
    eeprom_unlock();
    if(size > (16+16+64+128)*1024){
      eeprom_erase_sector(FLASH_Sector_6);
      toggleLed(); // inline
    }
    if(size > (16+16+64)*1024){
      eeprom_erase_sector(FLASH_Sector_5);
      toggleLed();
    }
    if(size > (16+16)*1024){
      eeprom_erase_sector(FLASH_Sector_4);
      toggleLed();
    }
    if(size > 16*1024){
      eeprom_erase_sector(FLASH_Sector_3);
      toggleLed();
    }
    eeprom_erase_sector(FLASH_Sector_2);
    toggleLed();
    eeprom_write_block(ADDR_FLASH_SECTOR_2, source, size);
    eeprom_lock();
    eeprom_wait();
    NVIC_SystemReset(); // (static inline)
  }

  void programFlashTask(void* p){
    int sector = flashSectorToWrite;
    uint32_t size = flashSizeToWrite;
    uint8_t* source = (uint8_t*)flashAddressToWrite;
    if(sector >= 0 && sector < MAX_USER_PATCHES && size <= 128*1024){
      uint32_t addr = getFlashAddress(sector);
      eeprom_unlock();
      int ret = eeprom_erase(addr);
      if(ret == 0)
	ret = eeprom_write_block(addr, source, size);
      eeprom_lock();
      registry.init();
      if(ret == 0){
	// load and run program
	int pc = registry.getNumberOfPatches()-MAX_USER_PATCHES+sector;
	program.loadProgram(pc);
	// program.loadDynamicProgram(source, size);
	program.resetProgram(false);
      }else{
	setErrorMessage(PROGRAM_ERROR, "Failed to write program to flash");
      }
    }else if(sector == 0xff && size < MAX_SYSEX_FIRMWARE_SIZE){
      flashFirmware(source, size);
    }else{
      setErrorMessage(PROGRAM_ERROR, "Invalid flash program command");
    }
    vTaskDelete(NULL);
  }

  void eraseFlashTask(void* p){
    int sector = flashSectorToWrite;
    if(sector == 0xff){
      for(int i=0; i<MAX_USER_PATCHES; ++i)
	eraseFlashProgram(i);
      settings.clearFlash();
    }else if(sector >= 0 && sector < MAX_USER_PATCHES){
      eraseFlashProgram(sector);
    }else{
      setErrorMessage(PROGRAM_ERROR, "Invalid flash erase command");
    }
    registry.init();
    vTaskDelete(NULL);
  }

  // static int midiMessagesToSend = 0;
  // void sendMidiDataTask(void* p){
  //   switch(midiMessagesToSend){
  //   case SYSEX_PRESET_NAME_COMMAND:
  //     midi.sendPatchNames();
  //     break;
  //   // case 0:
  //   //   midi.sendDeviceInfo();
  //   //   break;
  //   // case SYSEX_PARAMETER_NAME_COMMAND:
  //   //   midi.sendPatchParameterNames();
  //   //   break;
  //   // case SYSEX_FIRMWARE_VERSION:
  //   //   midi.sendFirmwareVersion();
  //   //   break;
  //   // case SYSEX_DEVICE_ID:
  //   //   midi.sendDeviceId();
  //   //   break;
  //   // case SYSEX_DEVICE_STATS:
  //   //   midi.sendDeviceStats();
  //   //   break;
  //   // case SYSEX_PROGRAM_MESSAGE:
  //   //   midi.sendProgramMessage();
//.........这里部分代码省略.........

void __init boardLedInit(void)
{
    PBP_LED_INFO pInfo;
    ETHERNET_MAC_INFO EnetInfo;
    unsigned short i;
    short gpio;

    spin_lock_init(&brcm_ledlock);

#if !(defined(CONFIG_BCM96328) || defined(CONFIG_BCM96362))
    /* Set blink rate for hardware LEDs. */
    GPIO->LEDCtrl &= ~LED_INTERVAL_SET_MASK;
    GPIO->LEDCtrl |= LED_INTERVAL_SET_80MS;
#endif

    gLedCtrl = (PLED_CTRL) kmalloc((kLedEnd * sizeof(LED_CTRL)), GFP_KERNEL);

    if( gLedCtrl == NULL ) {
        printk( "LED memory allocation error.\n" );
        return;
    }

    /* Initialize LED control */
    for (i = 0; i < kLedEnd; i++) {
        gLedCtrl[i].ledHwFunc = NULL;
        gLedCtrl[i].ledGreenGpio = -1;
        gLedCtrl[i].ledRedGpio = -1;
        gLedCtrl[i].ledState = kLedStateOff;
        gLedCtrl[i].blinkCountDown = 0;            // reset the blink count down
    }

    for( pInfo = bpLedInfo; pInfo->ledName != kLedEnd; pInfo++ ) {
        if( pInfo->bpFunc && (*pInfo->bpFunc) (&gpio) == BP_SUCCESS )
        {
            gLedCtrl[pInfo->ledName].ledGreenGpio = gpio;
        }
        if( pInfo->bpFuncFail && (*pInfo->bpFuncFail)(&gpio)==BP_SUCCESS )
        {
            gLedCtrl[pInfo->ledName].ledRedGpio = gpio;
        }
        setLed(&gLedCtrl[pInfo->ledName], kLedOff, kLedGreen);
        setLed(&gLedCtrl[pInfo->ledName], kLedOff, kLedRed);
    }

    if( BpGetEthernetMacInfo( &EnetInfo, 1 ) == BP_SUCCESS ) {
        if( EnetInfo.usGpioLedPhyLinkSpeed != BP_NOT_DEFINED ) {
            /* The internal Ethernet PHY has a GPIO for 10/100 link speed. */
            gLedCtrl[kLedEphyLinkSpeed].ledGreenGpio = EnetInfo.usGpioLedPhyLinkSpeed;
            setLed(&gLedCtrl[kLedEphyLinkSpeed], kLedOff, kLedGreen);
        }
    }

#if defined(DEBUG_LED)
    for (i=0; i < kLedEnd; i++)
        printk("initLed: led[%d]: Gpio=0x%04x, FailGpio=0x%04x\n", i, gLedCtrl[i].ledGreenGpio, gLedCtrl[i].ledRedGpio);
#endif

}

void panic(int led1, int led2)
{
   DBGMSG1("Panic!");
	for(;;) {
		_delay_ms(1000);
		setLed(led1);
		_delay_ms(1000);
		setLed(led2);
	}		
}

/*
* Disconnect. Switch off led 1
*/
uint8_t MWiFi::Disconnect()
{
          uint8_t cf=1;
          uint8_t ev=0;
          if (CONNSTATUS!=1) {CONNSTATUS=0; setLed(1,0);cf=0;return cf;}
          sendShortMess(91);                //cmd 91: disconnect
          receiveMessWait(1000); //ACK
          uint8_t i;
          // async for Connection status changed
          for (i=0;i<CONNTOUT;i++) {if(ev!=16) ev=getAsyncWait(1000);else break;}   
          if (ev==16) {CONNSTATUS=0; setLed(1,0);cf=0;} 
          return cf;
}

int main(void){
	initLed();
	initButton();
	while(!isPressed()){
		setLed(1,0,0);
		delayLoop();
		setLed(0,0,0);
		delayLoop();
	}
	setLed(1,1,1);
	DelayMs(1000);
	SoftReset();
}

std::vector <int> DynamixelSimpleAPI::servoScan(int start, int stop)
{
    // Check start/stop boundaries
    if (start < 0 || start > (maxId - 1))
        start = 0;

    if (stop < 1 || stop > maxId || stop < start)
        stop = maxId;

    TRACE_INFO(DAPI, "> Scanning for Dynamixel devices on '%s'... Range is [%i,%i]\n",
               serialGetCurrentDevice().c_str(), start, stop);

    // A vector of Dynamixel IDs found during the scan
    std::vector <int> ids;

    for (int id = start; id <= stop; id++)
    {
        PingResponse pingstats;

        // If the ping gets a response, then we have found a servo
        if (dxl_ping(id, &pingstats) == true)
        {
            setLed(id, 1, LED_GREEN);

            ids.push_back(id);

            TRACE_INFO(DAPI, "[#%i] Dynamixel servo found!\n", id);
            TRACE_INFO(DAPI, "[#%i] model: '%i' (%s)\n", id, pingstats.model_number,
                       dxl_get_model_name(pingstats.model_number).c_str());

            // Other informations, not printed by default:
            TRACE_1(DAPI, "[#%i] firmware: '%i' \n", id, pingstats.firmware_version);
            TRACE_1(DAPI, "[#%i] position: '%i' \n", id, readCurrentPosition(id));
            TRACE_1(DAPI, "[#%i] speed: '%i' \n", id, readCurrentSpeed(id));
            TRACE_1(DAPI, "[#%i] torque: '%i' \n", id, getTorqueEnabled(id));
            TRACE_1(DAPI, "[#%i] load: '%i' \n", id, readCurrentLoad(id));
            TRACE_1(DAPI, "[#%i] baudrate: '%i' \n", id, getSetting(id, REG_BAUD_RATE));

            setLed(id, 0);
        }
        else
        {
            printf(".");
        }
    }

    printf("\n");
    return ids;
}

inline unsigned long effediics(int max) {
    const signed int A1 = 20000;
    signed int Th0 = 0, Th1 = 8, Th2 = 10;
    signed long int K0, K1, res=0;
    float  kAmp=0;
    Th0 = 5;
    Th1 = Th0 + (max/2000)+3;
    Th2 = Th1 + (max/250)+20;

    K0 = 20000;
    
    kAmp = ((float)max+18000.0)/19000.0;
    //kAmp = 1;
    K1 = max/kAmp;

    if (thMilliseconds < Th0) {
        return K0;
    } else if (( thMilliseconds >= Th0 ) && ( thMilliseconds < Th1 )) {
        return ((K1-K0)*thMilliseconds + K0*Th1 - K1*Th0)/(Th1-Th0);
    } else if (thMilliseconds>=Th1) {
        res = (-K1*thMilliseconds + K1*Th2)/(Th2 - Th1);
        return res>249?res:250;
    }
    
    /*+++++++++++++++++++++++*/
    if (thMilliseconds>Th1) {
        setLed(2, 1);
    }
    if (thMilliseconds>Th2) {
        //setLed(2, 0);
    }

    return 1;
}

int main( void )
{
    halInit();
    moduleInit();    
    printf("\r\n****************************************************\r\n");    
    printf("Fragmentation Example - ROUTER - using AFZDO\r\n");
    buttonIsr = &handleButtonPress;    

#define MODULE_START_DELAY_IF_FAIL_MS 5000

    /* See basic communications examples for more information about module startup. */
    struct moduleConfiguration defaultConfiguration = DEFAULT_MODULE_CONFIGURATION_ROUTER;
    start: 
    while ((result = startModule(&defaultConfiguration, GENERIC_APPLICATION_CONFIGURATION)) != MODULE_SUCCESS)
    {
        printf("Module start unsuccessful. Error Code 0x%02X. Retrying...\r\n", result);
        delayMs(MODULE_START_DELAY_IF_FAIL_MS);
    }
    printf("On Network!\r\n");
    setLed(0);
    /* On network, display info about this network */ 
#ifdef DISPLAY_NETWORK_INFORMATION    //excluded to reduce code size
    displayNetworkConfigurationParameters();   
    displayDeviceInformation();
#endif 
    HAL_ENABLE_INTERRUPTS();

    /* Now the network is running - send a message to the coordinator every few seconds.*/
#define TEST_CLUSTER 0x77    

    /* Fill test message buffer with an incrementing counter */
    int i = 0;
    for (i=0; i<MESSAGE_LENGTH; i++)
    {
    	testMessage[i] = i;
    }
    printf("Sending the following message:\r\n");

    uint8_t counter = 0;
    while (1)
    {       
        printf("Sending Message #%u L%u to Short Address 0x0000 (Coordinator) ", counter++, MESSAGE_LENGTH);
        /* Send an extended length message to a short address */
        moduleResult_t result = afSendDataExtendedShort(DEFAULT_ENDPOINT, DEFAULT_ENDPOINT, 0, TEST_CLUSTER, testMessage, MESSAGE_LENGTH);  //a short message - coordinator will receive an AF_INCOMING_MSG_EXT
        if (result == MODULE_SUCCESS)
        {
            printf("Success\r\n");
        } else {
            printf("ERROR %i ", result);
#ifdef RESTART_AFTER_ZM_FAILURE
            printf("\r\nRestarting\r\n");
            goto start;
#else        
            printf("stopping\r\n");
            while(1);
#endif
        }
        delayMs(2000);  
    }   
}

static void ICACHE_FLASH_ATTR ledTimerCb(void *v) {
	int time = 1000;

	if (wifiState == wifiGotIP) {
		// connected, all is good, solid light
		ledState = 1-ledState;
		time = ledState ? 2900 : 100;
	} else if (wifiState == wifiIsConnected) {
		// waiting for DHCP, go on/off every second
		ledState = 1 - ledState;
		time = 1000;
	} else {
		// idle
		switch (wifi_get_opmode()) {
		case 1: // STA
			ledState = 0;
			break;
		case 2: // AP
			ledState = 1-ledState;
			time = ledState ? 50 : 1950;
			break;
		case 3: // STA+AP
			ledState = 1-ledState;
			time = ledState ? 50 : 950;
			break;
		}
	}

	setLed(ledState);
	os_timer_arm(&ledTimer, time, 0);
}

 void flashFirmware(uint8_t* source, uint32_t size){
   __disable_irq(); // Disable ALL interrupts. Can only be executed in Privileged modes.
   setLed(RED);
   eeprom_unlock();
   if(size > (16+16+64+128)*1024){
     eeprom_erase_sector(FLASH_Sector_6);
     toggleLed(); // inline
   }
   if(size > (16+16+64)*1024){
     eeprom_erase_sector(FLASH_Sector_5);
     toggleLed();
   }
   if(size > (16+16)*1024){
     eeprom_erase_sector(FLASH_Sector_4);
     toggleLed();
   }
   if(size > 16*1024){
     eeprom_erase_sector(FLASH_Sector_3);
     toggleLed();
   }
   eeprom_erase_sector(FLASH_Sector_2);
   toggleLed();
   eeprom_write_block(ADDR_FLASH_SECTOR_2, source, size);
   eeprom_lock();
   eeprom_wait();
   NVIC_SystemReset(); // (static inline)
 }

// Timer callback to update RFM69 status LED
static void rfmLedTimerCb(void *v)
{
   int time = 1000;
   #if 0
   if (rfmState == rfm_unknown)
   {
      // connected, all is good, solid light with a short dark blip every 3 seconds
      rfmLedState = 1-rfmLedState;
      time = rfmLedState ? 2900 : 1200;
   }
   else if (rfmState == rfm_connected)
   {
      // waiting for DHCP, go on/off every second
      rfmLedState = 1;
      time = 10001;
   }
   else
   {
      // not connected
      rfmLedState = 1-rfmLedState;
      time = rfmLedState ? 950 : 1950;
   }
   #endif
   setLed(rfmLedState, RFM_LED_PIN_NUM);
   os_timer_arm(&rfmLedTimer, time, 0);
}

/** Parse any received messages. If it's one of our OIDs then display it. */
void parseMessages()
{
    getMessage();
    if ((zmBuf[SRSP_LENGTH_FIELD] > 0) && (IS_AF_INCOMING_MESSAGE()))
    {
        setLed(0);                                  //LED will blink to indicate a message was received
#ifdef VERBOSE_MESSAGE_DISPLAY
        printAfIncomingMsgHeader(zmBuf);
        printf("\r\n");
#endif
        if ((AF_INCOMING_MESSAGE_CLUSTER()) == INFO_MESSAGE_CLUSTER)
        {
            processInfoMessage();            
        } else if ((AF_INCOMING_MESSAGE_CLUSTER()) == CONFIG_REQUEST_MESSAGE_CLUSTER) {
            processConfigRequestMessage();            
        } else {    //unknown cluster
            printf("Rx: ");
            printHexBytes(zmBuf+SRSP_HEADER_SIZE+17, zmBuf[SRSP_HEADER_SIZE+16]);   //print out message payload
        }
        clearLed(0);    
    } else if (IS_ZDO_END_DEVICE_ANNCE_IND()) {
        displayZdoEndDeviceAnnounce(zmBuf);
    } else { //unknown message, just print out the whole thing
        printf("MSG: ");
        printHexBytes(zmBuf, (zmBuf[SRSP_LENGTH_FIELD] + SRSP_HEADER_SIZE));
    }
    zmBuf[SRSP_LENGTH_FIELD] = 0;
}

void LedMatrixMAX7219Driver::setRow(unsigned char row, unsigned char value) {
    if (!isOutOfBounds(0, row)) {
        for (unsigned char i = 0; i < rows; i++) {
            setLed(row, i, value);
        }
    }
}

void CRC_PCA9635::reset()
{
	// TODO, Send Software Reset
	for (int i = 0; i < 15; i++) {
		setLed(i, 0);
	}
}

void BigButtons::fromJson( QJsonObject obj)
{
    setName(obj.value("name").toString());
    setColor(QColor(obj.value("col").toString()));
    setPatternName(obj.value("patternName").toString());
    setLed(obj.value("led").toDouble());
}

/*
* Power down function. It switches off radio and clears every setting.
* Shield can be switched on, only by reset (or begin()).
*/
void MWiFi::setPowerOff()
{
          uint8_t mess[4]={1,0,0,0};
          sendLongMess(102,mess,4);
          receiveMessWait(1000); //ACK
          setLed(0,0);
}

// Set led command handler
void PetduinoSerial::onSetLed()
{
  // Read led state argument, interpret string as boolean
  bool ledState = cmdMessenger.readBoolArg();

  // Set led
  setLed(ledState ? HIGH : LOW);
}

FUNC(int, OS_APPL_CODE) main(void)
{
    initLed();
    setLed(0, 1);

    StartOS(OSDEFAULTAPPMODE);
    return 0;
}