diff --git a/pstop_c/examples/client/client_app.c b/pstop_c/examples/client/client_app.c index e84a40f..0048cdc 100644 --- a/pstop_c/examples/client/client_app.c +++ b/pstop_c/examples/client/client_app.c @@ -6,13 +6,25 @@ #include #include +#include + #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; } diff --git a/pstop_c/examples/machine/machine_app.c b/pstop_c/examples/machine/machine_app.c index 1e83d69..c845d89 100644 --- a/pstop_c/examples/machine/machine_app.c +++ b/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,7 +77,12 @@ 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); } @@ -85,7 +90,7 @@ main(int argc, char *argv[]) fprintf(stderr, "Invalid response: %d\n", (int)error); } } - //machine.check_heartbeats_cb(&machine); + machine.check_heartbeats_cb(&machine); } transport_udp_close(&udp_transport); diff --git a/pstop_c/pstop/CMakeLists.txt b/pstop_c/pstop/CMakeLists.txt index 5d1032c..a3efde6 100644 --- a/pstop_c/pstop/CMakeLists.txt +++ b/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 diff --git a/pstop_c/pstop/include/pstop/error.h b/pstop_c/pstop/include/pstop/error.h index 62db677..cf41337 100644 --- a/pstop_c/pstop/include/pstop/error.h +++ b/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, diff --git a/pstop_c/pstop/include/pstop/machine.h b/pstop_c/pstop/include/pstop/machine.h index 7ae46ff..353816a 100644 --- a/pstop_c/pstop/include/pstop/machine.h +++ b/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; diff --git a/pstop_c/pstop/include/pstop/machine_client_data.h b/pstop_c/pstop/include/pstop/machine_client_data.h new file mode 100644 index 0000000..b21a81a --- /dev/null +++ b/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 + +#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 */ diff --git a/pstop_c/pstop/include/pstop/pstop_client.h b/pstop_c/pstop/include/pstop/pstop_client_data.h similarity index 95% rename from pstop_c/pstop/include/pstop/pstop_client.h rename to pstop_c/pstop/include/pstop/pstop_client_data.h index 96df13f..b2c2c51 100644 --- a/pstop_c/pstop/include/pstop/pstop_client.h +++ b/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 @@ -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 */ diff --git a/pstop_c/pstop/src/pstop/machine.c b/pstop_c/pstop/src/pstop/machine.c index d781226..a7522ba 100644 --- a/pstop_c/pstop/src/pstop/machine.c +++ b/pstop_c/pstop/src/pstop/machine.c @@ -7,7 +7,7 @@ #include #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; } diff --git a/pstop_c/pstop/src/pstop/machine_client_data.c b/pstop_c/pstop/src/pstop/machine_client_data.c new file mode 100644 index 0000000..c72f80b --- /dev/null +++ b/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; +} diff --git a/pstop_c/pstop/src/pstop/protocol.c b/pstop_c/pstop/src/pstop/protocol.c index a1b4ccd..e9196d6 100644 --- a/pstop_c/pstop/src/pstop/protocol.c +++ b/pstop_c/pstop/src/pstop/protocol.c @@ -2,34 +2,10 @@ // SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. // SPDX-License-Identifier: Apache-2.0 +#include #include #include "pstop/protocol.h" -#if 0 -static -void -init_response_from_request(const pstop_msg_t *req, pstop_msg_t *resp) -{ - device_id_copy(&(resp->receiver_id), &(req->id)); - device_id_copy(&(resp->id), &(req->receiver_id)); - resp->received_counter = req->counter; - resp->received_stamp = req->stamp; -} -#endif - -static -pstop_error_t -check_message_order(pstop_client_data_t *client, const pstop_msg_t *msg) -{ - if(client->last_timestamp >= msg->stamp) { - return PSTOP_MSG_OUT_OF_ORDER; - } - if(client->msg_counter >= msg->counter) { - return PSTOP_MSG_REPETITION; - } - - return PSTOP_OK; -} static int @@ -38,60 +14,45 @@ is_checksum_valid(const pstop_msg_t *req) return 1; } -static pstop_error_t -validate_message(pstop_machine_t *machine, pstop_client_data_t *client, const pstop_msg_t *req, pstop_msg_t **resp) +protocol_handle_message(pstop_machine_t *machine, const pstop_msg_t *req, pstop_msg_t **resp) { - // Check the message and ignore it if it's invalid - pstop_error_t result = check_message_order(client, req); - if(result != PSTOP_OK) { + // validate checksum + if(!is_checksum_valid(req)) { *resp = NULL; - return result; + return PSTOP_MSG_INVALID_CHECKSUM; } - // check for lost messages - if(req->counter != (client->msg_counter - 1U)) { - client->lost_message_counter++; - if(client->lost_message_counter >= machine->application->app_config.max_lost_messages) { - // too many lost messages - // clean up this client - *resp = NULL; - machine_stop_robot(machine); - return PSTOP_MSG_LOST; - } - } - else { - client->lost_message_counter = 0U; + // make sure te target ID is this machine ID + if(!device_id_cmp(&(machine->application->machine_device_id), &(req->receiver_id))) { + fprintf(stderr, "Invald receiver ID\n"); + *resp = NULL; + return PSTOP_ERROR_INVALID_ID; } - client->missed_heartbeats_counter = 0U; - - return PSTOP_OK; -} - -pstop_error_t -protocol_handle_message(pstop_machine_t *machine, const pstop_msg_t *req, pstop_msg_t **resp) -{ - // validate black channel bits - - if(!is_checksum_valid(req)) { + if(!machine->application->operator_allowed_cb(&(req->id))) { + fprintf(stderr, "Operator not allowed\n"); *resp = NULL; - return PSTOP_MSG_INVALID_CHECKSUM; + return PSTOP_OPERATOR_NOT_ALLOWED; } pstop_client_data_t *client = pstop_client_get(&(machine->pstops), &(req->id)); - if(client == NULL) { - // validate that this is a new client message - } - else { - // validate lost/out of order message - #if 0 - pstop_error_t result = validate_message(machine, client, req, resp); - if(result != PSTOP_OK) { - return result; + // if we've already seen this client, then validate the counter/timestamps + if(client != NULL) { + //fprintf(stderr, "Time = %ld <=> %ld\n", req->stamp, client->last_timestamp); + if(req->counter <= client->last_counter) { + *resp = NULL; + return PSTOP_MSG_OUT_OF_ORDER; + } + if((req->counter - client->last_counter) > machine->application->app_config.max_lost_messages) { + *resp = NULL; + return PSTOP_MSG_LOST; + } + if(req->stamp <= client->last_timestamp) { + *resp = NULL; + return PSTOP_MSG_OUT_OF_ORDER; } - #endif } // now send the message to the machine for pstop handling. @@ -105,14 +66,22 @@ protocol_handle_message(pstop_machine_t *machine, const pstop_msg_t *req, pstop_ if(resp != NULL) { // add in response black channel values client = pstop_client_get(&(machine->pstops), &(req->id)); - if(client == NULL) { - // problem. - return PSTOP_FATAL; - } + + uint64_t now = machine->application->env.get_time_cb(); // let's add response black channel values - client->msg_counter++; - client->last_timestamp = req->stamp; + (*resp)->counter = req->counter + 1U; + (*resp)->stamp = now; + (*resp)->received_counter = req->counter; + (*resp)->received_stamp = req->stamp; + device_id_copy(&(*resp)->id, &(machine->application->machine_device_id)); + device_id_copy(&(*resp)->receiver_id, &(req->id)); + + // null client will happen on unbond + if(client != NULL) { + client->last_counter = (*resp)->counter; + client->last_timestamp = now; + } return PSTOP_OK; } diff --git a/pstop_c/pstop/src/pstop/pstop_client.c b/pstop_c/pstop/src/pstop/pstop_client_data.c similarity index 98% rename from pstop_c/pstop/src/pstop/pstop_client.c rename to pstop_c/pstop/src/pstop/pstop_client_data.c index db2c2b8..a20aa78 100644 --- a/pstop_c/pstop/src/pstop/pstop_client.c +++ b/pstop_c/pstop/src/pstop/pstop_client_data.c @@ -5,7 +5,7 @@ #include #include -#include "pstop/pstop_client.h" +#include "pstop/pstop_client_data.h" static uint32_t next_client_id = 0U; diff --git a/pstop_c/pstop/test/src/pstop/pstop_client_test.c b/pstop_c/pstop/test/src/pstop/pstop_client_data_test.c similarity index 98% rename from pstop_c/pstop/test/src/pstop/pstop_client_test.c rename to pstop_c/pstop/test/src/pstop/pstop_client_data_test.c index 9d5b078..37187d9 100644 --- a/pstop_c/pstop/test/src/pstop/pstop_client_test.c +++ b/pstop_c/pstop/test/src/pstop/pstop_client_data_test.c @@ -2,7 +2,7 @@ // SPDX-FileCopyrightText: 2026 Polymath Robotics, Inc. // SPDX-License-Identifier: Apache-2.0 -#include "pstop/pstop_client.h" +#include "pstop/pstop_client_data.h" #include