linux c++ 掃描按鍵, 透過 UDP 遙控裝置

#include <sys/time.h> // struct timeval
#include <termios.h>  // struct termios
#include <unistd.h>   // close
#include <stdio.h>    // printf
#include <string.h>   // memset
#include <stdlib.h>   // atoi
#include <netdb.h>    // gethostbyname
#include <arpa/inet.h>// inet_ntoa
#include <signal.h>  // signal

class DeltaPWM {
  private:
    unsigned char value, click;
  public:
    void reset() {// reset to default value
      click  = 0;
      value  = 1;
    }
    auto operator ()(int key = 0) {
      if (key > 0) {
        if (key != click) { // key changes
          value = 1;// begin from 1
          click = key;// keep to compare next time
        } else if (value < 10) value ++;// same key pressed
      }// saturat at 10, increase nonlinearly
      return value;
    };
    DeltaPWM() { reset(); }
};

int main(void) {
  const char *hostname = "myDecive.local";// remote target
  const int udpPort = 8888;// service port number
  signal(SIGINT, [](int sig) {
    printf("CTRL-C\n");  
  });

  printf("Waiting: %s...\n", hostname);
  struct hostent *host = gethostbyname(hostname);
  if (host == NULL) {
    printf("%s not known\n", hostname);
    return 1; // NS lookup fail, nothing to do
  }
  int udpFD = socket(AF_INET, SOCK_DGRAM, 0);
  if (udpFD < 0) return 2; // socket init fail, nothing to do
    
  printf("'q' or double click 'ESC' to break, Arrow keys to remote control.\n");
  struct sockaddr_in udpAddr; // allocate a struct sockaddr_in from stack
  memset(&udpAddr, 0, sizeof(udpAddr));// empty udpAddr
  udpAddr.sin_family = AF_INET;
  bcopy(host->h_addr_list[0], (caddr_t) &udpAddr.sin_addr, host->h_length);
  // udpAddr.sin_addr.s_addr = inet_addr(inet_ntoa(*(struct in_addr *) host->h_addr));
  udpAddr.sin_port = htons(udpPort);
  auto udpSend = [&](const char *msg) {
      int len = strlen(msg);
      if(len > 0) sendto(udpFD, msg, len, 0,
          (const struct sockaddr *) &udpAddr,
           sizeof(struct sockaddr_in)
      );              
  };
 
  static char keyBuffer[1024]; // share keyBuffer
  static auto parseKey = [](const char *key, int *value) -> bool {
    static char valBuffer[32];
    if (keyBuffer[0] == 0 || ! key) return false;// scan fail
    char *findKey = strstr(keyBuffer, key);
    if (findKey) {
      char *rewind = findKey;
      while(rewind > keyBuffer) {// check if not comment
        rewind --;
        char &ar = *rewind;
        if (ar == '\n' || ar == '\r' || ar == ',' || ar == '#') break;
      }
      if(*rewind != '#') { // discard comment
        findKey += strlen(key);
        if(*findKey == '=') findKey ++;// skip '=' if provide
        char *dest = valBuffer;
        for(int i = 0; i < 31; i ++) { // tupto 31 characters
          char &af = *findKey ++;
          if (af == 0 || af == '\n' || af == '\r' || af == ' ' || af == ',' || af == '#') break;
          *dest ++ = af;
        }
        *dest = 0;// EOS to split string
        *value = atoi(valBuffer);// convert to integer
        return true;// sucess
      }
    }
    *value = 0;  // value invalid
    return false;// scan fail
  };
      
  struct termios keepTerm;
  tcgetattr(STDIN_FILENO, &keepTerm);
  auto term = keepTerm;// duplicate
  term.c_lflag &= ~(ICANON | ECHO); // mask for ICANON + ECHO
  tcsetattr(STDIN_FILENO, TCSANOW, &term);// set a new configuration

  DeltaPWM dp, dz, dr;
  int power =  0;// initial power to send
  int yaw   = 16;// initial yaw to send
  int pitch =  0;// initial pitch to send
  int roll  =  0;// initial roll to send
  int maxFD  = udpFD + 1;
  char txtBuffer[1024];
  sprintf(txtBuffer, "power=%d,yaw=%d,pitch=%d,roll=%d", power, yaw, pitch, roll);
  udpSend(txtBuffer);

  while (true) {
    fd_set fdBits;
    FD_ZERO(&fdBits);// reset all bits
    FD_SET(STDIN_FILENO, &fdBits);
    FD_SET(udpFD, &fdBits);
    struct timeval timeout = {.tv_sec = 0, .tv_usec = 40000};// 40 mSec = 0.04 Sec
    int event = select(maxFD, &fdBits, NULL, NULL, &timeout);
    if (event < 0) break; // error event proceed first

    if(FD_ISSET(udpFD, &fdBits)) { // udp received event
      int len = recv(udpFD, keyBuffer, 1023, 0);
        if (len > 0) {
        keyBuffer[len] = 0;
        printf("###:%s\n", keyBuffer);
        int temp = 0;
        if(parseKey("power", &temp)) power = temp;
        if(parseKey("yaw"  , &temp))   yaw = temp;
        if(parseKey("pitch", &temp)) pitch = temp;
        if(parseKey("roll" , &temp))  roll = temp;
        printf("power:%d@%d, yaw:%4d, pitch:%4d@%4d, roll:%4d@%4d\n",
          power, dp(), yaw, pitch, dz(), roll, dr());
      }// udp packet received, update and show current status
    }

    if(FD_ISSET(STDIN_FILENO, &fdBits)) { // user input event
      int ch = getchar();// printf("KEY CODE = %d\n", ch);
      if (ch == 'q') break;// user break by pressing 'q'
      if (ch == 27) {
        ch = getchar();  // printf("ESC KEY= %d\n", ch);
        if(ch == 27) break;// user break by double ESC clicks
        if(ch == 91) {// keypad scan
          ch = getchar();// printf("ARROW= %d\n", ch);
          switch(ch) {
            case 65: // arrow up, to increase power
                power += dp(ch);
                sprintf(txtBuffer, "power=%d", power);
                break;
            case 66: // arrow down, to decrease power
                power -= dp(ch);
                sprintf(txtBuffer, "power=%d", power);
                break;
            case 67:// arrow right,
                yaw ++;
                sprintf(txtBuffer, "yaw=%d", yaw);
                break;
            case 68:// arrow left,
                yaw --;
                sprintf(txtBuffer, "yaw=%d", yaw);
                break;
            case 50: //Ins, to toward right
                roll += dr(ch);
                sprintf(txtBuffer, "roll=%d", roll);
                break;
            case 51: //Del, to toward left
                roll -= dr(ch);
                sprintf(txtBuffer, "roll=%d", roll);
                break;         
            case 53: // pageup to forward
                pitch += dz(ch);
                sprintf(txtBuffer, "pitch=%d", pitch);
                break;
            case 54: // pagedown to backward                  
                pitch -= dz(ch);
                sprintf(txtBuffer, "pitch=%d", pitch);
                break;
                
            case 70: // home
            case 72: // end
            default:
                txtBuffer[0] = 0;// clear txtBuffer
                printf("KEY: %d not defined\n", ch);
                break;
          }// keyCode validate
          if (strlen(txtBuffer) > 0) udpSend(txtBuffer);
        }// remote control
      }// keyCode scan
    }

    if (event == 0) {// timeout event finally
      dp.reset();
      dz.reset();
      dr.reset();
    }
  }// loop until error happen or user break

  udpSend("power0,yaw0,pitch0,roll0");// set all to zero before end
  close(udpFD);// close udp port
  tcsetattr(STDIN_FILENO, TCSANOW, &keepTerm);// restore termial
  return 0;// no error
}