Improving client/machine app to handle black channel

pstop_c/examples/client/client_app.c

@@ -6,13 +6,25 @@
 #include <stdio.h>
 #include <stdlib.h>
 
+#include <time.h>
+
 #include "pstop/pstop_msg.h"
 #include "transport/udp/udp_transport.h"
 #include "pstop/device_id.h"
 #include "pstop/os.h"
+#include "pstop/machine_client_data.h"
 
 udp_transport_data_t udp_transport;
 
+void
+sleep_ms(uint64_t ms)
+{
+    struct timespec ts;
+    ts.tv_sec = 0;
+    ts.tv_nsec = ms * 1000000L;
+    nanosleep(&ts, NULL);
+}
+
 int
 read_msg(udp_transport_data_t *transport, pstop_os_env *env, pstop_msg_t *resp, uint64_t timeout)
 {
@@ -37,69 +49,69 @@ read_msg(udp_transport_data_t *transport, pstop_os_env *env, pstop_msg_t *resp,
     return 0;
 }
 
-void
-send_bond(udp_transport_data_t *transport, pstop_os_env *env, const device_id_t *uuid)
+int
+send_msg(udp_transport_data_t *transport, pstop_os_env *env, machine_client_data_t *machine, const device_id_t *uuid, uint8_t msg)
 {
     uint8_t reqbytes[PSTOP_MESSAGE_SIZE];
 
     pstop_msg_t req_msg;
     pstop_msg_t resp_msg;
 
-    req_msg.message = PSTOP_MESSAGE_BOND;
+    uint64_t now = env->get_time_cb();
+
+    req_msg.message = msg;
     device_id_copy(&req_msg.id, uuid);
+    device_id_copy(&req_msg.receiver_id, &(machine->client_id));
+    req_msg.counter = machine->last_counter + 1U;
+    req_msg.received_counter = machine->last_counter;
+    req_msg.received_stamp = machine->last_timestamp;
+    req_msg.stamp = now;
+
+    machine->last_timestamp = now;
+    machine->last_counter = req_msg.counter;
 
     pstop_message_encode(&req_msg, reqbytes);
 
     transport_udp_write(transport, reqbytes, PSTOP_MESSAGE_SIZE, NULL);
 
-    if(read_msg(transport, env, &resp_msg, 2000)) {
-        fprintf(stderr, "Received msg: %d\n", resp_msg.message);
+    if(!read_msg(transport, env, &resp_msg, 2000)) {
+        return 0;
     }
+    fprintf(stderr, "Received msg: %d counter=%d\n", resp_msg.message, resp_msg.counter);
+    machine->last_counter = resp_msg.counter;
+    machine->last_timestamp = resp_msg.stamp;
+
+    return 1;
 }
 
 void
-send_ok(udp_transport_data_t *transport, pstop_os_env *env, const device_id_t *uuid, int is_ok)
+send_bond(udp_transport_data_t *transport, pstop_os_env *env, machine_client_data_t *machine, const device_id_t *uuid)
 {
-    uint8_t reqbytes[PSTOP_MESSAGE_SIZE];
-
-    pstop_msg_t req_msg;
-    pstop_msg_t resp_msg;
+    if(send_msg(transport, env, machine, uuid, PSTOP_MESSAGE_BOND)) {
+        fprintf(stderr, "BOND Success\n");
+    }
+}
 
+void
+send_ok(udp_transport_data_t *transport, pstop_os_env *env, machine_client_data_t *machine, const device_id_t *uuid, int is_ok)
+{
     if(is_ok) {
-        req_msg.message = PSTOP_MESSAGE_OK;
+        if(send_msg(transport, env, machine, uuid, PSTOP_MESSAGE_OK)) {
+            fprintf(stderr, "OK Success\n");
+        }
     }
     else {
-        req_msg.message = PSTOP_MESSAGE_STOP;
-    }
-
-    device_id_copy(&req_msg.id, uuid);
-
-    pstop_message_encode(&req_msg, reqbytes);
-
-    transport_udp_write(transport, reqbytes, PSTOP_MESSAGE_SIZE, NULL);
-
-    if(read_msg(transport, env, &resp_msg, 2000)) {
-        fprintf(stderr, "Received msg: %d\n", resp_msg.message);
+        if(send_msg(transport, env, machine, uuid, PSTOP_MESSAGE_STOP)) {
+            fprintf(stderr, "STOP Success\n");
+        }
     }
 }
 
 void
-send_unbond(udp_transport_data_t *transport, pstop_os_env *env, const device_id_t *uuid)
+send_unbond(udp_transport_data_t *transport, pstop_os_env *env, machine_client_data_t *machine, const device_id_t *uuid)
 {
-    uint8_t reqbytes[PSTOP_MESSAGE_SIZE];
-
-    pstop_msg_t req_msg;
-    pstop_msg_t resp_msg;
-
-    req_msg.message = PSTOP_MESSAGE_UNBOND;
-    device_id_copy(&req_msg.id, uuid);
-
-    pstop_message_encode(&req_msg, reqbytes);
-
-    transport_udp_write(transport, reqbytes, PSTOP_MESSAGE_SIZE, NULL);
-
-    if(read_msg(transport, env, &resp_msg, 2000)) {
-        fprintf(stderr, "Received msg: %d\n", resp_msg.message);
+    if(send_msg(transport, env, machine, uuid, PSTOP_MESSAGE_UNBOND)) {
+        fprintf(stderr, "UNBOND Success\n");
     }
 }
 
@@ -125,31 +137,39 @@ main(int argc, char *argv[])
         return -1;
     }
 
-    uint8_t reqbytes[PSTOP_MESSAGE_SIZE];
-    uint8_t respbytes[PSTOP_MESSAGE_SIZE];
+    machine_client_data_t machine;
+    machine_client_init(&machine);
 
     pstop_os_env env;
     pstop_os_env_init(&env);
 
-    pstop_msg_t req_msg;
-    pstop_msg_t resp_msg;
+    device_id_t machine_uuid = {
+        .data = {
+            0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
+            0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0xFF
+        }
+    };
+    device_id_copy(&(machine.client_id), &machine_uuid);
 
-    device_id_t uuid = {
+    device_id_t this_uuid = {
         .data = {
             0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
             0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0xFF
         }
     };
-    uuid.data[15] = (uint8_t)(uuid_byte & 0xFFU);
+    this_uuid.data[15] = (uint8_t)(uuid_byte & 0xFFU);
+
+    fprintf(stderr, "uuid=%d\n", this_uuid.data[15]);
+    send_bond(&udp_transport, &env, &machine, &this_uuid);
 
-    fprintf(stderr, "uuid=%d\n", uuid.data[15]);
-    send_bond(&udp_transport, &env, &uuid);
+    sleep_ms(300);
 
-    for(int i = 0; i < 3; ++i) {
-        send_ok(&udp_transport, &env, &uuid, (i % 2));
+    for(int i = 0; i < 30; ++i) {
+        send_ok(&udp_transport, &env, &machine, &this_uuid, (i % 2));
+        sleep_ms(300);
     }
 
-    send_unbond(&udp_transport, &env, &uuid);
+    send_unbond(&udp_transport, &env, &machine, &this_uuid);
 
     return 0;
 }

pstop_c/examples/machine/machine_app.c

@@ -31,7 +31,7 @@ int
 robot_status(pstop_status_message_t status)
 {
     if(lastStatus != status) {
-        fprintf(stderr, "Status = %d\n", (int)status);
+        fprintf(stderr, "Robot Status = %d\n", (int)status);
         lastStatus = status;
     }
 
@@ -77,15 +77,20 @@ main(int argc, char *argv[])
 
             fprintf(stderr, "Got message: %d from %d\n", req_msg.message, req_msg.id.data[15]);
             pstop_error_t error = machine.handle_protocol_message_cb(&machine, &req_msg, &resp_msg_ptr);
+            if(error != PSTOP_OK) {
+                fprintf(stderr, "Invalid request: %d\n", (int)error);
+                continue;
+            }
             if(resp_msg_ptr != NULL) {
+                //fprintf(stderr, "Sending resp: counter=%d\n", resp_msg.counter);
                 pstop_message_encode(&resp_msg, respbytes);
                 transport_udp_write(&udp_transport, respbytes, PSTOP_MESSAGE_SIZE, (struct sockaddr_in *)&client);
             }
             else {
                 fprintf(stderr, "Invalid response: %d\n", (int)error);
             }
         }
-        //machine.check_heartbeats_cb(&machine);
+        machine.check_heartbeats_cb(&machine);
     }
 
     transport_udp_close(&udp_transport);

pstop_c/pstop/CMakeLists.txt

@@ -8,11 +8,12 @@ find_package(unity REQUIRED)
 add_library(pstop
   src/pstop/checksum.c
   src/pstop/device_id.c
+  src/pstop/machine_client_data.c
   src/pstop/machine.c
   src/pstop/os.c
   src/pstop/protocol.c
   src/pstop/pstop_application.c
-  src/pstop/pstop_client.c
+  src/pstop/pstop_client_data.c
   src/pstop/pstop_msg.c
   src/pstop/time.c
 )
@@ -25,7 +26,7 @@ add_executable(pstop_test
   test/src/pstop/device_id_test.c
   test/src/pstop/machine_test.c
   test/src/pstop/machine_timeout_test.c
-  test/src/pstop/pstop_client_test.c
+  test/src/pstop/pstop_client_data_test.c
   test/src/pstop/pstop_msg_test.c
 
   test/src/pstop/test_utils.c

pstop_c/pstop/include/pstop/error.h

@@ -15,6 +15,7 @@ typedef enum {
     PSTOP_MSG_DELAYED = 12,
     PSTOP_MSG_OUT_OF_ORDER = 13,
     PSTOP_MSG_INVALID_CHECKSUM = 14,
+    PSTOP_ERROR_INVALID_ID = 15,
 
     PSTOP_HEARTBEAT_INVALID = 20,
     PSTOP_MESSAGE_TYPE_INVALID = 21,

pstop_c/pstop/include/pstop/machine.h

@@ -7,7 +7,7 @@
 #include "pstop/pstop_msg.h"
 #include "pstop/error.h"
 #include "pstop/pstop_application.h"
-#include "pstop/pstop_client.h"
+#include "pstop/pstop_client_data.h"
 
 typedef struct pstop_machine_t pstop_machine_t;
 

pstop_c/pstop/include/pstop/machine_client_data.h

@@ -0,0 +1,30 @@
+// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc.
+// SPDX-License-Identifier: Apache-2.0
+
+#ifndef PSTOP_MACHINE_CLIENT_DATA_H
+#define PSTOP_MACHINE_CLIENT_DATA_H
+
+#include <stdint.h>
+
+#include "pstop/device_id.h"
+
+typedef struct {
+
+    device_id_t client_id;
+
+    // last time we've heard from this client
+    uint64_t last_timestamp;
+
+    // how frequently we should ping this client
+    uint64_t heartbeat_ms;
+
+    // the counter indicating each message we are sending
+    uint32_t msg_counter;
+
+    uint32_t last_counter;
+
+} machine_client_data_t;
+
+void machine_client_init(machine_client_data_t *client);
+
+#endif /* PSTOP_MACHINE_CLIENT_DATA_H */

pstop_c/pstop/include/pstop/pstop_client.h → pstop_c/pstop/include/pstop/pstop_client_data.h

@@ -1,8 +1,8 @@
 // SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc.
 // SPDX-License-Identifier: Apache-2.0
 
-#ifndef PSTOP_PSTOP_CLIENT_H
-#define PSTOP_PSTOP_CLIENT_H
+#ifndef PSTOP_PSTOP_CLIENT_DATA_H
+#define PSTOP_PSTOP_CLIENT_DATA_H
 
 #include <stdint.h>
 
@@ -87,4 +87,4 @@ void pstop_client_deactivate(pstop_client_data_t *client);
  */
 pstop_client_data_t *pstop_client_get(pstop_clients_t *clients, const device_id_t *client_id);
 
-#endif /* PSTOP_PSTOP_CLIENT_H */
+#endif /* PSTOP_PSTOP_CLIENT_DATA_H */

pstop_c/pstop/src/pstop/machine.c

@@ -7,7 +7,7 @@
 #include <stddef.h>
 
 #include "pstop/machine.h"
-#include "pstop/pstop_client.h"
+#include "pstop/pstop_client_data.h"
 #include "pstop/constants.h"
 #include "pstop/protocol.h"
 
@@ -282,6 +282,7 @@ machine_check_heartbeats(pstop_machine_t *machine)
     }
 
     if(needsStop != 0) {
+        fprintf(stderr, "Hearbeat failure!\n");
         machine_stop_robot(machine);
         return PSTOP_MISSED_HEARTBEATS;
     }

pstop_c/pstop/src/pstop/machine_client_data.c

@@ -0,0 +1,15 @@
+
+// SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc.
+// SPDX-License-Identifier: Apache-2.0
+
+#include "pstop/machine_client_data.h"
+
+void
+machine_client_init(machine_client_data_t *client)
+{
+    device_id_init(&(client->client_id));
+    client->last_timestamp = 0U;
+    client->heartbeat_ms = 0U;
+    client->msg_counter = 0U;
+    client->last_counter = 0U;
+}