スマートメーターから瞬時電力を取得して7セグに表示するArduino用スケッチ

SmartMeter.ino

#include <SoftwareSerial.h>

// Detail: http://d.hatena.ne.jp/NeoCat/20160110/1452407542

//-----------------------------------------------------------
// Configurations
//-----------------------------------------------------------

// B-route ID and Password (PANA authentication)
#define ID "0000XXXXXXXXXXXXXXXXXXXXXXXXXXXX"
#define PASSWORD "XXXXXXXXXXXX"

// Serial pins to BP35A1
#define BP35A1_rxPin A4  // <- BP35A1's TXD
#define BP35A1_txPin A5  // -> BP35A1's RXD

// Serial 7seg pins
#define dig_txPin 8      // -> serial 7seg RXpin
#define dig_gndPin 9     // -> serial 7seg GND (to reset)


//-----------------------------------------------------------
// data trasnfer buffer
//-----------------------------------------------------------
const int bufsize = 122;
char buf[bufsize];
const int size = bufsize - 1;

//-----------------------------------------------------------
// debug message utilities
//-----------------------------------------------------------
//#define DEBUG

char errbuf[80];  // error message buffer

#define error0(fmt) Serial.println(F(fmt));
#define error1(fmt, str) Serial.print(F(fmt)), Serial.println(str)
#define error(fmt, ...) sprintf(errbuf, fmt "\r\n", __VA_ARGS__), Serial.print(errbuf)
#define log0(fmt) Serial.println(F(fmt))
#define log1(fmt, str) Serial.print(F(fmt)), Serial.println(str)
#define log(fmt, ...) sprintf(errbuf, fmt "\r\n", __VA_ARGS__), Serial.print(errbuf)
#ifdef DEBUG
#define debug0(fmt, ...) Serial.println(F(fmt))
#define debug1(fmt, str) Serial.print(F(fmt)), Serial.println(str)
#define debug(fmt, ...) sprintf(errbuf, fmt "\r\n", __VA_ARGS__), Serial.print(errbuf)
#else
#define debug0(fmt)
#define debug1(fmt, str)
#define debug(fmt, ...)
#endif

//-----------------------------------------------------------
// BP35A1 communication class
//-----------------------------------------------------------
// Note: Before using this class, BP35A1 UART must be set to
//       9600 bps / 400us pause by "WUART 43" command to
//       ensure software serial to work properly.
//-----------------------------------------------------------

class BP35A1 : public Print {
  SoftwareSerial serial;
  unsigned short tid;
  bool found = false;
  bool connected = false;
  char channel[3];
  char panid[5];
  char addr[17];
  char ipv6_addr[40];
public:
  BP35A1(int rxPin, int txPin) : serial(rxPin, txPin), tid(0) {
    pinMode(rxPin, INPUT);
    pinMode(txPin, OUTPUT);
    serial.begin(9600);
  }
  
  void listen() {
    serial.listen();
  }

  virtual size_t write(uint8_t d) {
    size_t ret = serial.write(d);
    if (ret) {
      // skip echo back
      int max = 255;
      while (!serial.available()) {
        if (!--max)
          break;
      }
      serial.read(); // skip echo back
    }
    return ret;
  }
  
  void write_ln(const char *cmd, const char *arg) {
    // Serial.println(cmd);
    if (cmd)
      Print::write((uint8_t*)cmd, strlen(cmd));
    if (arg)
      Print::write((uint8_t*)arg, strlen(arg));
    write('\r');
    if (cmd) debug1("> ", cmd);
    if (arg) debug1("> ", arg);
  }

  bool available() {
    return serial.available();
  }
  
  int read() {
    return serial.read();
  }

  int read_sync(int maxsec=-1) {
    unsigned long start_ms = millis();
    unsigned long end_ms = start_ms + 1000*maxsec;
    int ret = -1;
    do {
      while (!serial.available())
        if (maxsec >= 0 && long(millis() - end_ms) >= 0)
          return -1;
      ret = read();
    } while (ret == '\n');
    return ret;
  }
  
  int readline(char *resbuf, int size, int maxsec=-1, bool need_flush=true) {
    for (int p = 0; p < size; p++) {
      resbuf[p] = read_sync(maxsec);
      if (resbuf[p] == '\r') {
        resbuf[p] = 0;
        debug1("< ", resbuf);
        return p;
      }
      if (resbuf[p] <= 0) {
        resbuf[p] = 0;
        return -1;
      }
    }
    debug1("<.. ", resbuf);
    if (need_flush)
      flush();
    return size;
  }

  void flush() {
    int ret;
    do {
      ret = read_sync();
    } while (ret != '\r' && ret > 0);
  }

  int hex(char c) {
    if (c >= '0' && c <= '9') return c - '0';
    if (c >= 'A' && c <= 'F') return c - 'A' + 10;
    if (c >= 'a' && c <= 'f') return c - 'a' + 10;
    return -1;
  }
  
  bool startswith(const char *str, const char *search) {
    while (*search)
      if (*(search++) != *(str++))
        return false;
    return true;
  }
  
  int recv_udp(char *resbuf, int size, int len) {
    unsigned short port, psize;
    if (size < 7+40+40+5+5+17+2+5) {
      error0("buffer is too short to handle ERXUDP!");
      flush();
      return -1;
    }
    int offset = 7+40+40+5;
    port = (hex(resbuf[offset+0]) << 12) |
      (hex(resbuf[offset+1]) << 8) |
      (hex(resbuf[offset+2]) << 4) |
      (hex(resbuf[offset+3]));
    offset = 7+40+40+5+5+17+2;
    psize = (hex(resbuf[offset+0]) << 12) |
      (hex(resbuf[offset+1]) << 8) |
      (hex(resbuf[offset+2]) << 4) |
      (hex(resbuf[offset+3]));
    log("UDP (port:%d size:%d): ", port, psize);
    if (len > 7+40+40+5+5+17+2+5)
      log1("  ", resbuf+7+40+40+5+5+17+2+5);
    while (size == len) {
      len = readline(resbuf, size, -1, false);
      if (len < 0)
        break;
      log1("  ", resbuf);
    }
    return psize;
  }
  
  enum {
    WAIT_UDP = 0x01,
    WAIT_OK = 0x02,
    HANDLE_PAN = 0x04,
  };
  
  int wait_response(byte *events, int elen, unsigned int flags,
                    bool &ok, int maxsec, char *resbuf, int size) {
    for (;;) {
      int len = readline(resbuf, size, maxsec, false);
      if (len == 0 || len == 1 && resbuf[0] == 0)
        continue;
      if (len < 0)
        return len;
  
      if (startswith(resbuf, "ERXUDP")) {
        int ret = recv_udp(resbuf, size, len);
        if (flags & WAIT_UDP)
          return ret;
        continue;
      }
  
      if (len == size)
        flush();
  
      if (startswith(resbuf, "OK")) {
        ok = true;
        if (flags & WAIT_OK)
          return 0;
        continue;
      }
      if (startswith(resbuf, "FAIL")) {
        error1("command failed -> ", resbuf);
        return -1;
      }
      if (startswith(resbuf, "EVER")) {
        log1("Version: ", resbuf + 5);
        continue;
      }
      if (startswith(resbuf, "EVENT")) {
        byte id = (hex(resbuf[6]) << 4) | hex(resbuf[7]);
        log("Event %x", id);
        for (int i = 0; i < elen; i++)
          if (id == events[i])
            return id;
        continue;
      }
      if (startswith(resbuf, "EPANDESC")) {
        if (!(flags & HANDLE_PAN))
          continue;
        found = true;
        readline(resbuf, size); // "  Channel:xx"
        strcpy(channel, resbuf + 10);
        readline(resbuf, size); // "  Channel Page:xx"
        readline(resbuf, size); // "  Pan ID:xxxx"
        strcpy(panid, resbuf + 9);
        readline(resbuf, size); // "  Addr:xxxxxxxxxxxxxxxx"
        strcpy(addr, resbuf + 7);
        readline(resbuf, size); // "  LQI:xx"
        char lqi[3] = {resbuf[6],resbuf[7],0};
        readline(resbuf, size); // "  PairID:xxxx"
        log("Pan ID %s (Channel:%s Addr:%s LQI:%s) found!",
            panid, channel, addr, lqi);
        continue;
      }
  
      error1("unknown response -> ", resbuf);
    }
  }

   int wait_udp(char *resbuf, int size, int maxsec, bool &ok) {
    if (size < 121) {
      error0("buffer size is too short!");
      return -1;
    }
    return wait_response(NULL, 0, WAIT_UDP, ok, maxsec, resbuf, size);
  }
  
  int wait_event(byte *events, int elen, char *resbuf, int size) {
    bool ok = false;
    return wait_response(events, elen, HANDLE_PAN, ok, -1, resbuf, size);
  }
  
  int exec_command(const char *cmd, const char *arg, char *resbuf, int size) {
    write_ln(cmd, arg);
    bool ok = false;
    return wait_response(NULL, 0, WAIT_OK, ok, -1, resbuf, size);
  }
  
  bool setup(const char *id, const char *password, char *buf, int size) {
    listen();
    disconnect(buf, size);

    if (exec_command("SKVER", NULL, buf, size) < 0)
      return false;
    if (exec_command("ROPT", NULL, buf, size) < 0)
      return false;
    if (buf[5] == '0') { // "OK 00"
      log0("enabling ASCII dump mode");
      if (exec_command("WOPT 01", NULL, buf, size) < 0)
        return false;
    }
  
    if (exec_command("SKSETPWD C " , password, buf, size)) // set password
      return false;
    if (exec_command("SKSETRBID ", id, buf, size))          // set RBID
      return false;
     
     return true;
  }

  void active_scan(char *buf, int size) {
    log0("Scanning PAN ...");
    if (exec_command("SKSCAN 2 FFFFFFFF 6", NULL, buf, size)) { // active scan
      delay(2000);
      return;
    }
    byte events[] = {0x22};
    wait_event(events, 1, buf, size);
    if (!found) {
      error0("PAN not found");
      delay(2000);
    }
  }
  
  bool connect(char *buf, int size) {
   rescan:
    while (!found)
      active_scan(buf, size);
  
    if (exec_command("SKSREG S2 ", channel, buf, size))
      return false;
    if (exec_command("SKSREG S3 ", panid, buf, size))
      return false;
    write_ln("SKLL64 ", addr);
    readline(buf, size); // echo
    if (readline(ipv6_addr, 40) != 39) {
      error1("invalid ipv6 address: ", ipv6_addr);
      return false;
    }
  
    log1("Connecting to ", ipv6_addr);
    if (exec_command("SKJOIN ", ipv6_addr, buf, size))
      return false;
    byte events[] = {0x24, 0x25};
    if (wait_event(events, 2, buf, size) != 0x25) {
      error0("connection failed");
      found = false;
      return false;
    }
    log1("Connected to ", ipv6_addr);
    connected = true;
    bool ok = false;
    wait_udp(buf, size, 3, ok); // wait instance announce
    return true;
  }

  void disconnect(char *buf, int size) {
    exec_command("SKTERM", NULL, buf, size);  // disconnect forcively
    delay(1000);
  }

  bool epc_get(unsigned char epc, char *buf, int size) {
    bool ok = false;
    Print::write("SKSENDTO 1 ", 11);
    Print::write(ipv6_addr, 39);
    char msg[] = " 0E1A 1 000E \x10\x81\x00\x01\x05\xFF\x01\x02\x88\x01\x62\x01\xe7\x00";
    tid++;
    msg[15] = tid >> 8;
    msg[16] = tid & 0xff;
    msg[25] = epc;
    Print::write(msg, sizeof(msg)-1);
    for (;;) {
      int ret = wait_udp(buf, size, 5, ok);
      if (ret < 0) {
        error1("wait_udp failed! -> ", buf);
        return false;
      }
      if (ret < 14) {
        error1("response error: ", buf);
        return false;
      }
      unsigned short rtid = (hex(buf[4]) << 12) | (hex(buf[5]) << 8) |
                            (hex(buf[6]) << 4) | hex(buf[7]);
      if (rtid != tid)
        continue;
      if (buf[20] == '5' && buf[21] == '2') {
        error0(" ** Get EPC not supported");
        return false;
      }
      if (buf[20] != '7' || buf[21] != '2') {
        error1("unknown message received: ", buf);
        return false;
      }
      epc = (hex(buf[24]) << 4) | hex(buf[25]);
      log(" ** EPC %x = ", epc);
      log1("   ", buf+28);
      if (!ok)
        exec_command(NULL, NULL, buf, size);
      return true;
    }
  }
  
  void epc_set(unsigned char epc, char *data, int len, char *buf, int size) {
    bool ok;
    Print::write("SKSENDTO 1 ", 11);
    Print::write(ipv6_addr, 39);
    char msg[64] = " 0E1A 1 000E \x10\x81\x00\x01\x05\xFF\x01\x02\x88\x01\x61\x01\xe7";
    msg[15] = tid >> 8;
    msg[16] = tid & 0xff;
    tid++;
    msg[25] = epc;
    msg[26] = len;
    memcpy(msg+27, data, len);
    sprintf(msg + 8, "%04X", len+14);
    msg[12] = ' ';
    Print::write(msg, len+27);
    int ret = wait_udp(buf, size, 3, ok);
    if (ret < 0) {
      error1("wait_udp failed! -> ", buf);
      return;
    }
    if (ret < 14) {
      error1("response error: ", buf);
      return;
    }
    if (buf[20] == '5' && buf[21] == '1') {
      error0(" ** Set EPC not supported");
      return;
    }
    if (buf[20] != '7' || buf[21] != '1') {
      error1("unknown message received: ", buf);
      return;
    }
    log(" ** EPC %x =>", epc);
    log1("    ", buf+28);
    if (!ok)
      exec_command(NULL, NULL, buf, size);
  }
};


//-----------------------------------------------------------
// Main routine
//-----------------------------------------------------------

BP35A1 wisun(BP35A1_rxPin, BP35A1_txPin);  // BP35A1 instance

SoftwareSerial dig(-1, dig_txPin);         // serial 7seg display

void setup()  {
  Serial.begin(115200);
  
  // setup serial 7seg display
  pinMode(dig_txPin, OUTPUT);
  pinMode(dig_gndPin, INPUT);  // power off 7seg display
  delay(100);
  pinMode(dig_gndPin, OUTPUT); // power on 7seg display
  digitalWrite(dig_gndPin, LOW);
  dig.begin(9600);

  Serial.println("Start");

  buf[size] = 0;

  if (!wisun.setup(ID, PASSWORD, buf, size)) {
    error0("setup failed");
    dig.print("eeee");
    for (;;);  // stop forever on error
  }

  dig.print("cccc");
  while (!wisun.connect(buf, size))
    delay(5000);  // reconnect on error after 5 sec

  // check if the smart meter is in running state (EPC 0x80 = 0x30)
  byte val = 0;
  if (wisun.epc_get(0x80, buf, size))
    val = (wisun.hex(buf[28]) << 4) | wisun.hex(buf[29]);
  if (val != 0x30)
    error0("SmartMeter is not working!");

  // check if the smart meter is not in error state (EPC 0x88 = 0x42)
  val = 0;
  if (wisun.epc_get(0x88, buf, size))
    val = (wisun.hex(buf[28]) << 4) | wisun.hex(buf[29]);
  if (val != 0x42)
    error0("SmartMeter is in error state!");
}

void loop() {
  static int err_cnt = 0;

  log0("getting EPC 0xe7 ...");
  bool ret = wisun.epc_get(0xe7, buf, size);

  if (ret) {
    // convert hex string into unsigned long
    unsigned long val = 0;
    for (int i = 28; buf[i]; i++) {
      val <<= 4;
      val |= wisun.hex(buf[i]);
    }

    // print immediate power value to Serial
    Serial.print(F("Power = "));
    Serial.print(val);
    Serial.println(F(" Watt"));

    // display power value to 7seg
    if (val > 9999) val = 9999;
    if (val < 1000) dig.print(0);
    if (val < 100) dig.print(0);
    if (val < 10) dig.print(0);
    dig.print(val);

    err_cnt = 0;
  } else {
    err_cnt++;
    error0("EPC get failed");
    if (err_cnt >= 5) {
      err_cnt = 0;
      dig.print("cccc");
      log0("reconnecting ...");
      wisun.disconnect(buf, size);
      wisun.connect(buf, size);
    }
  }
  
  bool ok;
  unsigned long end = millis() + 3000; // wait 3 sec
  while (long(millis() - end) < 0)
    wisun.wait_udp(buf, size, 3, ok);
}