Merge pull request #189 from stanford-ssi/payload/heartbeat

Created heartbeat_check function in payload_task.c

Author: sangeetsatpathy

src/drivers/payload_uart/payload_uart.c

@@ -25,6 +25,7 @@
 #define PARITY UART_PARITY_NONE
 #define WRITE_MAX_TIMEOUT 10000
 #define MAX_WRITE_TRIES 3
+#define UART_READ_WRITE_BLOCKING_MS 50
 
 // Packet parameters
 #define MAX_PACKET_LEN 4069
@@ -195,10 +196,17 @@ static void send_syn()
     }
 }
 
+void payload_restart(slate_t *slate)
+{
+    payload_turn_off(slate);
+    payload_turn_on(slate);
+}
+
 void payload_turn_on(slate_t *slate)
 {
     gpio_put(SAMWISE_RPI_ENAB, 1);
     slate->is_payload_on = true;
+    slate->payload_most_recent_ping_time = get_absolute_time();
 }
 
 void payload_turn_off(slate_t *slate)
@@ -399,8 +407,8 @@ uint16_t payload_uart_read_packet(slate_t *slate, uint8_t *packet)
 
     // Receive header
     packet_header_t header;
-    bytes_received =
-        receive_into(slate, &header, sizeof(packet_header_t), 1000);
+    bytes_received = receive_into(slate, &header, sizeof(packet_header_t),
+                                  UART_READ_WRITE_BLOCKING_MS);
 
     if (bytes_received < sizeof(packet_header_t))
     {
@@ -417,7 +425,8 @@ uint16_t payload_uart_read_packet(slate_t *slate, uint8_t *packet)
     }
 
     // Read actual packet
-    bytes_received = receive_into(slate, packet, header.length, 1000);
+    bytes_received =
+        receive_into(slate, packet, header.length, UART_READ_WRITE_BLOCKING_MS);
 
     if (bytes_received < header.length)
     {

src/drivers/payload_uart/payload_uart.h

@@ -23,5 +23,6 @@ payload_write_error_code payload_uart_write_packet(slate_t *slate,
                                                    uint16_t len,
                                                    uint16_t seq_num);
 
+void payload_restart(slate_t *slate);
 void payload_turn_on(slate_t *slate);
 void payload_turn_off(slate_t *slate);

src/slate/slate.h

@@ -129,6 +129,12 @@ typedef struct samwise_slate
     adcs_packet_t adcs_telemetry;
     bool is_adcs_telem_valid;
 
+    /*
+    Payload Heartbeat time: the time at which the Picubed last sent a request to
+    the payload.
+    */
+    absolute_time_t payload_most_recent_ping_time;
+
 } slate_t;
 
 extern slate_t slate;

src/tasks/command/command_parser.c

@@ -115,7 +115,6 @@ void dispatch_command(slate_t *slate, packet_t *packet)
             }
             break;
         }
-
         default:
             LOG_ERROR("Unknown command ID: %i", command_id);
             break;

src/tasks/payload/payload_task.c

@@ -9,6 +9,7 @@
 #include <stdio.h>
 
 #define MAX_RECEIVED_LEN 1024
+#define SEQUENCE_NUMBER_DUMMY 999
 
 void payload_task_init(slate_t *slate)
 {
@@ -30,63 +31,139 @@ void payload_task_init(slate_t *slate)
     // NOTE: Turning on payload is handled by command_parser
 }
 
+bool send_payload_exec(slate_t *slate, char msg[], uint16_t seq_num)
+{
+    LOG_INFO("Executing Payload Command: %s", msg);
+    // First attempt to execute the command but do not throw it away
+    // yet.
+    payload_write_error_code exec_successful =
+        payload_uart_write_packet(slate, msg, sizeof(msg), seq_num);
+
+    if (exec_successful == SUCCESSFUL_WRITE)
+    {
+        return true;
+    }
+    else if (exec_successful == PACKET_TOO_BIG)
+    {
+        LOG_DEBUG("Packet exceeds 4096 bytes...");
+        return false;
+    }
+    else if (exec_successful == SYN_UNSUCCESSFUL)
+    {
+        LOG_DEBUG("PiCubed was unable to sync with the Payload...");
+        return false;
+    }
+    else if (exec_successful == UART_WRITE_TIMEDOUT)
+    {
+        LOG_DEBUG("The transmission took too long and the write timed "
+                  "out...");
+        return false;
+    }
+    else if (exec_successful == HEADER_UNACKNOWLEDGED)
+    {
+        LOG_DEBUG("Payload did not acknowledge the header...");
+        return false;
+    }
+    else if (exec_successful == FINAL_WRITE_UNSUCCESSFUL)
+    {
+        LOG_DEBUG("Final packet transmission timed out...");
+        return false;
+    }
+    else
+    {
+        LOG_DEBUG("Payload command execution failed, retrying...");
+        // If the command was not successful, we will not remove it
+        // from the queue and will try again next time.
+        return false;
+    }
+}
+
+bool ping_command(slate_t *slate)
+{
+    char packet[] = "[\"ping\", [], {}]";
+    bool write_success =
+        send_payload_exec(slate, packet, SEQUENCE_NUMBER_DUMMY);
+    if (!write_success)
+    {
+        LOG_INFO("Writing packet to payload was not successful!");
+        return false;
+    }
+
+    char received[MAX_RECEIVED_LEN];
+    uint16_t received_len = payload_uart_read_packet(slate, received);
+    if (received_len == 0)
+    {
+        LOG_INFO("ACK was not received!");
+        return false;
+    }
+    else
+    {
+        LOG_INFO("ACK received!");
+        LOG_INFO("ACK:");
+        for (uint16_t i = 0; i < received_len; i++)
+        {
+            printf("%c", received[i]);
+        }
+        printf("\n");
+        return true;
+    }
+}
+
+bool send_heartbeat(slate_t *slate)
+{
+    if (slate->is_payload_on)
+    {
+        uint64_t timeDelta =
+            absolute_time_diff_us(slate->payload_most_recent_ping_time,
+                                  get_absolute_time()) /
+            1000;
+        if (timeDelta >= PAYLOAD_HEARTBEAT_TIMEOUT_MS)
+        {
+            payload_restart(slate); // this resets the most_recent_ping_time.
+            // still return false; don't let the payload send commands (since we
+            // aren't getting responses.)
+            return false;
+        }
+        else
+        {
+            bool response_received = ping_command(slate);
+            if (response_received)
+            {
+                slate->payload_most_recent_ping_time = get_absolute_time();
+                return true;
+                // operational! we can send commands now.
+            }
+            // if we don't get a response back, don't update the time.
+            return false;
+        }
+    }
+    return false;
+}
+
 bool try_execute_payload_command(slate_t *slate)
 {
     if (!queue_is_empty(&slate->payload_command_data))
     {
         PAYLOAD_COMMAND_DATA payload_command;
         if (queue_try_peek(&slate->payload_command_data, &payload_command))
         {
-            LOG_INFO("Executing Payload Command: %s",
-                     payload_command.serialized_command);
-            // First attempt to execute the command but do not throw it away
-            // yet.
-            payload_write_error_code exec_successful =
-                payload_uart_write_packet(
-                    slate, payload_command.serialized_command,
-                    sizeof(payload_command.serialized_command),
-                    payload_command.seq_num);
+            bool writeSuccess =
+                send_payload_exec(slate, payload_command.serialized_command,
+                                  payload_command.seq_num);
+            // removeFromQueue is true if we succesfully sent the packet to the
+            // payload
+
             // If the command was successful, remove it from the queue.
             // Alternatively, if we have already retried the command up to
             // a maximum number of times, remove it from the queue.
-            if (exec_successful == SUCCESSFUL_WRITE ||
-                RETRY_COUNT >= MAX_PAYLOAD_RETRY_COUNT)
+            if (writeSuccess || RETRY_COUNT > MAX_PAYLOAD_RETRY_COUNT)
             {
                 // Return success when the command is removed.
                 return queue_try_remove(&slate->payload_command_data,
                                         &payload_command);
             }
-            else if (exec_successful == PACKET_TOO_BIG)
-            {
-                LOG_DEBUG("Packet exceeds 4096 bytes...");
-                return false;
-            }
-            else if (exec_successful == SYN_UNSUCCESSFUL)
-            {
-                LOG_DEBUG("PiCubed was unable to sync with the Payload...");
-                return false;
-            }
-            else if (exec_successful == UART_WRITE_TIMEDOUT)
-            {
-                LOG_DEBUG("The transmission took too long and the write timed "
-                          "out...");
-                return false;
-            }
-            else if (exec_successful == HEADER_UNACKNOWLEDGED)
-            {
-                LOG_DEBUG("Payload did not acknowledge the header...");
-                return false;
-            }
-            else if (exec_successful == FINAL_WRITE_UNSUCCESSFUL)
-            {
-                LOG_DEBUG("Final packet transmission timed out...");
-                return false;
-            }
             else
             {
-                LOG_DEBUG("Payload command execution failed, retrying...");
-                // If the command was not successful, we will not remove it
-                // from the queue and will try again next time.
                 return false;
             }
         }
@@ -135,32 +212,6 @@ void beacon_down_command_test(slate_t *slate)
     }
 }
 
-void ping_command_test(slate_t *slate)
-{
-    char packet[] = "[\"ping\", [], {}]";
-    int len = sizeof(packet) - 1;
-    payload_uart_write_packet(slate, packet, len, 999);
-
-    safe_sleep_ms(1000);
-
-    char received[MAX_RECEIVED_LEN];
-    uint16_t received_len = payload_uart_read_packet(slate, received);
-    if (received_len == 0)
-    {
-        LOG_INFO("ACK was not received!");
-    }
-    else
-    {
-        LOG_INFO("ACK received!");
-        LOG_INFO("ACK:");
-        for (uint16_t i = 0; i < received_len; i++)
-        {
-            printf("%c", received[i]);
-        }
-        printf("\n");
-    }
-}
-
 /*** BRINGUP TESTS ***/
 void power_on_off_payload_test(slate_t *slate)
 {
@@ -258,13 +309,13 @@ void payload_task_dispatch(slate_t *slate)
     neopixel_set_color_rgb(PAYLOAD_TASK_COLOR);
     LOG_INFO("Sending an Info Request Command to the RPI...");
     // beacon_down_command_test(slate);
-    // ping_command_test(slate);
+    // ping_command(slate);
 
     payload_uart_write_on_test(slate);
 
     return;
 
-    if (slate->is_payload_on)
+    if (send_heartbeat(slate))
     {
         LOG_INFO("Payload is ON, executing commands...");
         // Attempts to execute k commands per dispatch.

src/tasks/payload/payload_task.h

@@ -16,6 +16,7 @@
 
 #define MAX_PAYLOAD_COMMANDS_PER_DISPATCH 3
 #define MAX_PAYLOAD_RETRY_COUNT 3
+#define PAYLOAD_HEARTBEAT_TIMEOUT_MS 10000
 
 void payload_task_init(slate_t *slate);
 void payload_task_dispatch(slate_t *slate);