C++ UTIL1_strcpy函数代码示例

static void PrintPIDstatus(PID_Config *config, const unsigned char *kindStr, const CLS1_StdIOType *io) {
  unsigned char buf[48];
  unsigned char kindBuf[16];

  UTIL1_strcpy(kindBuf, sizeof(buf), (unsigned char*)"  ");
  UTIL1_strcat(kindBuf, sizeof(buf), kindStr);
  UTIL1_strcat(kindBuf, sizeof(buf), (unsigned char*)" PID");
  UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"p: ");
  UTIL1_strcatNum32s(buf, sizeof(buf), config->pFactor100);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" i: ");
  UTIL1_strcatNum32s(buf, sizeof(buf), config->iFactor100);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" d: ");
  UTIL1_strcatNum32s(buf, sizeof(buf), config->dFactor100);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
  CLS1_SendStatusStr(kindBuf, buf, io->stdOut);

  UTIL1_strcpy(kindBuf, sizeof(buf), (unsigned char*)"  ");
  UTIL1_strcat(kindBuf, sizeof(buf), kindStr);
  UTIL1_strcat(kindBuf, sizeof(buf), (unsigned char*)" windup");
  UTIL1_Num32sToStr(buf, sizeof(buf), config->iAntiWindup);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
  CLS1_SendStatusStr(kindBuf, buf, io->stdOut);

  UTIL1_strcpy(kindBuf, sizeof(buf), (unsigned char*)"  ");
  UTIL1_strcat(kindBuf, sizeof(buf), kindStr);
  UTIL1_strcat(kindBuf, sizeof(buf), (unsigned char*)" speed");
  UTIL1_Num8uToStr(buf, sizeof(buf), config->maxSpeedPercent);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"%\r\n");
  CLS1_SendStatusStr(kindBuf, buf, io->stdOut);
 
}

uint8_t ESP_SelectMode(uint8_t mode) {
  /* AT+CWMODE=<mode>, where <mode> is 1=Sta, 2=AP or 3=both */
  uint8_t txBuf[sizeof("AT+CWMODE=x\r\n")];
  uint8_t rxBuf[sizeof("AT+CWMODE=x\r\r\nno change\r\n")];
  uint8_t expected[sizeof("AT+CWMODE=x\r\r\nno change\r\n")];
  uint8_t res;

  if (mode<1 || mode>3) {
    return ERR_RANGE; /* only 1, 2 or 3 */
  }
  UTIL1_strcpy(txBuf, sizeof(txBuf), "AT+CWMODE=");
  UTIL1_strcatNum16u(txBuf, sizeof(txBuf), mode);
  UTIL1_strcat(txBuf, sizeof(txBuf), "\r\n");
  UTIL1_strcpy(expected, sizeof(expected), "AT+CWMODE=");
  UTIL1_strcatNum16u(expected, sizeof(expected), mode);
  UTIL1_strcat(expected, sizeof(expected), "\r\r\n\n");
  res = ESP_SendATCommand(txBuf, rxBuf, sizeof(rxBuf), expected, ESP_DEFAULT_TIMEOUT_MS, NULL);
  if (res!=ERR_OK) {
    /* answer could be as well "AT+CWMODE=x\r\r\nno change\r\n"!! */
    UTIL1_strcpy(txBuf, sizeof(txBuf), "AT+CWMODE=");
    UTIL1_strcatNum16u(txBuf, sizeof(txBuf), mode);
    UTIL1_strcat(txBuf, sizeof(txBuf), "\r\n");
    UTIL1_strcpy(expected, sizeof(expected), "AT+CWMODE=");
    UTIL1_strcatNum16u(expected, sizeof(expected), mode);
    UTIL1_strcat(expected, sizeof(expected), "\r\r\nno change\r\n");
    if (UTIL1_strcmp(rxBuf, expected)==0) {
      res = ERR_OK;
    }
  }
  return res;
}

static uint8_t PrintStatus(const CLS1_StdIOType *io) {
  uint8_t buf[64], buf2[16];
  QuadTime_t timing;
  int i;
  
  CLS1_SendStatusStr((unsigned char*)"quadcalib", (unsigned char*)"\r\n", io->stdOut);
  for(i=0; i<NOF_SIGNALS; i++) {
    if (Measure(i, &timing)==ERR_OK) {
      buf[0] = '\0';
      UTIL1_strcatNum8u(buf, sizeof(buf), timing.lowPercent);
      UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"% to ");
      UTIL1_strcatNum8u(buf, sizeof(buf), timing.highPercent);
      UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"%, ");
      UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"high: ");
      UTIL1_strcatNum32u(buf, sizeof(buf), timing.highTicks);
      UTIL1_strcat(buf, sizeof(buf), (uint8_t*)" ticks, low: ");
      UTIL1_strcatNum32u(buf, sizeof(buf), timing.lowTicks);
      UTIL1_strcat(buf, sizeof(buf), (uint8_t*)" ticks\r\n");
    } else {
      UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)"TIMEOUT\r\n");
    }
    if (i==0) {
      UTIL1_strcpy(buf2, sizeof(buf2), (uint8_t*)"  Right A,C0");
    } else if (i==1) {
      UTIL1_strcpy(buf2, sizeof(buf2), (uint8_t*)"  Right B,C1");
    } else if (i==2) {
      UTIL1_strcpy(buf2, sizeof(buf2), (uint8_t*)"  Left  C,C2");
    } else {
      UTIL1_strcpy(buf2, sizeof(buf2), (uint8_t*)"  Left  D,C3");
    }
    CLS1_SendStatusStr(buf2, buf, io->stdOut);
  }
  return ERR_OK;
}

uint8_t ESP_SetServer(bool startIt, uint16_t port, const CLS1_StdIOType *io, uint16_t timeoutMs) {
  /* AT+CIPSERVER=<en>,<port>, where <en>: 0: stop, 1: start */
  uint8_t res;
  uint8_t cmd[sizeof("AT+CIPSERVER=1,80\r\n\r\nOK\r\n")+sizeof("no change")];
  uint8_t rxBuf[sizeof("AT+CIPSERVER=1,80\r\n\r\nOK\r\n")+sizeof("no change")];

  UTIL1_strcpy(cmd, sizeof(cmd), "AT+CIPSERVER=");
  if (startIt) {
    UTIL1_strcat(cmd, sizeof(cmd), "1,");
  } else {
    UTIL1_strcat(cmd, sizeof(cmd), "0,");
  }
  UTIL1_strcatNum16u(cmd, sizeof(cmd), port);
  UTIL1_strcat(cmd, sizeof(cmd), "\r\n");
  res = ESP_SendATCommand(cmd, rxBuf, sizeof(rxBuf), "OK\r\n", timeoutMs, io);
  if (res!=ERR_OK) { /* accept "no change" too */
    UTIL1_strcpy(cmd, sizeof(cmd), "AT+CIPSERVER=");
    if (startIt) {
      UTIL1_strcat(cmd, sizeof(cmd), "1,");
    } else {
      UTIL1_strcat(cmd, sizeof(cmd), "0,");
    }
    UTIL1_strcatNum16u(cmd, sizeof(cmd), port);
    UTIL1_strcat(cmd, sizeof(cmd), "\r\r\nno change\r\n");
    if (UTIL1_strcmp(rxBuf, cmd)==0) {
      res = ERR_OK;
    }
  }
  return res;
}

uint8_t ESP_GetFirmwareVersionString(uint8_t *fwBuf, size_t fwBufSize) {
  /* AT+GMR */
  uint8_t rxBuf[32];
  uint8_t res;
  const unsigned char *p;

  res = ESP_SendATCommand("AT+GMR\r\n", rxBuf, sizeof(rxBuf), "\r\n\r\nOK\r\n", ESP_DEFAULT_TIMEOUT_MS, NULL);
  if (res!=ERR_OK) {
    if (UTIL1_strtailcmp(rxBuf, "\r\n\r\nOK\r\n")) {
      res = ERR_OK;
    }
  }
  if (res==ERR_OK) {
    if (UTIL1_strncmp(rxBuf, "AT+GMR\r\r\n", sizeof("AT+GMR\r\r\n")-1)==0) { /* check for beginning of response */
      UTIL1_strCutTail(rxBuf, "\r\n\r\nOK\r\n"); /* cut tailing response */
      p = rxBuf+sizeof("AT+GMR\r\r\n")-1;  /* skip beginning */
      UTIL1_strcpy(fwBuf, fwBufSize, p); /* copy firmware information string */
    } else {
      res = ERR_FAILED;
    }
  }
  if (res!=ERR_OK) {
    UTIL1_strcpy(fwBuf, fwBufSize, "ERROR"); /* default error */
  }
  return res;
}

uint8_t ESP_GetIPAddrString(uint8_t *ipBuf, size_t ipBufSize) {
  /* AT+CIFSR */
  uint8_t rxBuf[32];
  uint8_t res;
  const unsigned char *p;

  res = ESP_SendATCommand("AT+CIFSR\r\n", rxBuf, sizeof(rxBuf), NULL, ESP_DEFAULT_TIMEOUT_MS, NULL);
  if (res!=ERR_OK) {
    if (UTIL1_strtailcmp(rxBuf, "\r\n")) {
      res = ERR_OK;
    }
  }
  if (res==ERR_OK) {
    if (UTIL1_strncmp(rxBuf, "AT+CIFSR\r\r\n", sizeof("AT+CIFSR\r\r\n")-1)==0) { /* check for beginning of response */
      UTIL1_strCutTail(rxBuf, "\r\n"); /* cut tailing response */
      p = rxBuf+sizeof("AT+CIFSR\r\r\n")-1; /* skip beginning */
      SkipNewLines(&p);
      UTIL1_strcpy(ipBuf, ipBufSize, p); /* copy IP information string */
    } else {
      res = ERR_FAILED;
    }
  }
  if (res!=ERR_OK) {
    UTIL1_strcpy(ipBuf, ipBufSize, "ERROR");
  }
  return res;
}

uint8_t ESP_OpenConnection(int8_t ch_id, bool isTCP, const uint8_t *IPAddrStr, uint16_t port, uint16_t msTimeout, const CLS1_StdIOType *io) {
  /* AT+CIPSTART=4,"TCP","184.106.153.149",80 */
  uint8_t txBuf[54];
  uint8_t rxBuf[72];
  uint8_t expected[72];

  UTIL1_strcpy(txBuf, sizeof(txBuf), "AT+CIPSTART=");
  if (ch_id>=0) {
    UTIL1_strcatNum16u(txBuf, sizeof(txBuf), ch_id);
    UTIL1_chcat(txBuf, sizeof(txBuf), ',');
  }
  if (isTCP) {
    UTIL1_strcat(txBuf, sizeof(txBuf), "\"TCP\",\"");
  } else {
    UTIL1_strcat(txBuf, sizeof(txBuf), "\"UDP\",\"");
  }
  UTIL1_strcat(txBuf, sizeof(txBuf), IPAddrStr);
  UTIL1_strcat(txBuf, sizeof(txBuf), "\",");
  UTIL1_strcatNum16u(txBuf, sizeof(txBuf), port);

  UTIL1_strcpy(expected, sizeof(expected), txBuf);
  UTIL1_strcat(expected, sizeof(expected), "\r\r\n\r\nOK\r\nLinked\r\n");

  UTIL1_strcat(txBuf, sizeof(txBuf), "\r\n");

  return ESP_SendATCommand(txBuf, rxBuf, sizeof(rxBuf), expected, msTimeout, io);
}

static uint8_t ESP_PrintStatus(const CLS1_StdIOType *io) {
  uint8_t buf[48];

  CLS1_SendStatusStr("ESP8266", "\r\n", io->stdOut);

  CLS1_SendStatusStr("  Webserver", ESP_WebServerIsOn?"ON\r\n":"OFF\r\n", io->stdOut);

  if (ESP_GetFirmwareVersionString(buf, sizeof(buf)) != ERR_OK) {
    UTIL1_strcpy(buf, sizeof(buf), "FAILED\r\n");
  } else {
    /* 00160901: 0016 is SDK version, 0901 is the AT version */
    UTIL1_strcat(buf, sizeof(buf), "\r\n");
  }
  CLS1_SendStatusStr("  AT+GMR", buf, io->stdOut);

  if (ESP_GetModeString(buf, sizeof(buf)) != ERR_OK) {
    UTIL1_strcpy(buf, sizeof(buf), "FAILED\r\n");
  } else {
    if (UTIL1_strcmp(buf, "1")==0) {
      UTIL1_strcat(buf, sizeof(buf), " (device)");
    } else if (UTIL1_strcmp(buf, "2")==0) {
      UTIL1_strcat(buf, sizeof(buf), " (AP)");
    } else if (UTIL1_strcmp(buf, "3")==0) {
      UTIL1_strcat(buf, sizeof(buf), " (device+AP)");
    } else {
      UTIL1_strcat(buf, sizeof(buf), " (ERROR)");
    }
    UTIL1_strcat(buf, sizeof(buf), "\r\n");
  }
  CLS1_SendStatusStr("  AT+CWMODE?", buf, io->stdOut);

  if (ESP_GetIPAddrString(buf, sizeof(buf)) != ERR_OK) {
    UTIL1_strcpy(buf, sizeof(buf), "FAILED\r\n");
  } else {
    UTIL1_strcat(buf, sizeof(buf), "\r\n");
  }
  CLS1_SendStatusStr("  AT+CIFSR", buf, io->stdOut);

  if (ESP_GetConnectedAPString(buf, sizeof(buf)) != ERR_OK) {
    UTIL1_strcpy(buf, sizeof(buf), "FAILED\r\n");
  } else {
    UTIL1_strcat(buf, sizeof(buf), "\r\n");
  }
  CLS1_SendStatusStr("  AT+CWJAP?", buf, io->stdOut);

  if (ESP_GetCIPMUXString(buf, sizeof(buf)) != ERR_OK) {
    UTIL1_strcpy(buf, sizeof(buf), "FAILED\r\n");
  } else {
    if (UTIL1_strcmp(buf, "0")==0) {
      UTIL1_strcat(buf, sizeof(buf), " (single connection)");
    } else if (UTIL1_strcmp(buf, "1")==0) {
      UTIL1_strcat(buf, sizeof(buf), " (multiple connections)");
    } else {
      UTIL1_strcat(buf, sizeof(buf), " (ERROR)");
    }
    UTIL1_strcat(buf, sizeof(buf), "\r\n");
  }
  CLS1_SendStatusStr("  CIPMUX", buf, io->stdOut);
  return ERR_OK;
}

static void RADIO_PrintStatus(const CLS1_StdIOType *io) {
  uint8_t buf[24];
  uint8_t val0, val1;
  
  CLS1_SendStatusStr((unsigned char*)"Radio", (unsigned char*)"\r\n", io->stdOut);
  
  CLS1_SendStatusStr((unsigned char*)"  state", RadioStateStr(RADIO_AppStatus), io->stdOut);
  CLS1_SendStr((unsigned char*)"\r\n", io->stdOut);
  
  CLS1_SendStatusStr((unsigned char*)"  sniff", RADIO_isSniffing?(unsigned char*)"yes\r\n":(unsigned char*)"no\r\n", io->stdOut);
  
  UTIL1_Num8uToStr(buf, sizeof(buf), RADIO_channel);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
  CLS1_SendStatusStr((unsigned char*)"  channel", buf, io->stdOut);
  
  (void)RF1_ReadObserveTxRegister(&val0, &val1);
  UTIL1_Num8uToStr(buf, sizeof(buf), val0);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" lost, ");
  UTIL1_strcatNum8u(buf, sizeof(buf), val1);
  UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" retry\r\n");
  CLS1_SendStatusStr((unsigned char*)"  OBSERVE_TX", buf, io->stdOut);
#if 0  /* The RPD status will get reset very fast by another (e.g. WLAN) packet. So this is not really a useful feature :-( */
  (void)RF1_ReadReceivedPowerDetector(&val0); /*! \todo only works in RX mode, but somehow this still does not work? */
  if (val0&1) {
    UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"1, > -64 dBm\r\n");
  } else {
    UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"0, < -64 dBm\r\n");
  }
  CLS1_SendStatusStr((unsigned char*)"  RPD", buf, io->stdOut);
#endif
}

uint8_t ESP_PrepareMsgSend(int8_t ch_id, size_t msgSize, uint16_t msTimeout, const CLS1_StdIOType *io) {
  /* AT+CIPSEND=<ch_id>,<size> */
  uint8_t cmd[24], rxBuf[48], expected[48];

  UTIL1_strcpy(cmd, sizeof(cmd), "AT+CIPSEND="); /* parameters are <ch_id>,<size> */
  UTIL1_strcatNum8u(cmd, sizeof(cmd), ch_id);
  UTIL1_chcat(cmd, sizeof(cmd), ',');
  UTIL1_strcatNum16u(cmd, sizeof(cmd), msgSize);
  UTIL1_strcpy(expected, sizeof(expected), cmd); /* we expect the echo of our command */
  UTIL1_strcat(expected, sizeof(expected), "\r\r\n> "); /* expect "> " */
  UTIL1_strcat(cmd, sizeof(cmd), "\r\n");
  return ESP_SendATCommand(cmd, rxBuf, sizeof(rxBuf), expected, msTimeout, io);
}

void I2C_StoreCmd(void) {
#if PL_HAS_UI
    unsigned char buf[32];
    uint8_t strSize;

#if PL_HAS_RUNNER
    buf[0] = '@';
    buf[1] = '\0';
    RUNNER_GetCmdString(buf, sizeof(buf));
#elif PL_HAS_SLIDER
    SLIDER_GetCmdString(buf, sizeof(buf));
#else
    buf[0] = '\0';
#endif
    if (buf[0]!='\0') {
      uint8_t cnt=3;

      while(cnt>0 && memDevice.u.data.cmdLength!=0) { /* poll cmdLength: this will be set to zero by the master if it is ok to place the string */
        FRTOS1_vTaskDelay(50/portTICK_RATE_MS); /* give master some time to clear the flasg */
        cnt--;
      }
      if (cnt==0) { /* timeout. Will loose that command. Not ideal, but simple :-) */
        return; /* get out here */
      }
      strSize = (uint8_t)(UTIL1_strlen(buf)+1); /* size of string including zero byte */
      if (strSize>sizeof(memDevice.u.data.cmd)) {
        strSize = sizeof(memDevice.u.data.cmd);
      }
      EnterCritical();
      memDevice.u.data.cmdLength = strSize;
      UTIL1_strcpy(memDevice.u.data.cmd, sizeof(memDevice.u.data.cmd), buf);
      ExitCritical();
    }
#endif /* PL_HAS_UI */
}

/*!
 \brief A simple state machine iterating through different transceiver states
 */
static void RADIO_HandleState(void) {
  tTxPacket ackTxPacket;  /* SMAC structure for TX packets   */
  static uint8_t ackTxDataBuffer[SMAC1_RADIO_BUF_SIZE];    /*Data buffer to hold TX data */

  switch (RADIO_AppStatus) {
    case RADIO_INITIAL_STATE:
        RADIO_AppStatus = RADIO_RECEIVER_ALWAYS_ON;
        break;

    case RADIO_RECEIVER_ALWAYS_ON:
        RADIO_AppStatus = RADIO_READY_FOR_TX_RX_DATA;
        (void)SMAC1_MLMERXEnableRequest(0); /* Zero means wait forever with RX ON. */
        break;

    case RADIO_READY_FOR_TX_RX_DATA: /* we are ready to receive/send data data */
        break;

    case RADIO_TRANSMIT_DATA:
        if (SMAC1_MLMERXDisableRequest() != SMAC1_SUCCESS) { /* Turn off the RX forever mode. */
          RADIO_AppStatus = RADIO_TRANSMIT_DATA; /* retry */
          break;
        }
        LED1_Neg();
        if ((SMAC1_MCPSDataRequest(&RADIO_TxPacket) == SMAC1_SUCCESS)) { /* transmit data */
          RADIO_AppStatus = RADIO_WAITING_FOR_ACK;
          (void)SMAC1_MLMERXEnableRequest(RADIO_TIMEOUT_COUNT);
        } else {
          RADIO_AppStatus = RADIO_RECEIVER_ALWAYS_ON; /* what should we otherwise do? */
        }
        break;

    case RADIO_TRANSMIT_ACK:
        /*Initialize the packet.*/
        /*! \todo RADIO: Below we send back the acknowledge message:
         *        check that your RADIO_ACK_STR is what you want and need.
         *        Notice the implicit string concatenation for efficiency and reduced code size. */
        UTIL1_strcpy(ackTxDataBuffer, sizeof(ackTxDataBuffer), (unsigned char*)RADIO_PREFIX_STR RADIO_ACK_STR);
        ackTxPacket.pu8Data = &ackTxDataBuffer[0]; /* Load the address of our txbuffer into the tx structure*/
        ackTxPacket.u8DataLength = (byte)(UTIL1_strlen((char*)ackTxDataBuffer)+1); /* set the size of the packet */
        (void)SMAC1_MCPSDataRequest(&ackTxPacket); /* transmit data */
        RADIO_AppStatus = RADIO_RECEIVER_ALWAYS_ON;
        break;

    case RADIO_RESET_STATE:
        /* MC13192 Reset, reinitialize and return to default state.   */
        SMAC1_RadioInit();
        RADIO_AppStatus = RADIO_INITIAL_STATE;
        break;

    case RADIO_WAITING_FOR_ACK:
        /* At this point only two things happen, 1-we receive the ack packet or 2-timeout.
         * Either way the TX will leave this state and continue. Low power mode could be placed here
         * because both 1 and 2 are interrupt driven, in this case we keep it simple
         */
        break;

    default:
        break;
  }
}

static void Radio_StdIOSendChar(byte ch) {
#define TX_BUF_SIZE   (RADIO_MAX_TX_DATA_SIZE-sizeof("stdout ")) /* data size we can transmit in one message */
    unsigned char buf[TX_BUF_SIZE+sizeof("stdout ")];
    uint8_t i;

    CLS1_GetStdio()->stdOut(ch); /* copy on local shell */
    if (RadioTx_Put(ch)!=ERR_OK) {
        /* lost character */
    }
    if (ch=='\n' || RadioTx_NofElements()>TX_BUF_SIZE) { /* send string over radio */
        UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"stdout ");
        i = sizeof("stdout ")-1;
        while (i<sizeof(buf) && RadioTx_Get(&buf[i])==ERR_OK) {
            i++;
        }
        if (RADIO_SendData(buf, i)!=ERR_OK) {
            CLS1_GetStdio()->stdOut('\n');
            CLS1_GetStdio()->stdOut('*');
            CLS1_GetStdio()->stdOut('F');
            CLS1_GetStdio()->stdOut('A');
            CLS1_GetStdio()->stdOut('I');
            CLS1_GetStdio()->stdOut('L');
            CLS1_GetStdio()->stdOut('*');
            CLS1_GetStdio()->stdOut('\n');
        }
    }
}

static uint8_t Tune(const CLS1_StdIOType *io, uint8_t channel, MOT_MotorDevice *motorHandle) {
	#define TUNE_MOTOR_PERCENT 20
  uint16_t dac;
  int i;
  QuadTime_t timing;
  uint8_t buf[48];
  uint8_t res;
 
//#if PL_HAS_DRIVE
//  DRV_SetMode(DRV_MODE_NONE); /* turn off drive mode */
//#endif

MOT_SetSpeedPercent(motorHandle, TUNE_MOTOR_PERCENT);
  CLS1_SendStr((uint8_t*)"Tuning channel...\r\n", io->stdOut);
  res = ERR_FAILED;
  for(i=0,dac=0;dac<=MCP4728_MAX_DAC_VAL;i++) {
    UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)"Channel: ");
    UTIL1_chcat(buf, sizeof(buf), (uint8_t)('A'+channel)); /* 0:A, 1:B, 2:C, 3:D */
    UTIL1_strcat(buf, sizeof(buf), (uint8_t*)" DAC: 0x");
    UTIL1_strcatNum16Hex(buf, sizeof(buf), dac);
    UTIL1_chcat(buf, sizeof(buf), ' ');
    CLS1_SendStr(buf, io->stdOut);
    if (MCP4728_FastWriteDAC(channel, dac)!=ERR_OK) { /* writes single channel DAC value, not updating EEPROM */
      CLS1_SendStr((uint8_t*)"ERROR writing DAC channel!\r\n", io->stdErr);
      res = ERR_FAILED;
      break;
    }
    WAIT1_WaitOSms(100); /* wait some time to allow DAC and OP-Amp change */
    if (Measure(channel, &timing)==ERR_OK) {
      buf[0] = '\0';
      UTIL1_strcatNum8u(buf, sizeof(buf), timing.highPercent);
      UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"% high, low ");
      UTIL1_strcatNum8u(buf, sizeof(buf), timing.lowPercent);
      UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"%\r\n");
      CLS1_SendStr(buf, io->stdOut);
      if (timing.highPercent==50 || timing.lowPercent==50) {
        CLS1_SendStr((uint8_t*)"Set!\r\n", io->stdErr);
        CLS1_SendStr((uint8_t*)"Writing to EEPROM...\r\n", io->stdOut);
        if (MCP4728_WriteDACandEE(channel, dac)!=ERR_OK) {
          CLS1_SendStr((uint8_t*)"ERROR writing DAC/EEPROM\r\n", io->stdErr);
          res = ERR_FAILED;
          break;
        }
        CLS1_SendStr((uint8_t*)"...done!\r\n", io->stdOut);
        res = ERR_OK;
        break; /* go to next channel */
      }
      dac += 0x1; /* smaller increase */
    } else {
      CLS1_SendStr((uint8_t*)"No signal\r\n", io->stdErr);
      dac += 0x10; /* larger increase */
    }
  } /* for finding DAC value */
  MOT_SetSpeedPercent(motorHandle, 0); /* turn off again */
  if (res!=ERR_OK) {
    CLS1_SendStr((uint8_t*)"ERROR!\r\n", io->stdErr);
  }
  CLS1_SendStr((uint8_t*)"Tuning finished!\r\n", io->stdOut);
  return res;
}

void APP_Run(void) {
  int i;
  uint32_t val = 0;
  unsigned char buf[16];

  for(;;) {
    while(CDC1_App_Task(cdc_buffer, sizeof(cdc_buffer))==ERR_BUSOFF) {
      /* device not enumerated */
      LED1_Neg(); LED2_Off();
      WAIT1_Waitms(10);
    }
    LED2_Off(); LED1_Neg();
    if (CDC1_GetCharsInRxBuf()!=0) {
      i = 0;
      while(   i<sizeof(in_buffer)-1
            && CDC1_GetChar(&in_buffer[i])==ERR_OK
           )
      {
        i++;
      }
      in_buffer[i] = '\0';
      (void)CDC1_SendString((unsigned char*)"echo: ");
      (void)CDC1_SendString(in_buffer);
      UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"val: ");
      UTIL1_strcatNum32u(buf, sizeof(buf), val);
      UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
      (void)CDC1_SendString(buf);
      val++;
    } else {
      WAIT1_Waitms(10);
    }
  }
}