benedettelli-nxt2wifi-test1.c

#pragma config(Sensor, S1,     HTBM,           sensorI2CCustom)
#pragma config(Sensor, S2,     LIGHT,          sensorLightInactive)
#pragma config(Sensor, S3,     SOUND,          sensorSoundDB)
#pragma config(Sensor, S4,     NXT2WIFI,       sensorHighSpeed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
 * $Id: benedettelli-nxt2wifi-test1.c 133 2013-03-10 15:15:38Z xander $
 */

/**
 * benedettelli-nxt2wifi.h provides an API for Daniele's NXT2WiFi Sensor. This program
 * demonstrates how to use that API.  This program is an SNMP agent implementation.
 * Query it with:
 *
 * Fetch current temperature: snmpget  -r 0 -c public -v 1 $IP iso.3.6.1.3.1
 * Fetch current pressure: snmpget  -r 0 -c public -v 1 $IP iso.3.6.1.3.2
 * Fetch current sound level: snmpget  -r 0 -c public -v 1 $IP iso.3.6.1.3.3
 * Fetch current ambient light level: snmpget  -r 0 -c public -v 1 $IP iso.3.6.1.3.4
 * To query the brick's friendly name, use snmpget  -r 0 -c public -v 1 $IP iso.3.6.1.2.1.1.5.0
 *
 * More info here: http://botbench.com/blog/2012/06/11/little-big-data-splunk-meets-nxt/
 *
 * Changelog:
 * - 0.1: Initial release
 *
 * Credits:
 * - Big thanks to Daniele Benedettelli for providing me with the hardware necessary to write and test this.
 *
 * License: You may use this code as you wish, provided you give credit where it's due.
 *
 * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER. 

 * Xander Soldaat (xander_at_botbench.com)
 * 22 July 2012
 * version 0.1
 */

#include "drivers/common.h"
#include "drivers/timer.h"
#include "drivers/hitechnic-barometer.h"
#include "drivers/benedettelli-nxt2wifi.h"

// These are the ASN constants as used by SNMP
#define ASN_INTEGER           0x02
#define ASN_OCTET_STRING      0x04
#define ASN_NULL              0x05
#define ASN_OBJECT_IDENTIFIER 0x06
#define ASN_SEQUENCE          0x30
#define ASN_IPADDRESS         0x40
#define ASN_COUNTER32         0x41
#define ASN_GAUGE32           0x42
#define ASN_TIMETICKS         0x43

// These are SNMP protocol specific constants
#define SNMP_VERSION_1        0x00
#define SNMP_GET_REQUEST      0xA0
#define SNMP_NEXT_REQUEST     0xA1
#define SNMP_GET_RESPONSE     0xA2
#define SNMP_SET_REQUEST      0x03

long reqID;

long dataTemp = 0;
long dataPress = 0;
long dataLight = 0;
long dataSound = 0;
long txbytes = 0;
long rxbytes = 0;
string IPaddress = "0.0.0.0";
string connStatus = "disconnected";
tHugeByteArray data;
tHugeByteArray payloadData;

task updateScreen()
{
  while(true)
  {
    nxtDisplayTextLine(0, "Stat: %s", connStatus);
    nxtDisplayTextLine(1, "%s",IPaddress);
    nxtDisplayTextLine(2, "-------------------");
    nxtDisplayTextLine(3, "Temp : %2.2f", (float)dataTemp/100.0);
    nxtDisplayTextLine(4, "Press: %d", dataPress);
    nxtDisplayTextLine(5, "Light: %d", dataLight);
    nxtDisplayTextLine(6, "Sound: %d", dataSound);
    nxtDisplayTextLine(7, "RX/TX: %d/%d", rxbytes, txbytes);
    wait1Msec(100);
  }
}

// Constantly read the sensors
task pollSensors()
{
  while (true)
  {
    dataPress = HTBMreadMInHg(HTBM);
    wait1Msec(100);
    dataTemp = HTBMreadTemp(HTBM) * 100;
    wait1Msec(100);
    dataLight = SensorValue[LIGHT];
    wait1Msec(100);
    dataSound = SensorValue[SOUND];
    wait1Msec(100);
  }
}

int genSNMPreply(long reqNo, string &communityName, tByteArray &oid, ubyte oidlen, ubyte dataType, tHugeByteArray &payloadData, int size)
{
  ubyte communityNameLen = strlen(communityName);
  ubyte payloadSize = size;

  // Precalculate all the offsets
  ubyte OFFS_VERS = 2;
  ubyte OFFS_COMM = 5;
  ubyte OFFS_RQTP = 7 + strlen(communityName);
  ubyte OFFS_RQID = 2 + OFFS_RQTP;
  ubyte OFFS_ERNO = 6 + OFFS_RQID;
  ubyte OFFS_ERID = 3 + OFFS_ERNO;
  ubyte OFFS_SEQ1 = 3 + OFFS_ERID;
  ubyte OFFS_SEQ2 = 2 + OFFS_SEQ1;
  ubyte OFFS_OBID = 2 + OFFS_SEQ2;
  ubyte OFFS_DATA = oidlen + 2 + OFFS_OBID;
  ubyte TOTL_SIZE = 2 + OFFS_DATA + payloadSize;


  memset(data, 0, sizeof(data));

  writeDebugStreamLine("TOTL_SIZE: %d", TOTL_SIZE);
  writeDebugStreamLine("oidlen: %d", oidlen);
  writeDebugStreamLine(communityName);

  // Inital sequence marker
  data[0] = 0x30;
  data[1] = OFFS_DATA + payloadSize;

  // SNMP version part
  data[OFFS_VERS + 0] = ASN_INTEGER;
  data[OFFS_VERS + 1] = 1;
  data[OFFS_VERS + 2] = SNMP_VERSION_1;

  // Community
  data[OFFS_COMM + 0] = ASN_OCTET_STRING;
  data[OFFS_COMM + 1] = communityNameLen;
  memcpy(&data[OFFS_COMM + 2], communityName, communityNameLen);

  // Request type
  data[OFFS_RQTP + 0] = SNMP_GET_RESPONSE;
  data[OFFS_RQTP + 1] = TOTL_SIZE - OFFS_RQID;

  // Request ID
  // 6=requestID, 3=error, 3=erroridx, 2=sequence, 2=sequence,2=payload
  data[OFFS_RQID + 0] = ASN_INTEGER;
  data[OFFS_RQID + 1] = 4;  // we're sending a long
  data[OFFS_RQID + 2] = (reqNo >> 24) & 0xFF;
  data[OFFS_RQID + 3] = (reqNo >> 16) & 0xFF;
  data[OFFS_RQID + 4] = (reqNo >>  8) & 0xFF;
  data[OFFS_RQID + 5] = (reqNo >>  0) & 0xFF;

  // error
  data[OFFS_ERNO + 0] = ASN_INTEGER;
  data[OFFS_ERNO + 1] = 1;
  data[OFFS_ERNO + 2] = 0;

  // erorr index
  data[OFFS_ERID + 0] = ASN_INTEGER;
  data[OFFS_ERID + 1] = 1;
  data[OFFS_ERID + 2] = 0;

  // sequence 1
  data[OFFS_SEQ1 + 0] = ASN_SEQUENCE;
  data[OFFS_SEQ1 + 1] = TOTL_SIZE - OFFS_SEQ2;

  // sequence 2
  data[OFFS_SEQ2 + 0] = ASN_SEQUENCE;
  data[OFFS_SEQ2 + 1] = TOTL_SIZE - OFFS_OBID;

  // OID
  data[OFFS_OBID + 0] = ASN_OBJECT_IDENTIFIER;
  data[OFFS_OBID + 1] = oidlen;
  memcpy(&data[OFFS_OBID + 2], oid, oidlen);

  data[OFFS_DATA + 0] = dataType;
  data[OFFS_DATA + 1] = payloadSize;
  memcpy(&data[OFFS_DATA + 2], payloadData, payloadSize);

  for (unsigned int i = 0; i < TOTL_SIZE;i++)
  {
    if ((i % 8) == 0)
      writeDebugStreamLine("");

    writeDebugStream("0x%02x ", data[i] & 0xFF);
  }
  return TOTL_SIZE;
}


int genSNMPreply(long reqNo, string &communityName, tByteArray &oid, ubyte oidlen, string &dataString)
{
  int len = strlen(dataString);
  memcpy(payloadData, dataString, len);
  return genSNMPreply(reqNo, communityName, oid, oidlen, (ubyte)ASN_OCTET_STRING, payloadData, len);
}

int genSNMPreply(long reqNo, string &communityName, tByteArray &oid, ubyte oidlen, long value)
{
  payloadData[0] = (value >> 24) & 0xFF;
  payloadData[1] = (value >> 16) & 0xFF;
  payloadData[2] = (value >>  8) & 0xFF;
  payloadData[3] = (value >>  0) & 0xFF;
  //memcpy(payloadData, value, len);
  return genSNMPreply(reqNo, communityName, oid, oidlen, (ubyte)ASN_INTEGER, payloadData, 4);
}

bool SNMPdecode (long &reqID, tByteArray &oid)
{
  ubyte length = RS485rxbuffer[1];
  ubyte snmpversion = RS485rxbuffer[4] + 1;
  string community;
  ubyte communityLength = RS485rxbuffer[6];
  memcpy(community, &RS485rxbuffer[7], communityLength);

  ubyte reqTypeOffset = 7 + communityLength;
  ubyte reqType = RS485rxbuffer[reqTypeOffset];

  ubyte reqIDOffset = reqTypeOffset + 4;
  reqID = ((long)(RS485rxbuffer[reqIDOffset + 0] & 0xFF) << 24) +
               ((long)(RS485rxbuffer[reqIDOffset + 1] & 0xFF) << 16) +
               ((long)(RS485rxbuffer[reqIDOffset + 2] & 0xFF) <<  8) +
               ((long)(RS485rxbuffer[reqIDOffset + 3] & 0xFF) <<  0);

  ubyte error = RS485rxbuffer[reqIDOffset + 6];
  ubyte errorIdx = RS485rxbuffer[reqIDOffset + 9];

  ubyte oidOffset = reqIDOffset + 15;
  ubyte oidSize = RS485rxbuffer[oidOffset];
  //ubyte oid[9];
  memcpy(oid, &RS485rxbuffer[oidOffset + 1], oidSize);
  ubyte valueOffset = oidOffset + oidSize;
  ubyte valueType = RS485rxbuffer[valueOffset];
  ubyte valueSize = RS485rxbuffer[valueOffset + 1];
  string valueString;

  switch(valueType)
  {
    case ASN_INTEGER: break;
    case ASN_OCTET_STRING:
      memcpy(valueString, &RS485rxbuffer[valueOffset + 2], valueSize);
      break;
    case ASN_NULL: break;
  }

  writeDebugStreamLine("Length: %d", length);
  writeDebugStreamLine("SNMP version: %d", snmpversion);
  writeDebugStreamLine("community: %s", community);
  writeDebugStreamLine("Req Type: 0x%X", reqType);
  writeDebugStreamLine("Reqid: 0x%8X (%d)", reqID, reqID);
	writeDebugStreamLine("error: 0x%X", error);
	writeDebugStreamLine("errorIdx: 0x%X", errorIdx);
	writeDebugStreamLine("oidSize: %d", oidSize);
	if (oidSize > 9)
	  return false;
	writeDebugStream("OID: ");
	for (int i = 0; i < (oidSize - 1); i++)
	{
	  writeDebugStream("%d.", oid[i]);
	}
	writeDebugStreamLine("%d", oid[oidSize - 1]);
	writeDebugStreamLine("valueType: 0x%X", valueType);
	writeDebugStreamLine("valueSize: 0x%X", valueSize);
	if (valueType == ASN_OCTET_STRING)
	  writeDebugStreamLine("S: %s", valueString);

	return true;
}


task main ()
{
  StartTask(pollSensors);
  StartTask(updateScreen);
  string communityName = "public";
  string dataString;
  getFriendlyName(dataString);

  static tByteArray oid;
  const byte oidName[] = {43, 6, 1, 2, 1, 1, 5, 0};
  const byte oidTree[] = {43, 6, 1, 3};

  const ubyte oidTemp = 1;
  const ubyte oidPress = 2;
  const ubyte oidSound = 3;
  const ubyte oidLight = 4;

  long sensorData;

  int avail = 0;
  int totsize = 0;

  nNxtButtonTask = -2;

  // initialise the port, etc
  RS485initLib();

  memset(RS485rxbuffer, 0, sizeof(RS485rxbuffer));
  memset(RS485txbuffer, 0, sizeof(RS485txbuffer));

  // Disconnect if already connected
  N2WDisconnect();
  N2WchillOut();

  // if a custom profile exists, use that instead
    if (!N2WCustomExist())
  {
    StopTask(updateScreen);
    wait1Msec(50);
    eraseDisplay();
    PlaySound(soundException);
    nxtDisplayCenteredBigTextLine(1, "ERROR");
    nxtDisplayTextLine(3, "No custom profile");
    nxtDisplayTextLine(4, "configured!!");
    while(true) EndTimeSlice();
  }

  N2WLoad();

  wait1Msec(100);
  N2WConnect(true);
  connStatus = "connecting";

  while (!N2WConnected())
    wait1Msec(1000);

  connStatus = "connected";
  PlaySound(soundBeepBeep);

  wait1Msec(3000);
  N2WgetIP(IPaddress);

  wait1Msec(1000);
  if (!N2WUDPOpenServer(1, 161))
    writeDebugStreamLine("Err open port");
  while (true)
  {
    avail = N2WUDPAvail(1);
    if (avail > 0)
    {
      rxbytes += avail;
      N2WchillOut();
      N2WUDPRead(1, avail);
      if (SNMPdecode(reqID, oid))
      {
				writeDebugStreamLine("------------------");
				for (int i = 0; i < sizeof(oid); i++)
				{
				  writeDebugStream("0x%02x ", oid[i] & 0xFF);
				}
				writeDebugStreamLine("");
				for (int i = 0; i < sizeof(oidTree); i++)
				{
				  writeDebugStream("0x%02x ", oidTree[i] & 0xFF);
				}
				writeDebugStreamLine("\n------------------");
	      if (memcmp(oid, oidName, 8) == 0)
	      {
	        getFriendlyName(dataString);
		      totsize = genSNMPreply(reqID, communityName, oid, 8, dataString);
		      N2WUDPWrite(1, data, totsize);
		      txbytes += totsize;
		    }
		    else if (memcmp(oid, oidTree, 4) == 0)
		    {
		      dataString = "unknown";

		      switch(oid[4])
		      {
		        case oidTemp:  sensorData = dataTemp; break;
		        case oidPress: sensorData = dataPress; break;
		        case oidSound: sensorData = dataSound; break;
		        case oidLight: sensorData = dataLight; break;
		      }
		      totsize = genSNMPreply(reqID, communityName, oid, 5, sensorData);
		      N2WUDPWrite(1, data, totsize);
		      txbytes += totsize;
		    }
		    else
	      {
	        writeDebugStreamLine("unknown oid");
	      }
	    }
	    else
	    {
	      writeDebugStreamLine("Invalid packet");
	    }
	  }
    wait1Msec(100);
  }
}