Diff: main.cpp

Revision:

2:3e7685cfb2a7

Parent:

1:6055c6873f85

Child:

3:a38ad504a18c

--- a/main.cpp	Tue Mar 03 15:14:10 2015 -0600
+++ b/main.cpp	Thu Mar 05 15:47:08 2015 +0000
@@ -1,23 +1,920 @@
-// Hello world example of MCR20
 #include "mbed.h"
 
-SPI mcr20(D11, D12, D13);
-DigitalOut cs(D10, 1);
-DigitalOut led(LED1, 1);
-Serial pc(USBTX, USBRX);
+#include "driverDebug.h"
+
+#include "socket_api.h"
+#include "net_interface.h"
+#include "system_event.h"
+#include "string.h"
+#include "net_nwk_scan.h"
+#include "socket_security.h"
+#include "nwk_stats_api.h"
+#include "multicast_api.h"
+#include "nsdynmemLIB.h"
+#include "randLIB.h"
+#include "arm_hal_timer.h"
+#include "common_functions.h"
+#include "driverRFPhy.h"
+#include "driverAtmelRFInterface.h"
+
+#include "nsdl_support.h"
+#include "node_cfg.h"
+
+#if NODE_SENSOR_STATION
+//  #include "DHT.h"
+  #include "BMP085.h"
+#endif
+
+
+/*** DEBUG ******/
+#define DEBUG       1
+
+#if DEBUG
+#define debug   printf
+#else
+#define debug(...)  ""
+#endif  // DEBUG
+
+/*******  Hardware Definition  **********/
+#if NODE_CONTROLLER
+  DigitalOut ledR(PTB22);      // RGB - Red LED
+  DigitalOut ledG(PTE26);      // RGB - Green LED
+  DigitalOut ledB(PTB21);      // RGB - Blue LED    
+  InterruptIn TAP_INT(PTC6);  // INT1 from FXOS8700Q
+#endif
+
+#if NODE_HOME
+DigitalOut  light(D2);
+DigitalOut  fire(D3);
+DigitalOut  fan(D4);
+InterruptIn motion(D6);
+DigitalIn door(D8);
+#endif
+
+#if NODE_SENSOR_STATION
+//DHT sensor(D2, DHT11);
+BMP085 barometer(D14, D15);
+AnalogIn moisture(A0);
+AnalogIn HCHO(A1);
+AnalogIn UVsensor(A2);
+#endif
+
+/*******  Timers Definitions   ************/
+#define RETRY_TIMER             1
+#define NSP_REGISTRATION_TIMER  2
+#if NODE_CONTROLLER 
+  #define ACCELEROMETER_TIMER    3
+#endif
+#if NODE_HOME
+  #define DOOR_CHECK_TIMER        4
+#endif
+
+
+void app_heap_error_handler(heap_fail_t event);
+void tasklet_main(arm_event_s *event);
+void button_main(arm_event_s *event);
+
+
+#define APP_DEV_HEAP_SIZE 6000
+static uint8_t app_defined_stack_heap[APP_DEV_HEAP_SIZE];
+static int8_t main_tasklet_id = -1;
+
+
+static const uint8_t app_MAC_address[8] = NODE_MAC_ADDRESS;
+static int8_t rf_phy_device_register_id = -1;
+static int8_t net_6lowpan_id = -1;
+static uint32_t channel_list = 0x07FFF800; // All channels
+
+int8_t coap_udp_socket = -1;
+
+#define AUDP_SOCKET_PORT    61630
+#define RECV_CTRL_UDP_PORT   1050    // receive
+#define SEND_CTRL_UDP_PORT   1060    // send
+
+/** Used for Receive Data source Address store*/
+static  ns_address_t app_src;
+static sn_nsdl_addr_s sn_addr_s;
+
+static int access_point_status = 0;
+
+link_layer_setups_s app_link_info;
+link_layer_address_s app_link_address_info;
+network_layer_address_s app_nd_address_info;
+
+/* Prototype functions  */
+void NSDL_receive_socket(void *cb);
+void NSP_registration(void);
+
+extern void create_lqi_resource(uint8_t);
+void* own_alloc(uint16_t size);
+void own_free(void *ptr);
+
+/***************  Application variables  ***********************/
+uint8_t batteryLevel = 100;
+enum LED {NONE, RED, GREEN, BLUE};
+int ledState = NONE;
+
+
+/* #defines, definitions and declarations according node profile  */
+#if NODE_HOME
+#define DOOR_CHECK_TIME     1000
+uint8_t door_last_state = 0;
+/* Since the PIR Motion detector throws a (false) trigger everytime the
+other sensors/actuators change (possibly an interference), we will need
+to disable it for 1s. So every time the sensors/actuators change, we set a variable
+which is reset in the DOOR_CHECK_TIMER. This variable needs to be cleared to
+consider a real motion detection */
+uint8_t motion_disable = 0;         
+int8_t ctrl_udp_socket = -1;
+void ctrl_udp_receive(void * cb);
+void ctrl_udp_send(uint8_t cmd);
+#endif
+
+#if NODE_CONTROLLER
+int8_t ctrl_udp_socket = -1;
+void empty_udp_receive(void * cb);
+void ctrl_udp_send(uint8_t cmd);
+
+#include "FXOS8700Q_TD.h"
+
+#define ACCEL_SAMPLE_TIME   200
+FXOS8700Q accel( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
+bool accel_enable = 1;
+   
+void tap_detector_handler(void) {
+   printf("Tap Detected!\r\n"); 
+   
+   switch(ledState) {
+     case NONE:
+        break;
+     case RED: 
+        ctrl_udp_send('R');   
+        break;
+     case GREEN:
+        ctrl_udp_send('G');
+        break;
+     case BLUE:
+        ctrl_udp_send('B');
+        break;            
+    }
+}
+
+void update_LED(void) {
+  
+  switch(ledState) {
+    case NONE:
+      ledState = GREEN;
+      ledR = 1;
+      ledG = 0;
+      ledB = 1;
+      break;
+      
+    case GREEN:
+      ledState = RED;
+      ledR = 0;
+      ledG = 1;
+      ledB = 1;
+      break;
+      
+    case RED:
+      ledState = BLUE;
+      ledR = 1;
+      ledG = 1;
+      ledB = 0; 
+      break;
+
+    case BLUE:
+      ledState = NONE;
+      ledR = 1;
+      ledG = 1;
+      ledB = 1;
+      break;                
+  }       
+}
+
+#endif
 
-int main(void)
+#if 1
+void printf_array(uint8_t *ptr , uint16_t len)
+{
+    uint16_t i;
+    for(i=0; i<len; i++)
+    {
+        if(i)
+        {
+            if(i%16== 0)
+            {
+                debug("\r\n");
+                if(len > 64)
+                {
+                    uint8_t x =254;
+                    while(x--);
+                }
+            }
+            else
+            {
+                debug(":");
+            }
+        }
+        debug_hex(*ptr++);
+    }
+    debug("\r\n");
+}
+
+
+
+void debug_integer(uint8_t width, uint8_t base, int16_t n)
+{
+  uint8_t bfr[8];
+  uint8_t *ptr = bfr;
+  uint8_t ctr = 0;
+
+  if (width > 7) width = 7;
+
+  ptr += width;
+  *ptr-- = 0;
+
+  if (base == 16)
+  {
+      do
+      {
+          *ptr = n & 0x0F;
+          if (*ptr < 10) *ptr += '0';
+          else *ptr += ('A'-10);
+          ptr--;
+          n >>= 4;
+          ctr++;
+      }while((ctr & 1) || (ctr < width));
+  }
+  else
+  {
+      uint8_t negative = 0;
+      if (n < 0)
+      { negative = 1;
+        n = -n;
+      }
+      ctr++;
+      do
+      {
+        *ptr-- = (n % 10) + '0';
+        n /= 10;
+        ctr++;
+      }while ((ctr < width) && n);
+      if (negative)
+      {
+        *ptr-- = '-';
+      }
+      else
+      {
+        *ptr-- = ' ';
+      }
+  }
+  ptr++;
+  //debug_send(ptr);
+    debug((const char *)ptr);
+}
+
+
+void printf_ipv6_address(uint8_t *addr_ptr)
 {
-    char control;
-    char buf[128];
+    if(addr_ptr)
+    {
+        uint8_t i, d_colon = 0;
+        uint16_t current_value = 0, last_value = 0;
+
+        for(i=0; i< 16;i += 2)
+        {
+            current_value =  (*addr_ptr++ << 8);
+            current_value += *addr_ptr++;
+
+            if(i == 0)
+            {
+                last_value = current_value;
+                debug_hex(current_value >> 8);
+                debug_hex(current_value );
+                debug(":");
+            }
+            else
+            {
+                if(current_value == 0)
+                {
+                    if(i== 14)
+                    {
+                        debug(":");
+                        //debug_put('0');
+                        debug("0");
+                    }
+                    else
+                    {
+                        if(last_value == 0)
+                        {
+                            if(d_colon == 0)
+                            {
+                                d_colon=1;
+                            }
+                        }
+                        else
+                        {
+                            if(d_colon == 2)
+                            {
+                                //debug_put('0');
+                                debug("0");
+                                debug(":");
+                            }
+                        }
+                    }
+                }
+                else
+                {
+                    if(last_value == 0)
+                    {
+                        if(d_colon == 1)
+                        {
+                            debug(":");
+                            d_colon = 2;
+                        }
+                        else
+                        {
+                            //debug_put('0');
+                            debug("0");
+                            debug(":");
+                        }
+                    }
+                    if(current_value > 0x00ff)
+                    {
+                        debug_hex(current_value >> 8);
+                    }
+                    debug_hex(current_value );
+                    if(i< 14)
+                    {
+                        debug(":");
+                    }
+                }
+                last_value = current_value;
+            }
+        }
+    }
+    else
+    {
+        debug("Address Print: pointer NULL");
+    }
+    debug("\r\n");
+}
+#endif
+
+#if NODE_HOME
+void motion_handler(void)
+{
+  if (motion_disable)
+    return;
+  printf("***************Motion Detected!**********************\r\n");
+  if (door)
+    ctrl_udp_send('O');
+  else
+    ctrl_udp_send('C');
+}
+#endif
+
+
+int main()
+{
+#if NODE_CONTROLLER      
+  ledR = 1;
+  ledG = 1;
+  ledB = 1;
+  TAP_INT.rise(&tap_detector_handler); 
+#endif
+ 
+#if NODE_HOME  
+  light = 0;
+  motion.rise(&motion_handler);
+#endif
+     
+  debug("\r\nApplication Start\r\n");
+  ns_dyn_mem_init(app_defined_stack_heap, APP_DEV_HEAP_SIZE, app_heap_error_handler,0);
+  rf_set_mac_address(app_MAC_address);
+      
+  // init RF interface
+  debug("Init RF Interface...\r\n");
+  rf_phy_device_register_id = rf_device_register();
+  randLIB_seed_random();
     
-    while(1) {
-        if (pc.readable()) {
-            led = !led;
-            pc.getc();
+  debug("Init ARM Timer...\r\n");
+  arm_timer_init();//Init Timer
+  //Init nanostack & Event OS
+  debug("Init Net Core...\r\n");
+  net_init_core();
+  debug("Create Tasklets...\r\n");
+  main_tasklet_id = arm_ns_tasklet_create(&tasklet_main);
+  if(main_tasklet_id < 0)
+  {
+    //Tasklet cerate fail
+    error("Tasklet create fail.."); 
+    while(1); 
+  }
+  debug("Event Dispatch\r\n");
+  event_dispatch();
+}
+
+/**
+  * @brief  This function handles Hard Fault exception.
+  * @param  None
+  * @retval None
+  */
+void HardFault_Handler(void)
+{
+  /* Go to infinite loop when Hard Fault exception occurs */
+  while (1)
+  {
+  }
+}
+void app_heap_error_handler(heap_fail_t event)
+{
+    switch (event)
+    {
+        case NS_DYN_MEM_NULL_FREE:
+            break;
+        case NS_DYN_MEM_DOUBLE_FREE:
+            break;
+
+        case NS_DYN_MEM_ALLOCATE_SIZE_NOT_VALID:
+            break;
+        case NS_DYN_MEM_POINTER_NOT_VALID:
+            break;
+
+        case NS_DYN_MEM_HEAP_SECTOR_CORRUPTED:
+            break;
+
+        case NS_DYN_MEM_HEAP_SECTOR_UNITIALIZED:
+            break;
+                
+        default:
+
+            break;
+    }
+    while(1);
+}
+
+
+/**
+  * \brief Network state event handler.
+  * \param event show network start response or current network state.
+  *
+  * - NET_READY: Save NVK peristant data to NVM and Net role
+  * - NET_NO_BEACON: Link Layer Active Scan Fail, Stack is Already at Idle state
+  * - NET_NO_ND_ROUTER: No ND Router at current Channel Stack is Already at Idle state
+  * - NET_BORDER_ROUTER_LOST: Connection to Access point is lost wait for Scan Result
+  * - NET_PARENT_POLL_FAIL: Host should run net start without any PAN-id filter and all channels
+  * - NET_PANA_SERVER_AUTH_FAIL: Pana Authentication fail, Stack is Already at Idle state
+  */
+void app_parse_network_event(arm_event_s *event )
+{
+  arm_nwk_interface_status_type_e status = (arm_nwk_interface_status_type_e)event->event_data;
+  switch (status)
+  {
+    case ARM_NWK_BOOTSTRAP_READY:
+      /* NEtwork is ready and node is connected to Access Point */
+      if(access_point_status==0)
+      {
+        uint8_t temp_ipv6[16];
+        debug("Network Connection Ready\r\n");
+        access_point_status=1;
+        //Read Address
+
+        if( arm_nwk_nd_address_read(net_6lowpan_id,&app_nd_address_info) != 0)
+        {
+          debug("ND Address read fail\r\n");
+        }
+        else
+        {
+          debug("ND Access Point: ");
+          printf_ipv6_address(app_nd_address_info.border_router); //REVIEW
+
+          debug("ND Prefix 64: ");
+          printf_array(app_nd_address_info.prefix, 8);            //REVIEW
+
+          if(arm_net_address_get(net_6lowpan_id,ADDR_IPV6_GP,temp_ipv6) == 0)
+          {
+            debug("GP IPv6: ");
+            printf_ipv6_address(temp_ipv6);         //REVIEW
+          }
+        }
+
+        if( arm_nwk_mac_address_read(net_6lowpan_id,&app_link_address_info) != 0)
+        {
+          debug("MAC Address read fail\r\n");
+        }
+        else
+        {
+          uint8_t temp[2];
+          common_write_16_bit(app_link_address_info.mac_short,temp);
+          debug("MAC 16-bit: ");
+          printf_array(temp, 2);  //REVIEW
+          common_write_16_bit(app_link_address_info.PANId,temp);
+          debug("PAN-ID: ");
+          printf_array(temp, 2);  //REVIEW
+          debug("MAC 64-bit: ");
+          printf_array(app_link_address_info.long_euid64, 8); //REVIEW
+          debug("EUID64(Based on MAC 64-bit address): ");
+          printf_array(app_link_address_info.euid64, 8);  //REVIEW
+        }
+      }
+      break;
+    
+    case ARM_NWK_NWK_SCAN_FAIL:
+      /* Link Layer Active Scan Fail, Stack is Already at Idle state */
+      debug("Link Layer Scan Fail: No Beacons\r\n");
+      access_point_status=0;
+      //dnssd_disable(1);
+      break;
+    
+    case ARM_NWK_IP_ADDRESS_ALLOCATION_FAIL:
+      /* No ND Router at current Channel Stack is Already at Idle state */
+      debug("ND Scan/ GP REG fail\r\n");
+      access_point_status=0;
+      //dnssd_disable(1);
+      break;
+    
+    case ARM_NWK_NWK_CONNECTION_DOWN:
+      /* Connection to Access point is lost wait for Scan Result */
+      debug("ND/RPL scan new network\r\n");
+      access_point_status=0;
+      break;
+
+    case ARM_NWK_NWK_PARENT_POLL_FAIL:
+      access_point_status=0;
+      break;
+    
+    case ARM_NWK_AUHTENTICATION_FAIL:
+      debug("Network authentication fail\r\n");
+      access_point_status=0;
+      break;
+    
+    default:
+      debug_hex(status);    //REVIEW
+      debug("Unknow event");
+      break;
+  }
+  
+  if(access_point_status == 0)
+  {
+    //Set Timer for new Trig
+    timer_sys_event(RETRY_TIMER, 10000);
+  }
+}
+
+
+/**
+  * \brief Handler for events sent to the application.
+  * \param event received event.
+  *
+  * - EV_NETWORK event, Network Event state event handler
+  * - EV_INIT, Set Certificate Chain list, init multicast, Start net start if NVM have session
+  * - EV_DEBUG, Terminal handler
+  */
+void tasklet_main(arm_event_s *event)
+{
+  if(event->sender == 0)
+  {
+    arm_library_event_type_e event_type;
+    event_type = (arm_library_event_type_e)event->event_type;
+
+    switch(event_type)
+    {
+      case ARM_LIB_NWK_INTERFACE_EVENT:
+        /* Network Event state event handler */
+        debug("Event: ARM_LIB_NWK_INTERFACE\r\n");
+        app_parse_network_event(event);                  
+        break;
+
+      case ARM_LIB_TASKLET_INIT_EVENT:
+        /*Init event from stack at start-up*/
+        debug("Event: ARM_LIB_TASKLET_INIT\r\n");
+#if NODE_CONTROLLER  
+        timer_sys_event(ACCELEROMETER_TIMER, ACCEL_SAMPLE_TIME);
+#endif  
+        multicast_set_parameters(10,0,20,3,75 );
+                                       
+#if NODE_HOME
+        /* open a socket to support control from NODE_CONTROLLER */                                             
+        ctrl_udp_socket = socket_open(SOCKET_UDP, RECV_CTRL_UDP_PORT, ctrl_udp_receive);
+        /* init a timer to check door state  */
+        timer_sys_event(DOOR_CHECK_TIMER, DOOR_CHECK_TIME);
+#endif
+
+#if NODE_CONTROLLER
+        /* open a socket to support control for REMOTE_NODE */                                             
+        ctrl_udp_socket = socket_open(SOCKET_UDP, SEND_CTRL_UDP_PORT, empty_udp_receive); 
+#endif
+
+        net_6lowpan_id = arm_nwk_interface_init(NET_INTERFACE_RF_6LOWPAN, rf_phy_device_register_id, "6LoWPAN_BORDER_ROUTER");
+        if(net_6lowpan_id < 0) 
+        {
+          debug("Interface Generate Fail\r\n");
+          while(1);
+        } 
+        else 
+        {
+          //SET Bootsrap
+          if(arm_nwk_interface_configure_6lowpan_bootstrap_set(net_6lowpan_id, NET_6LOWPAN_HOST, 1) != 0)  // Last parameter enables MLE protocol
+          {
+            //Bootsrap SET fail
+            debug("Bootstrap Fail\r\n");
+            while(1);
+          }
+          else
+          {
+            int8_t retval = -1;
+            arm_nwk_6lowpan_gp_address_mode(net_6lowpan_id, NET_6LOWPAN_GP16_ADDRESS, NODE_SHORT_ADDRESS, 1);  // 5 = short address for link-layer // 1 = generate automatically if duplicate address is encountered
+            arm_nwk_link_layer_security_mode(net_6lowpan_id, NET_SEC_MODE_NO_LINK_SECURITY, 0, 0);
+            arm_nwk_6lowpan_link_scan_paramameter_set(rf_phy_device_register_id, channel_list, 5);
+            retval = arm_nwk_interface_up(net_6lowpan_id);
+            if(retval != 0)
+            {
+              //6Lowpan Bootsrap start fail
+              debug("6LowPAN Bootstrap start Fail\r\n");
+              while(1);
+            }
+            else
+            {
+              //6Lowpan Bootsrap start OK
+              debug("6LowPAN Bootstrap Start OK\r\n");
+            }  
+            // open sockets
+            coap_udp_socket = socket_open(SOCKET_UDP, AUDP_SOCKET_PORT, NSDL_receive_socket);
+            // Start NSDL
+            nsdl_run();     
+          }
+          timer_sys_event(NSP_REGISTRATION_TIMER, RD_UPDATE_PERIOD);
         }
-        //pc.scanf("%c%s\n", &control, &buf);
-        
-        //Chris was here
-    }    
-}
\ No newline at end of file
+        break;
+                                    
+      case ARM_LIB_SYSTEM_TIMER_EVENT:
+        timer_sys_event_cancel(event->event_id);
+        if (event->event_id == NSP_REGISTRATION_TIMER) 
+        {   
+          printf("Time to register...\r\n");                                    
+          NSP_registration();
+      #if BATTERY
+          batteryLevel -= 10;
+          if (batteryLevel == 0)
+            batteryLevel = 100;
+      #endif
+          timer_sys_event(NSP_REGISTRATION_TIMER, RD_UPDATE_PERIOD);                                 
+        } 
+        else if (event->event_id == RETRY_TIMER) 
+        {
+          debug("Event: ARM_LIB_SYSTEM_TIMER (event_id = 1)\r\n");
+          int8_t retval = -1;
+          retval = arm_nwk_interface_up(net_6lowpan_id);
+          if(retval != 0)
+          {
+            //6Lowpan Bootsrap start fail
+            debug("6LowPAN Bootstrap Start Failure\r\n");
+            while(1);
+          }
+          else
+          {
+            //6Lowpan Bootsrap start OK
+            debug("6LowPAN Bootstrap Start OK\r\n");
+          }             
+        }
+#if NODE_CONTROLLER        
+        else if (event->event_id == ACCELEROMETER_TIMER)
+        {
+          if (accel.getAccX() < -0.85f && accel_enable) 
+          {
+            accel_enable = 0;
+            update_LED();
+          }  
+          if (accel.getAccX() > -0.35f) 
+            accel_enable = 1;
+          
+          timer_sys_event(ACCELEROMETER_TIMER, ACCEL_SAMPLE_TIME);   
+        }    
+#endif  
+#if NODE_HOME        
+        else if (event->event_id == DOOR_CHECK_TIMER)
+        {
+          uint8_t door_state = door.read();
+          if (door_state != door_last_state) 
+          {
+            door_last_state = door_state;
+            printf("Door state %d\r\n", door_state);
+          }            
+          motion_disable = 0;
+          timer_sys_event(DOOR_CHECK_TIMER, DOOR_CHECK_TIME);   
+        }    
+#endif       
+        break;
+                                
+      default:
+        break;
+    }
+  }
+}
+
+void NSDL_receive_socket(void * cb)
+{
+  socket_callback_t * cb_res =0;
+  int16_t length;
+  cb_res = (socket_callback_t *) cb;
+  uint8_t *payload;
+
+  if(cb_res->event_type == SOCKET_DATA)
+  {
+    debug("LINK LQI:");
+    debug_hex(cb_res->LINK_LQI);
+    create_lqi_resource(cb_res->LINK_LQI);
+    debug("\r\n");
+
+    if ( cb_res->d_len > 0)
+    {
+      payload = (uint8_t *) own_alloc(cb_res->d_len);
+      if(payload)
+      {
+        //Read data to the RX buffer
+        length = socket_read(cb_res->socket_id, &app_src, payload, cb_res->d_len);  //replace rx_buffer payload
+        if(length)
+        {
+          if(cb_res->socket_id == coap_udp_socket)
+          {
+            // Handles data received in UDP socket                      
+            // Call application protocol parser.
+            sn_addr_s.type = SN_NSDL_ADDRESS_TYPE_IPV6;
+            sn_addr_s.addr_len = 16;
+            sn_addr_s.port = app_src.identifier;
+            sn_addr_s.addr_ptr = app_src.address;
+            printf("Data 1\r\n");   
+            if(sn_nsdl_process_coap(payload, length, &sn_addr_s))  // 0= ok, -1=failure
+            {
+              debug("Error processing CoAP packet\r\n");
+            }
+            printf("Data 4\r\n");   
+          }
+        }
+        own_free(payload);
+      }
+    }
+  }
+#if 1       // enabled for debug
+  else if(cb_res->event_type == SOCKET_TX_DONE)
+  {
+    //debug("*");
+  }
+  else if(cb_res->event_type == SOCKET_NO_ROUTE)
+  {
+    debug("SOCKET_NO_ROUTE\r\n");
+  }
+  else if(cb_res->event_type == SOCKET_TX_FAIL)
+  {
+    debug("SOCKET_TX_FAIL\r\n");
+  }
+#endif   
+}
+
+#if NODE_HOME
+/******  Control UDP Socket Receive  *******/
+void ctrl_udp_receive(void * cb)
+{
+  socket_callback_t * cb_res =0;
+  int16_t length;
+  cb_res = (socket_callback_t *) cb;
+
+  if(cb_res->event_type == SOCKET_DATA)
+  {
+    uint8_t *payload = 0;
+    /*Read Data*/
+    if ( cb_res->d_len > 0)
+    {
+      /*Read data to the RX buffer*/
+      payload = (uint8_t *) ns_dyn_mem_alloc(cb_res->d_len);
+      if(payload)
+      {
+        /*Read data to the RX buffer*/
+        length = socket_read(cb_res->socket_id, &app_src, payload, cb_res->d_len);
+        if(length)
+        {
+          uint8_t * ptr = payload ;
+          /*Handles data received in UDP socket*/
+          debug("UDP Data From: ");
+          printf_ipv6_address(&(app_src.address[0]));        //REVIEW
+          
+          if(app_src.identifier == SEND_CTRL_UDP_PORT)       // check source port to avoid interference
+          {
+            debug("Data: ");
+            printf("%s\r\n",ptr);     
+            switch(payload[0]) {
+              case 'R':
+                fire = !fire;
+                fan = 0;
+                break;    
+              case 'G':
+                light = !light; 
+                break;              
+              case 'B':
+                fan = !fan;
+                fire = 0; 
+                break;              
+            } 
+            motion_disable = 1;
+          }  
+        }
+        ns_dyn_mem_free(payload);
+      }
+    }
+    else
+    {
+      debug("No Data\r\n");
+      socket_read(cb_res->socket_id, &app_src, NULL, 0);
+    }
+  }
+  else if(cb_res->event_type == SOCKET_TX_DONE)
+  {
+    debug("UDP TX Done\r\n");
+  }
+  /* No Route to Packet Destination Address */
+  else if(cb_res->event_type == SOCKET_NO_ROUTE) 
+  {
+    debug("ND/RPL not ready\r\n");
+  }
+  /* Link Layer TX Fail for socket packet */
+  else if(cb_res->event_type == SOCKET_TX_FAIL)
+  {
+    debug("Link Layer Tx fail\r\n");
+  }
+}
+
+void ctrl_udp_send(uint8_t cmd)
+{
+  uint8_t * payload = 0;
+  ns_address_t address;
+  /*SET UDP echo Port*/
+  address.identifier = ALARM_CTRL_UDP_PORT;  
+
+  uint8_t node_address[16] = ALARM_IP_ADDRESS;       
+    
+  payload = (uint8_t *) ns_dyn_mem_alloc(1);        
+  if(payload)
+  {
+    uint8_t *ptr = payload;
+     
+    memcpy(address.address,node_address,16); 
+    address.type = ADDRESS_IPV6;
+    *ptr = cmd;
+    if(socket_sendto(ctrl_udp_socket, &address, payload, (1)) != 0)
+    {
+      debug("Ctrl UDP Failed\r\n");
+    }
+    else
+    {
+      debug("Ctrl UDP Ok\r\n");
+    }
+    ns_dyn_mem_free(payload);
+  }
+  else
+  {
+    debug("No Heap for Ctrl UDP\r\n");
+  }
+}
+#endif
+
+#if NODE_CONTROLLER 
+void ctrl_udp_send(uint8_t cmd)
+{
+  uint8_t * payload = 0;
+  ns_address_t address;
+  /*SET UDP echo Port*/
+  address.identifier = RECV_CTRL_UDP_PORT;  
+
+  uint8_t node_address[16] = REMOTE_NODE;       
+    
+  payload = (uint8_t *) ns_dyn_mem_alloc(1);        
+  if(payload)
+  {
+    uint8_t *ptr = payload;
+     
+    memcpy(address.address,node_address,16); 
+    address.type = ADDRESS_IPV6;
+    *ptr = cmd;
+    if(socket_sendto(ctrl_udp_socket, &address, payload, (1)) != 0)
+    {
+      debug("Ctrl UDP Failed\r\n");
+    }
+    else
+    {
+      debug("Ctrl UDP Ok\r\n");
+    }
+    ns_dyn_mem_free(payload);
+  }
+  else
+  {
+    debug("No Heap for Ctrl UDP\r\n");
+  }
+}
+// we don't expect to receive data
+void empty_udp_receive(void * cb) {
+    
+}
+#endif
+