time_t getNtpTime()
{
while (Udp.parsePacket() > 0) ; // discard any previously received packets
Serial.println("Transmit NTP Request");
sendNTPpacket(ntp_server);
uint32_t beginWait = millis();
while (millis() - beginWait < 2000) {
int size = Udp.parsePacket();
if (size >= NTP_PACKET_SIZE) {
Serial.println("Receive NTP Response");
Udp.read(packetBuffer, NTP_PACKET_SIZE); // read packet into the buffer
unsigned long secsSince1900;
unsigned long fracSecs;
// convert four bytes starting at location 40 to a long integer
secsSince1900 = (unsigned long)packetBuffer[40] << 24;
secsSince1900 |= (unsigned long)packetBuffer[41] << 16;
secsSince1900 |= (unsigned long)packetBuffer[42] << 8;
secsSince1900 |= (unsigned long)packetBuffer[43];
fracSecs = (unsigned long)packetBuffer[44] << 24;
fracSecs |= (unsigned long)packetBuffer[45] << 16;
fracSecs |= (unsigned long)packetBuffer[46] << 8;
fracSecs |= (unsigned long)packetBuffer[47];
return ((time_t)(secsSince1900 - 2208988800UL)<<32) + fracSecs;
}
}
Serial.println("No NTP Response :-(");
return 0; // return 0 if unable to get the time
}
int readUdp( char * buf, int size )
{
if ( ! active ) return 0 ;
int count = Udp.parsePacket() ;
if ( count ) Udp.read( buf, size ) ;
return count ;
}
void loop() {
// if there's data available, read a packet
int packetSize = Udp.parsePacket();
char Size[4];
if(packetSize)
{
if(packetSize == 4)
{
IPAddress remote = Udp.remoteIP();
// read the packet into packetBufffer
Udp.read(packetBuffer,UDP_TX_PACKET_MAX_SIZE);
char Command[4];
for(int nCom = 0; nCom < 4; nCom++)
{
Command[nCom] = packetBuffer[nCom];
}
if (Command[0] == 't')
{
digitalWrite(13, HIGH);
}
else if (Command[0] == 'r')
{
digitalWrite(12, HIGH);
}
else if (Command[0] == 'e')
{
digitalWrite(10, HIGH);
}
else if (Command[0] == 'w')
{
analogWrite(11, 65);
}
else
{
digitalWrite(13, LOW);
digitalWrite(12, LOW);
analogWrite(11, 0);
}
// send a reply, to the IP address and port that sent us the packet we received
itoa(packetSize, Size, 10);
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
Udp.write(ReplyBuffer);
Udp.write(packetBuffer);
Udp.write(Size);
Udp.endPacket();
delay(1000);
}
else
{
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
Udp.write("NOPE");
Udp.endPacket();
}
}
}
// This function's purpose is to receive data and prepare it for parsing
void RobotOpenClass::handleData() {
_packetBufferSize = Udp.parsePacket();
// If there's data, read the packet in
if (_packetBufferSize > 0) {
_remotePort = Udp.remotePort();
_remoteIp = Udp.remoteIP();
Udp.read(_packetBufferAccessor, 256);
parsePacket(); // Data is all set, time to parse through it
}
}
/*
* Send an NTP request to get the current time.
*
* This code was lifted from sample code that was part of an
* arduino ntp libary.
*/
time_t parseNtpPacket() {
Udp.read(packetBuffer,NTP_PACKET_SIZE); // read the packet into the buffer
unsigned long highWord = word(packetBuffer[40], packetBuffer[41]);
unsigned long lowWord = word(packetBuffer[42], packetBuffer[43]);
unsigned long ntpTime = highWord << 16 | lowWord;
return (ntpTime - 2208988800UL); // NTP time is based on 01-Jan-1900, but
// we want Unix time which is seconds
// since 01-Jan-1970
}
void udpToUart()
{
int packetSize = Udp.parsePacket();
if(packetSize)
{
// read the packet into packetBufffer
Udp.read(udpBuffer,MAX_HDLC_FRAME_SIZE);
Serial.write(HDLC_SS_BYTE);
Serial.write(udpBuffer, packetSize);
Serial.write(0x12);
Serial.write(0x34);
Serial.write(HDLC_SS_BYTE);
}
}
roveEthernet_Error roveEthernet_GetUdpMsg(roveIP* senderIP, void* buffer, size_t bufferSize)
{
int packetSize = udpReceiver.parsePacket();
if (packetSize > 0) //if there is a packet available
{
udpReceiver.read((char*)buffer, bufferSize);
*senderIP = udpReceiver.remoteIP();
return ROVE_ETHERNET_ERROR_SUCCESS;
}
else
{
return ROVE_ETHERNET_ERROR_WOULD_BLOCK;
}
}
void UdpController::Loop()
{
unsigned long currentMillis = millis();
int packetSize = Udp.parsePacket();
if (packetSize && packetSize == 3)
{
Udp.read(Buffer, UDP_TX_PACKET_MAX_SIZE);
if (Buffer[0] == 0x28 && Buffer[1] == 0x06)
{
_traffic->SetMode((LightMode)Buffer[2]);
_lastUpdateMillis = currentMillis;
}
}
if ((currentMillis - _lastUpdateMillis) > UpdateIntervalMillis)
{
_traffic->ShowInconclusive();
}
}
unsigned long getNtpTime()
{
sendNTPpacket(timeServer); // send an NTP packet to a time server
// wait to see if a reply is available
delay(1000);
if ( Udp.parsePacket() ) {
// We've received a packet, read the data from it
Udp.read(packetBuffer,NTP_PACKET_SIZE); // read the packet into the buffer
//the timestamp starts at byte 40 of the received packet and is four bytes,
// or two words, long. First, esxtract the two words:
unsigned long highWord = word(packetBuffer[40], packetBuffer[41]);
unsigned long lowWord = word(packetBuffer[42], packetBuffer[43]);
// combine the four bytes (two words) into a long integer
// this is NTP time (seconds since Jan 1 1900):
unsigned long secsSince1900 = highWord << 16 | lowWord;
#ifdef LOGGING
Serial.print("Seconds since Jan 1 1900 = " );
Serial.println(secsSince1900);
#endif
// now convert NTP time into everyday time:
// Unix time starts on Jan 1 1970. In seconds, that's 2208988800:
const unsigned long seventyYears = 2208988800UL;
// subtract seventy years:
unsigned long epoch = secsSince1900 - seventyYears;
// print Unix time:
#ifdef LOGGING
Serial.print(UNIX_TIME);
Serial.println(epoch);
#endif
return epoch;
}
return 0; // return 0 if unable to get the time
}
void commandInterfaceTick() {
int packetSize = cmdsock.parsePacket();
if(cmdsock.available()) {
// read the packet into packetBufffer
cmdsock.read(udpPacketBuffer, PACKET_SIZE);
if(memcmp("INGV\0", udpPacketBuffer, 5) != 0) {
return;
}
bool reboot = false;
unsigned long unixTimeM = getUNIXTime();
unsigned long uptime = getUNIXTime() - getBootTime();
byte macaddress[6] = { 0 };
getMACAddress(macaddress);
uint32_t probeSpeed = getProbeSpeedStatistic();
uint32_t freeramkb = freeMemory();
float latency = 0;
if(udpPacketBuffer[5] == PKTTYPE_GETINFO) {
latency = tcpLatency();
}
float longitude = 0;
float latitude = 0;
switch(udpPacketBuffer[5]) {
case PKTTYPE_DISCOVERY:
// Reply to discovery
udpPacketBuffer[5] = PKTTYPE_DISCOVERY_REPLY;
memcpy(udpPacketBuffer + 6, macaddress, 6);
memcpy(udpPacketBuffer + 12, getVersionAsString().c_str(), 4);
memcpy(udpPacketBuffer + 16, "uno", 3);
break;
case PKYTYPE_PING:
// Reply to ping
udpPacketBuffer[5] = PKYTYPE_PONG;
break;
case PKTTYPE_SENDGPS:
// Get coords
udpPacketBuffer[5] = PKTTYPE_OK;
memcpy(&latitude, udpPacketBuffer + 12, 4);
memcpy(&longitude, udpPacketBuffer + 16, 4);
reverse4bytes((byte*)&latitude);
reverse4bytes((byte*)&longitude);
break;
case PKTTYPE_REBOOT:
// Reboot
// Reply with OK
udpPacketBuffer[5] = PKTTYPE_OK;
reboot = true;
break;
case PKTTYPE_GETINFO:
udpPacketBuffer[5] = PKTTYPE_GETINFO_REPLY;
memcpy(udpPacketBuffer + 6, macaddress, 6);
memcpy(udpPacketBuffer + 28, &uptime, 4);
memcpy(udpPacketBuffer + 32, &unixTimeM, 4);
memcpy(udpPacketBuffer + 36, VERSION, 4);
memcpy(udpPacketBuffer + 40, &freeramkb, 4);
memcpy(udpPacketBuffer + 44, &latency, 4);
memcpy(udpPacketBuffer + 53, "uno", 3);
memcpy(udpPacketBuffer + 57, "MMA7361", 7);
memcpy(udpPacketBuffer + 65, &probeSpeed, 4);
break;
#ifdef RESET_ENABLED
case PKTTYPE_RESET:
initEEPROM();
reboot = true;
break;
#endif
default:
// Unknown packet or invalid command
return;
}
if(longitude != 0 && latitude != 0) {
setLongitude(longitude);
setLatitude(latitude);
}
cmdsock.beginPacket(cmdsock.remoteIP(), cmdsock.remotePort());
cmdsock.write(udpPacketBuffer, PACKET_SIZE);
cmdsock.endPacket();
cmdsock.flush();
if(reboot) {
soft_restart();
}
}
}
uint64_t getNTPtime()
{
// send an NTP packet to a time server
sendNTPpacket(timeServer);
// wait to see if a reply is available
delay(1000);
if ( Udp.available() )
{
// read the packet into the buffer
Udp.read(pb, NTP_PACKET_SIZE); // New from IDE 1.0,
// NTP contains four timestamps with an integer part and a fraction part
// we only use the integer part here
unsigned long t1, t2, t3, t4;
t1 = t2 = t3 = t4 = 0;
for (int i=0; i< 4; i++)
{
t1 = t1 << 8 | pb[16+i];
t2 = t2 << 8 | pb[24+i];
t3 = t3 << 8 | pb[32+i];
t4 = t4 << 8 | pb[40+i];
}
// part of the fractional part
// could be 4 bytes but this is more precise than the 1307 RTC
// which has a precision of ONE second
// in fact one byte is sufficient for 1307
float f1,f2,f3,f4;
f1 = ((long)pb[20] * 256 + pb[21]) / 65536.0;
f2 = ((long)pb[28] * 256 + pb[29]) / 65536.0;
f3 = ((long)pb[36] * 256 + pb[37]) / 65536.0;
f4 = ((long)pb[44] * 256 + pb[45]) / 65536.0;
// NOTE:
// one could use the fractional part to set the RTC more precise
// 1) at the right (calculated) moment to the NEXT second!
// t4++;
// delay(1000 - f4*1000);
// RTC.adjust(DateTime(t4));
// keep in mind that the time in the packet was the time at
// the NTP server at sending time so one should take into account
// the network latency (try ping!) and the processing of the data
// ==> delay (850 - f4*1000);
// 2) simply use it to round up the second
// f > 0.5 => add 1 to the second before adjusting the RTC
// (or lower threshold eg 0.4 if one keeps network latency etc in mind)
// 3) a SW RTC might be more precise, => ardomic clock :)
// convert NTP to UNIX time, differs seventy years = 2208988800 seconds
// NTP starts Jan 1, 1900
// Unix time starts on Jan 1 1970.
const unsigned long seventyYears = 2208988800UL;
t1 -= seventyYears;
t2 -= seventyYears;
t3 -= seventyYears;
t4 -= seventyYears;
//Serial.println("T1 .. T4 && fractional parts");
//PrintDateTime(DateTime(t1)); Serial.println(f1,4);
//PrintDateTime(DateTime(t2)); Serial.println(f2,4);
//PrintDateTime(DateTime(t3)); Serial.println(f3,4);
//PrintDateTime(t4);
char ntpTime[21];
//GetDatetimeString(t4, ntpTime);
Serial.println(f4,4);
Serial.println();
// Adjust timezone and DST... in my case substract 4 hours for Chile Time
// or work in UTC?
t4 -= (3 * 3600L); // Notice the L for long calculations!!
t4 += 1; // adjust the delay(1000) at begin of loop!
if (f4 > 0.4) t4++; // adjust fractional part, see above
}
else
{
Serial.println("Failed to get NTP UDP packet!");
}
}
bool gatewayTransportSend(MyMessage &message)
{
bool ret = true;
char *_ethernetMsg = protocolFormat(message);
setIndication(INDICATION_GW_TX);
_w5100_spi_en(true);
#if defined(MY_CONTROLLER_IP_ADDRESS)
#if defined(MY_USE_UDP)
_ethernetServer.beginPacket(_ethernetControllerIP, MY_PORT);
_ethernetServer.write(_ethernetMsg, strlen(_ethernetMsg));
// returns 1 if the packet was sent successfully
ret = _ethernetServer.endPacket();
#else
EthernetClient client;
#if defined(MY_CONTROLLER_URL_ADDRESS)
if (client.connected() || client.connect(MY_CONTROLLER_URL_ADDRESS, MY_PORT)) {
#else
if (client.connected() || client.connect(_ethernetControllerIP, MY_PORT)) {
#endif
client.write(_ethernetMsg, strlen(_ethernetMsg));
}
else {
// connecting to the server failed!
ret = false;
}
#endif
#else
// Send message to connected clients
#if defined(MY_GATEWAY_ESP8266)
for (uint8_t i = 0; i < ARRAY_SIZE(clients); i++)
{
if (clients[i] && clients[i].connected())
{
clients[i].write((uint8_t*)_ethernetMsg, strlen(_ethernetMsg));
}
}
#else
_ethernetServer.write(_ethernetMsg);
#endif
#endif
_w5100_spi_en(false);
return ret;
}
#if defined(MY_GATEWAY_ESP8266)
bool _readFromClient(uint8_t i) {
while (clients[i].connected() && clients[i].available()) {
char inChar = clients[i].read();
if (inputString[i].idx < MY_GATEWAY_MAX_RECEIVE_LENGTH - 1) {
// if newline then command is complete
if (inChar == '\n' || inChar == '\r') {
// Add string terminator and prepare for the next message
inputString[i].string[inputString[i].idx] = 0;
debug(PSTR("Client %d: %s\n"), i, inputString[i].string);
inputString[i].idx = 0;
if (protocolParse(_ethernetMsg, inputString[i].string)) {
return true;
}
} else {
// add it to the inputString:
inputString[i].string[inputString[i].idx++] = inChar;
}
} else {
// Incoming message too long. Throw away
debug(PSTR("Client %d: Message too long\n"), i);
inputString[i].idx = 0;
// Finished with this client's message. Next loop() we'll see if there's more to read.
break;
}
}
return false;
}
#else
bool _readFromClient() {
while (client.connected() && client.available()) {
char inChar = client.read();
if (inputString.idx < MY_GATEWAY_MAX_RECEIVE_LENGTH - 1) {
// if newline then command is complete
if (inChar == '\n' || inChar == '\r') {
// Add string terminator and prepare for the next message
inputString.string[inputString.idx] = 0;
debug(PSTR("Eth: %s\n"), inputString.string);
inputString.idx = 0;
if (protocolParse(_ethernetMsg, inputString.string)) {
return true;
}
} else {
// add it to the inputString:
inputString.string[inputString.idx++] = inChar;
}
} else {
// Incoming message too long. Throw away
debug(PSTR("Eth: Message too long\n"));
inputString.idx = 0;
// Finished with this client's message. Next loop() we'll see if there's more to read.
break;
}
}
return false;
}
#endif
bool gatewayTransportAvailable()
{
_w5100_spi_en(true);
#if !defined(MY_IP_ADDRESS) && defined(MY_GATEWAY_W5100)
// renew IP address using DHCP
gatewayTransportRenewIP();
#endif
#ifdef MY_USE_UDP
int packet_size = _ethernetServer.parsePacket();
if (packet_size) {
//debug(PSTR("UDP packet available. Size:%d\n"), packet_size);
setIndication(INDICATION_GW_RX);
#if defined(MY_GATEWAY_ESP8266)
_ethernetServer.read(inputString[0].string, MY_GATEWAY_MAX_RECEIVE_LENGTH);
inputString[0].string[packet_size] = 0;
debug(PSTR("UDP packet received: %s\n"), inputString[0].string);
return protocolParse(_ethernetMsg, inputString[0].string);
#else
_ethernetServer.read(inputString.string, MY_GATEWAY_MAX_RECEIVE_LENGTH);
inputString.string[packet_size] = 0;
debug(PSTR("UDP packet received: %s\n"), inputString.string);
_w5100_spi_en(false);
return protocolParse(_ethernetMsg, inputString.string);
#endif
}
#else
#if defined(MY_GATEWAY_ESP8266)
// ESP8266: Go over list of clients and stop any that are no longer connected.
// If the server has a new client connection it will be assigned to a free slot.
bool allSlotsOccupied = true;
for (uint8_t i = 0; i < ARRAY_SIZE(clients); i++) {
if (!clients[i].connected()) {
if (clientsConnected[i]) {
debug(PSTR("Client %d disconnected\n"), i);
clients[i].stop();
}
//check if there are any new clients
if (_ethernetServer.hasClient()) {
clients[i] = _ethernetServer.available();
inputString[i].idx = 0;
debug(PSTR("Client %d connected\n"), i);
gatewayTransportSend(buildGw(_msg, I_GATEWAY_READY).set("Gateway startup complete."));
if (presentation)
presentation();
}
}
bool connected = clients[i].connected();
clientsConnected[i] = connected;
allSlotsOccupied &= connected;
}
if (allSlotsOccupied && _ethernetServer.hasClient()) {
//no free/disconnected spot so reject
debug(PSTR("No free slot available\n"));
EthernetClient c = _ethernetServer.available();
c.stop();
}
// Loop over clients connect and read available data
for (uint8_t i = 0; i < ARRAY_SIZE(clients); i++) {
if (_readFromClient(i)) {
setIndication(INDICATION_GW_RX);
_w5100_spi_en(false);
return true;
}
}
#else
// W5100/ENC module does not have hasClient-method. We can only serve one client at the time.
EthernetClient newclient = _ethernetServer.available();
// if a new client connects make sure to dispose any previous existing sockets
if (newclient) {
if (client != newclient) {
client.stop();
client = newclient;
debug(PSTR("Eth: connect\n"));
_w5100_spi_en(false);
gatewayTransportSend(buildGw(_msg, I_GATEWAY_READY).set("Gateway startup complete."));
_w5100_spi_en(true);
if (presentation)
presentation();
}
}
if (client) {
if (!client.connected()) {
debug(PSTR("Eth: disconnect\n"));
client.stop();
} else {
if (_readFromClient()) {
setIndication(INDICATION_GW_RX);
_w5100_spi_en(false);
return true;
}
}
}
#endif
#endif
_w5100_spi_en(false);
return false;
}
SNMP_API_STAT_CODES AgentuinoClass::requestPdu(SNMP_PDU *pdu)
{
char *community;
// sequence length
byte seqLen;
// version
byte verLen, verEnd;
// community string
byte comLen, comEnd;
// pdu
byte pduTyp, pduLen;
byte ridLen, ridEnd;
byte errLen, errEnd;
byte eriLen, eriEnd;
byte vblTyp, vblLen;
byte vbiTyp, vbiLen;
byte obiLen, obiEnd;
byte valTyp, valLen, valEnd;
byte i;
//
// set packet packet size (skip UDP header)
_packetSize = Udp.available();
//
// reset packet array
memset(_packet, 0, SNMP_MAX_PACKET_LEN);
//
// validate packet
if ( _packetSize != 0 && _packetSize > SNMP_MAX_PACKET_LEN ) {
//
//SNMP_FREE(_packet);
return SNMP_API_STAT_PACKET_TOO_BIG;
}
//
// get UDP packet
//Udp.parsePacket();
Udp.read(_packet, _packetSize);
// Udp.readPacket(_packet, _packetSize, _dstIp, &_dstPort);
//
// packet check 1
if ( _packet[0] != 0x30 ) {
//
//SNMP_FREE(_packet);
return SNMP_API_STAT_PACKET_INVALID;
}
//
// sequence length
seqLen = _packet[1];
// version
verLen = _packet[3];
verEnd = 3 + verLen;
// community string
comLen = _packet[verEnd + 2];
comEnd = verEnd + 2 + comLen;
// pdu
pduTyp = _packet[comEnd + 1];
pduLen = _packet[comEnd + 2];
ridLen = _packet[comEnd + 4];
ridEnd = comEnd + 4 + ridLen;
errLen = _packet[ridEnd + 2];
errEnd = ridEnd + 2 + errLen;
eriLen = _packet[errEnd + 2];
eriEnd = errEnd + 2 + eriLen;
vblTyp = _packet[eriEnd + 1];
vblLen = _packet[eriEnd + 2];
vbiTyp = _packet[eriEnd + 3];
vbiLen = _packet[eriEnd + 4];
obiLen = _packet[eriEnd + 6];
obiEnd = eriEnd + obiLen + 6;
valTyp = _packet[obiEnd + 1];
valLen = _packet[obiEnd + 2];
valEnd = obiEnd + 2 + valLen;
//
// extract version
pdu->version = 0;
for ( i = 0; i < verLen; i++ ) {
pdu->version = (pdu->version << 8) | _packet[5 + i];
}
//
// validate version
//
// pdu-type
pdu->type = (SNMP_PDU_TYPES)pduTyp;
_dstType = pdu->type;
//
// validate community size
if ( comLen > SNMP_MAX_NAME_LEN ) {
// set pdu error
pdu->error = SNMP_ERR_TOO_BIG;
//
return SNMP_API_STAT_NAME_TOO_BIG;
}
//
//
// validate community name
if ( pdu->type == SNMP_PDU_SET && comLen == _setSize ) {
//
for ( i = 0; i < _setSize; i++ ) {
if( _packet[verEnd + 3 + i] != (byte)_setCommName[i] ) {
// set pdu error
pdu->error = SNMP_ERR_NO_SUCH_NAME;
//
return SNMP_API_STAT_NO_SUCH_NAME;
}
}
} else if ( pdu->type == SNMP_PDU_GET && comLen == _getSize ) {
//
for ( i = 0; i < _getSize; i++ ) {
if( _packet[verEnd + 3 + i] != (byte)_getCommName[i] ) {
// set pdu error
pdu->error = SNMP_ERR_NO_SUCH_NAME;
//
return SNMP_API_STAT_NO_SUCH_NAME;
}
}
} else if ( pdu->type == SNMP_PDU_GET_NEXT && comLen == _getSize ) {
//
for ( i = 0; i < _getSize; i++ ) {
if( _packet[verEnd + 3 + i] != (byte)_getCommName[i] ) {
// set pdu error
pdu->error = SNMP_ERR_NO_SUCH_NAME;
//
return SNMP_API_STAT_NO_SUCH_NAME;
}
}
} else {
// set pdu error
pdu->error = SNMP_ERR_NO_SUCH_NAME;
//
return SNMP_API_STAT_NO_SUCH_NAME;
}
//
//
// extract reqiest-id 0x00 0x00 0x00 0x01 (4-byte int aka int32)
pdu->requestId = 0;
for ( i = 0; i < ridLen; i++ ) {
pdu->requestId = (pdu->requestId << 8) | _packet[comEnd + 5 + i];
}
//
// extract error
pdu->error = SNMP_ERR_NO_ERROR;
int32_t err = 0;
for ( i = 0; i < errLen; i++ ) {
err = (err << 8) | _packet[ridEnd + 3 + i];
}
pdu->error = (SNMP_ERR_CODES)err;
//
// extract error-index
pdu->errorIndex = 0;
for ( i = 0; i < eriLen; i++ ) {
pdu->errorIndex = (pdu->errorIndex << 8) | _packet[errEnd + 3 + i];
}
//
//
// validate object-identifier size
if ( obiLen > SNMP_MAX_OID_LEN ) {
// set pdu error
pdu->error = SNMP_ERR_TOO_BIG;
return SNMP_API_STAT_OID_TOO_BIG;
}
//
// extract and contruct object-identifier
memset(pdu->OID.data, 0, SNMP_MAX_OID_LEN);
pdu->OID.size = obiLen;
for ( i = 0; i < obiLen; i++ ) {
pdu->OID.data[i] = _packet[eriEnd + 7 + i];
}
//
// value-type
pdu->VALUE.syntax = (SNMP_SYNTAXES)valTyp;
//
// validate value size
if ( obiLen > SNMP_MAX_VALUE_LEN ) {
// set pdu error
pdu->error = SNMP_ERR_TOO_BIG;
return SNMP_API_STAT_VALUE_TOO_BIG;
}
//
// value-size
pdu->VALUE.size = valLen;
//
// extract value
memset(pdu->VALUE.data, 0, SNMP_MAX_VALUE_LEN);
for ( i = 0; i < valLen; i++ ) {
pdu->VALUE.data[i] = _packet[obiEnd + 3 + i];
}
//
return SNMP_API_STAT_SUCCESS;
}
void loop() {
if ((unsigned long) (millis() - previousEth) >= interval_ethernet) { //entra a cada 50ms
previousEth = millis();
if(count_eth == 0) { //espera completar um ciclo de todas as funções chamadas
int packetSize = Udp.parsePacket();
count_eth = 2;
Serial.println("entrou eth");
received = 0;
if (packetSize) {
set_debug(PC3);
#ifdef debug
Serial.println("entrou eth RX ");
Serial.print("Received packet of size ");
Serial.println(packetSize);
Serial.print("From ");
IPAddress remote = Udp.remoteIP();
for (int i =0; i < 4; i++)
{
Serial.print(remote[i], DEC);
if (i < 3)
{
Serial.print(".");
}
}
Serial.print(", port ");
Serial.println(Udp.remotePort());
Serial.println("Contents:");
Serial.println(packetBuffer);
#endif
received = Udp.read();
charBuffer.put(received);
#ifdef debug
Serial.println("\rchar recebido:");
Serial.write(received);
#endif
/////////////////////////////
clear_debug(PC3);
}
} else {
count_eth--;
Serial.println("entrou eth NADA");
if (charBuffer.getSize() > 5) charBuffer.clear();
if( charBuffer.getSize() > 0 )
received = charBuffer.get();
Serial.println("received:");
Serial.println(received);
}
}
#ifdef debug
Serial.println("client disconnected");
#endif
/****************FIM ROTINA ETHERNET ***************************************************/
if ((unsigned long) (millis() - previousADC) >= interval_adc) { //entra a cada 60ms
previousADC = millis();
set_debug(PC4);
if (count_adc == 0) {
count_adc = 1;
/*testa obstaculo IR*/
Serial.println("entrou adc OBSTACULO ");
IR_obstaculo = 0;
IR_obstaculo = verificaObstaculoIR();
#ifdef debug
Serial.println("distancia_ir");
Serial.println(IR_obstaculo);
Serial.println("\r");
#endif
if (IR_obstaculo > IR_OBSTACLE_THRESHOLD)
obstacle_flag = 0;
else if (IR_obstaculo > IR_OBSTACLE_THRESHOLD
&& IR_obstaculo < IR_OBSTACLE_UPPER_THRESHOLD)
obstacle_flag = 1;
else if (IR_obstaculo <= IR_OBSTACLE_THRESHOLD)
obstacle_flag = 2;
clear_debug(PC4);
} else {
count_adc=0;
Serial.println("entrou adc NADA ");
}
}
/***************** FIM ROTINA ADC********************************/
if ((unsigned long) (millis() - previousMotores) >= interval_motores) { //entra a cada 100ms
previousMotores = millis();
if (ciclosClock_motor == 2) { //duas bordas de subida depois de acionar o motor (200ms depois de acionar o motor)
Serial.println("motor stop ");
stopMove();
ciclosClock_motor = 0;
}
if (count_motor == 0) {
Serial.println("motor ANDA ");
if (obstacle_flag == 0 || obstacle_flag == 1) {
count_motor = 1;
/*move motores*/
switch (received) {
case 'u':
goForward(obstacle_flag);
break;
case 'd':
goBack(obstacle_flag);
break;
case 'l':
goLeft(obstacle_flag);
break;
case 'r':
goRight(obstacle_flag);
break;
default:
received = 0;
stopMove();
break;
}
} else {
if (received == 'd') {
goBack(WITH_CARE);
} else {
stopMove();
}
}
} else {
ciclosClock_motor++;
count_motor = 0;
Serial.println("ciclosClock_motor++ ");
}
}
// _delay_ms(500);
}
/**
* Parses incoming SNMP messages.
*
* Original Auther: Rex Park
* Updated: November 7, 2015 (Added support for Inform responses (SNMP_PDU_RESPONSE)
*/
SNMP_API_STAT_CODES SNMPClass::requestPdu(SNMP_PDU *pdu, char *extra_data, int extra_data_max_size)
{
// sequence length
uint16_t seqLen, valLen, vblLen, pduLen;
// version
byte verLen, verEnd;
// community string
byte comLen, comEnd;
// pdu
byte pduTyp, pduEnd;
byte ridLen, ridEnd;
byte errLen, errEnd;
byte eriLen, eriEnd;
byte vblTyp;
byte vbiTyp, vbiLen;
byte obiLen, obiEnd;
byte valTyp, valEnd;
int i;
// set packet packet size (skip UDP header)
_packetSize = Udp.available();
_packetPos = 0;
// reset packet array
memset(_packet, 0, SNMP_MAX_PACKET_LEN);
//
// validate packet
if ( _packetSize <= 0) {
return SNMP_API_STAT_PACKET_TOO_BIG;
}
// get UDP packet
if(_udp_extra_data_packet == true){
if(extra_data != NULL){
memset(extra_data, 0, extra_data_max_size);
}
Udp.read(_packet, _packetSize - extra_data_max_size);
if(extra_data != NULL){
Udp.read((byte*)extra_data, extra_data_max_size);
}
}else{
Udp.read(_packet, _packetSize);
}
// Serial.println("Incomming: ");
// for(int i = 0; i < _packetSize; i++){
// Serial.print(_packet[i],HEX);
// Serial.print(" ");
// }
// Serial.println();
// packet check 1
if ( _packet[0] != 0x30 ) {
return SNMP_API_STAT_PACKET_INVALID;
}
// sequence length
if(_packet[1] >= 0x82){
seqLen = combine_msb_lsb(_packet[2], _packet[3]);
_packetPos = 4;
}
else if(_packet[1] == 0x81){
seqLen = _packet[2];
_packetPos = 3;
}else{
seqLen = _packet[1];
_packetPos = 2;
}
// version
if(_packet[_packetPos] != 0x02){
return SNMP_API_STAT_PACKET_INVALID;
}
verLen = _packet[_packetPos+1];//used to be hard coded as index 3
verEnd = _packetPos+1 + verLen;
// community string
comLen = _packet[verEnd + 2];
comEnd = verEnd + 2 + comLen;
// pdu
pduTyp = _packet[comEnd + 1];
if(_packet[comEnd + 2] >= 0x82){
pduLen = combine_msb_lsb(_packet[comEnd +3], _packet[comEnd +4]);
pduEnd = comEnd + 2 + pduLen + 2;
_packetPos = comEnd + 2 + 2;
}
else if(_packet[comEnd + 2] == 0x81){
pduLen = _packet[comEnd +3];
pduEnd = comEnd + 2 + pduLen + 1;
_packetPos = comEnd + 2 + 1;
}else{
pduLen = _packet[comEnd + 2];
pduEnd = comEnd + 2 + pduLen;
_packetPos = comEnd + 2;
}
//request id
ridLen = _packet[_packetPos + 2];
ridEnd = _packetPos + 2 + ridLen;
//error
errLen = _packet[ridEnd + 2];
errEnd = ridEnd + 2 + errLen;
//error index
eriLen = _packet[errEnd + 2];
eriEnd = errEnd + 2 + eriLen;
//variable bindings
vblTyp = _packet[eriEnd + 1];
if(_packet[eriEnd + 2] >= 0x82){
vblLen = combine_msb_lsb(_packet[eriEnd +3], _packet[eriEnd +4]);
_packetPos = eriEnd + 2 + 2;
}
else if(_packet[eriEnd + 2] == 0x81){
vblLen = _packet[eriEnd +3];
_packetPos = eriEnd + 2 + 1;
}else{
vblLen = _packet[eriEnd + 2];
_packetPos = eriEnd + 2;
}
//variable bindings id
vbiTyp = _packet[_packetPos + 1];
if(_packet[_packetPos + 2] > 0x80){
vbiLen = combine_msb_lsb(_packet[_packetPos +3], _packet[_packetPos +4]);
_packetPos = _packetPos + 2 + 2;
}else{
vbiLen = _packet[_packetPos + 2];
_packetPos = _packetPos + 2;
}
//object identifier
obiLen = _packet[_packetPos + 2];
obiEnd = _packetPos + 2 + obiLen;
//unknown
valTyp = _packet[obiEnd + 1];
if(_packet[obiEnd + 2] >= 0x82){
valLen = combine_msb_lsb(_packet[obiEnd +3], _packet[obiEnd +4]);
valEnd = obiEnd + 2 + valLen + 2;
}
else if(_packet[obiEnd + 2] == 0x81){
valLen = _packet[obiEnd + 3];
valEnd = obiEnd + 2 + valLen + 1;
}else{
valLen = _packet[obiEnd + 2];
valEnd = obiEnd + 2 + valLen;
}
// Serial.println(verEnd);
// Serial.println(comEnd);
// Serial.println(ridEnd);
// Serial.println(errEnd);
// Serial.println(eriEnd);
// Serial.println(obiEnd);
// Serial.println(pduEnd);
// Serial.println(pduTyp,HEX);
// extract version
pdu->version = _packet[verEnd];
// Serial.println(pdu->version,HEX);
// validate version
if(pdu->version != 0x0 && pdu->version != 0x1){
return SNMP_API_STAT_PACKET_INVALID;
}
// pdu-type
pdu->type = (SNMP_PDU_TYPES)pduTyp;
_dstType = pdu->type;
// validate community size
if ( comLen > SNMP_MAX_NAME_LEN ) {
// set pdu error
pdu->error = SNMP_ERR_TOO_BIG;
//
return SNMP_API_STAT_NAME_TOO_BIG;
}
// validate community name
if ( pdu->type == SNMP_PDU_SET && comLen == _setSize ) {
if(memcmp(_setCommName,_packet+verEnd+3,_setSize) != 0){
pdu->error = SNMP_ERR_NO_SUCH_NAME;
return SNMP_API_STAT_NO_SUCH_NAME;
}
} else if ( pdu->type == SNMP_PDU_GET) {
if(memcmp(_getCommName,_packet+verEnd+3,comLen) != 0 && memcmp(_setCommName,_packet+verEnd+3,comLen) != 0){
pdu->error = SNMP_ERR_NO_SUCH_NAME;
return SNMP_API_STAT_NO_SUCH_NAME;
}
} else if (pdu->type == SNMP_PDU_RESPONSE){
if(memcmp(_trapCommName,_packet+verEnd+3,comLen) != 0){
pdu->error = SNMP_ERR_NO_SUCH_NAME;
return SNMP_API_STAT_NO_SUCH_NAME;
}
}
else {
// set pdu error
pdu->error = SNMP_ERR_NO_SUCH_NAME;
//
return SNMP_API_STAT_NO_SUCH_NAME;
}
// extract reqiest-id 0x00 0x00 0x00 0x01 (4-byte int aka int32)
pdu->requestId = 0;
for ( i = 0; i < ridLen; i++ ) {
pdu->requestId = (pdu->requestId << 8) | _packet[ridEnd-ridLen+1 + i];
}
// extract error
pdu->error = SNMP_ERR_NO_ERROR;
int32_t err = 0;
for ( i = 0; i < errLen; i++ ) {
err = (err << 8) | _packet[errEnd-errLen+1 + i];
}
pdu->error = (SNMP_ERR_CODES)err;
// extract error-index
pdu->errorIndex = 0;
for ( i = 0; i < eriLen; i++ ) {
pdu->errorIndex = (pdu->errorIndex << 8) | _packet[eriEnd-eriLen+1 + i];
}
/**
* OID
*/
//validate length
if ( obiLen > SNMP_MAX_OID_LEN ) {
pdu->error = SNMP_ERR_TOO_BIG;
return SNMP_API_STAT_OID_TOO_BIG;
}
//decode OID
memset(pdu->value.OID.data, 0, SNMP_MAX_OID_LEN);
if(pdu->value.OID.decode(_packet + (obiEnd-obiLen+1), obiLen) != SNMP_API_STAT_SUCCESS){
return SNMP_API_STAT_MALLOC_ERR;
}
/**
* Value
*/
//syntax type
pdu->value.syntax = (SNMP_SYNTAXES)valTyp;
//check length of data
if ( valLen > SNMP_MAX_VALUE_LEN && _udp_extra_data_packet == false) {
pdu->error = SNMP_ERR_TOO_BIG;
return SNMP_API_STAT_VALUE_TOO_BIG;
}
//set value size
pdu->value.size = valLen;
if(_udp_extra_data_packet == true){
// memset(extra_data, '\0', extra_data_max_size);
// for ( i = 0; i < valLen; i++ ) {
// extra_data[i] = _packet[obiEnd+3 + i];
// }
}else{
memset(pdu->value.data, 0, SNMP_MAX_VALUE_LEN);
for ( i = 0; i < valLen; i++ ) {
pdu->value.data[i] = _packet[obiEnd+3 + i];
}
}
return SNMP_API_STAT_SUCCESS;
}
// --------------------------------------------------- Ethernet Receive Data ------------------------------------------------------------
void UDP_Recv_Data()
{
Udp.read(cam.ch, sizeof(cam.pose)-4);
}
