Add lighting for SIL.

Co-authored-by: James Goppert <james.goppert@gmail.com>
Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>

Author: jgoppert

app/b3rb/src/lighting.c

@@ -160,10 +160,8 @@ static void lighting_work_handler(struct k_work* work)
         const double* color = NULL;
         if (ctx->safety.status == synapse_msgs_Safety_Status_SAFETY_SAFE) {
             color = color_safe;
-        } else if (ctx->safety.status == synapse_msgs_Safety_Status_SAFETY_UNSAFE) {
-            color = color_unsafe;
         } else {
-            color = color_unknown;
+            color = color_unsafe;
         }
         set_led(safety_leds[i], color, brightness, &ctx->led_array.led[led_msg_index]);
         led_msg_index++;

lib/dream/sil/zephyr_main.c

@@ -35,11 +35,13 @@ static struct k_thread my_thread_data;
 static void zephyr_sim_entry_point(void* p0, void* p1, void* p2)
 {
     struct zros_node node;
-    struct zros_sub sub_actuators;
+    struct zros_sub sub_actuators, sub_led_array;
     synapse_msgs_Actuators actuators;
+    synapse_msgs_LEDArray led_array;
 
     zros_node_init(&node, "dream_sil");
     zros_sub_init(&sub_actuators, &node, &topic_actuators, &actuators, 10);
+    zros_sub_init(&sub_led_array, &node, &topic_led_array, &led_array, 10);
 
     sil_context_t* ctx = p0;
     ARG_UNUSED(p1);
@@ -103,6 +105,24 @@ static void zephyr_sim_entry_point(void* p0, void* p1, void* p2)
             }
         }
 
+        // send led array if subscription updated
+        if (zros_sub_update_available(&sub_led_array)) {
+            zros_sub_update(&sub_led_array);
+            TF_Msg msg;
+            TF_ClearMsg(&msg);
+            uint8_t buf[synapse_msgs_LEDArray_size];
+            pb_ostream_t stream = pb_ostream_from_buffer((pu8)buf, sizeof(buf));
+            int status = pb_encode(&stream, synapse_msgs_LEDArray_fields, &led_array);
+            if (status) {
+                msg.type = SYNAPSE_LED_ARRAY_TOPIC;
+                msg.data = buf;
+                msg.len = stream.bytes_written;
+                TF_Send(&ctx->tf, &msg);
+            } else {
+                LOG_ERR("encoding failed: %s", PB_GET_ERROR(&stream));
+            }
+        }
+
         // compute board time
         uint64_t uptime = k_uptime_get();
         struct timespec ts_board;