Receive PiEMOS data using new radio task.

controller/inc/radio.h

@@ -25,5 +25,10 @@ BaseType_t radioInit();
 void radioPushUbjson(const char *ubjson, size_t len);
 void radioPushString(const char *str, size_t len);
 
+// TODO(kzentner): This was moved here from main.c
+// TODO(rqou): This really doesn't go here.
+extern int8_t PiEMOSAnalogVals[7];
+extern uint8_t PiEMOSDigitalVals[8];
+
 
 #endif  // INC_RADIO_H_

controller/src/main.c

@@ -52,10 +52,6 @@
 uint8_t *code_buffer;
 uint32_t code_buffer_len;
 
-// TODO(rqou): This really doesn't go here.
-int8_t PiEMOSAnalogVals[7];
-uint8_t PiEMOSDigitalVals[8];
-
 // TODO(rqou): Wat r abstraction?
 // TODO(rqou): My macro-fu is not up to par
 extern ngl_error *ngl_set_motor(ngl_float motor, ngl_float val);

controller/src/radio.c

@@ -41,6 +41,11 @@ typedef struct {
 
 QueueHandle_t radioQueue = NULL;
 
+// TODO(kzentner): This was moved here from main.c
+// TODO(rqou): This really doesn't go here.
+int8_t PiEMOSAnalogVals[7];
+uint8_t PiEMOSDigitalVals[8];
+
 
 // Hard coded
 volatile uint64_t host_addr = 0;  // Bytes reversed
@@ -159,6 +164,14 @@ static portTASK_FUNCTION_PROTO(radioNewTask, pvParameters) {
   portTickType time = xTaskGetTickCount();
   portTickType lastTime = time;
 
+  // Initialize PiEMOS values.
+  for (int i = 0; i < 7; i++) {
+    PiEMOSAnalogVals[i] = 0.0;
+  }
+  for (int i = 0; i < 8; i++) {
+    PiEMOSDigitalVals[i] = 0;
+  }
+
   while (1) {
     recvMsg = NULL;
     recvSize = 0;
@@ -239,9 +252,29 @@ static portTASK_FUNCTION_PROTO(radioNewTask, pvParameters) {
       recXbeeHeader = (xbee_rx64_header*)&(recXbeePacket->payload);
       if (recXbeeHeader->xbee_api_type == XBEE_API_TYPE_RX64) {
         host_addr = recXbeeHeader->xbee_src_addr;
-        if (uartRecvSize >= prefixLen && recXbeeHeader->data[0] == NDL3_IDENT) {
-          NDL3_L2_push(target, (uint8_t*)recXbeeHeader->data+1,
-            recXbeePacket->length-sizeof(xbee_rx64_header)-prefixLen);
+        uint8_t ident_byte = recXbeeHeader->data[0];
+        switch (ident_byte) {
+          case NDL3_IDENT:
+            {
+              NDL3_L2_push(target, (uint8_t*)recXbeeHeader->data+1,
+                recXbeePacket->length-sizeof(xbee_rx64_header)-prefixLen);
+            }
+            break;
+          case PIER_INCOMINGDATA_IDENT:
+            {
+              pier_incomingdata *incomingData =
+                (pier_incomingdata *)(recXbeeHeader->data);
+              // TODO(kzentner): This was copied from main.c
+              // TODO(rqou): This code is terribly hardcoded.
+              for (int i = 0; i < 7; i++) {
+                PiEMOSAnalogVals[i] =
+                  (float)((int)incomingData->analog[i] - 127) / 127.0f * 100.0f;
+              }
+              for (int i = 0; i < 8; i++) {
+                PiEMOSDigitalVals[i] = !!(incomingData->digital & (1 << i));
+              }
+            }
+            break;
         }
       }
       vPortFree(recXbeePacket);