Initial prototype PSTOP C implementation

pstop_c/CMakeLists.txt

@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 3.12)
+project(PSTOP C)
+
+add_subdirectory(pstop)
+

pstop_c/pstop/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.12)
+project(PSTOPCore C)
+
+find_package(unity REQUIRED)
+
+add_library(pstop
+  src/pstop/checksum.c
+  src/pstop/device_id.c
+  src/pstop/machine.c
+  src/pstop/protocol.c
+  src/pstop/pstop_application.c
+  src/pstop/pstop_client.c
+  src/pstop/pstop_msg.c
+  src/pstop/time.c
+)
+
+target_compile_options(pstop PRIVATE -Wall -fanalyzer)
+
+target_include_directories(pstop PUBLIC include)
+
+add_executable(pstop_test
+  test/src/pstop/device_id_test.c
+  test/src/pstop/machine_test.c
+  test/src/pstop/pstop_client_test.c
+
+  test/src/pstop/test_utils.c
+
+  test/src/pstop/main.c
+)
+
+target_include_directories(pstop_test PUBLIC
+  include
+  test/include
+)
+
+target_link_libraries(pstop_test PUBLIC
+  pstop
+  unity
+)

pstop_c/pstop/include/pstop/checksum.h

@@ -0,0 +1,9 @@
+#ifndef PSTOP_CHECKSUM_H
+#define PSTOP_CHECKSUM_H
+
+#include <stdint.h>
+#include <stddef.h>
+
+uint16_t checksum_crc16(const uint8_t *data, size_t data_length);
+
+#endif /* PSTOP_CHECKSUM_H */

pstop_c/pstop/include/pstop/constants.h

@@ -0,0 +1,12 @@
+#ifndef PSTOP_CONSTANTS_H
+#define PSTOP_CONSTANTS_H
+
+#define PSTOP_SECOND_MS 1000U
+#define PSTOP_MINUTE_MS (PSTOP_SECOND_MS * 60U)
+#define PSTOP_HOUR_MS (PSTOP_MINUTE_MS * 60U)
+#define PSTOP_DAY_MS (PSTOP_HOUR_MS * 24U)
+
+#define PSTOP_MIN_HEARTBEAT 10U
+#define PSTOP_MAX_HEARTBEAT (PSTOP_HOUR_MS * 1U)
+
+#endif /* PSTOP_CONSTANTS_H */

pstop_c/pstop/include/pstop/device_id.h

@@ -0,0 +1,29 @@
+#ifndef PSTOP_DEVICE_ID_H
+#define PSTOP_DEVICE_ID_H
+
+#include <stdint.h>
+
+#define DEVICE_ID_LENGTH 16
+
+typedef struct {
+    uint8_t data[DEVICE_ID_LENGTH];
+} device_id_t;
+
+/**
+ * Initializes the specified device to 0.
+ */
+void device_id_init(device_id_t *device_id);
+
+/**
+ * Copies the device ID from id to device_id.
+ */
+void device_id_copy(device_id_t *device_id, const device_id_t *id);
+
+/**
+ * Compares two device ids.
+ *
+ * @Return 0 if lhs == rhs. Otherwise returns non-zero.
+ */
+int device_id_cmp(const device_id_t *lhs, const device_id_t *rhs);
+
+#endif /* PSTOP_DEVICE_ID_H */

pstop_c/pstop/include/pstop/error.h

@@ -0,0 +1,24 @@
+#ifndef PSTOP_ERROR_H
+#define PSTOP_ERROR_H
+
+typedef enum {
+
+    PSTOP_OK = 0,
+    PSTOP_OPERATOR_NOT_ALLOWED = 1,
+    PSTOP_OUT_OF_OPERATOR_SPACE = 2,
+
+    PSTOP_MSG_LOST = 10,
+    PSTOP_MSG_REPETITION = 11,
+    PSTOP_MSG_DELAYED = 12,
+    PSTOP_MSG_OUT_OF_ORDER = 13,
+    PSTOP_MSG_INVALID_CHECKSUM = 14,
+
+    PSTOP_HEARTBEAT_INVALID = 20,
+    PSTOP_MESSAGE_TYPE_INVALID = 21,
+    PSTOP_MISSED_HEARTBEATS = 22,
+
+    PSTOP_FATAL = 255
+
+} pstop_error_t;
+
+#endif /* PSTOP_ERROR_H */

pstop_c/pstop/include/pstop/machine.h

@@ -0,0 +1,79 @@
+#ifndef PSTOP_MACHINE_H
+#define PSTOP_MACHINE_H
+
+#include "pstop/pstop_msg.h"
+#include "pstop/error.h"
+#include "pstop/pstop_application.h"
+#include "pstop/pstop_client.h"
+
+typedef struct pstop_machine_t pstop_machine_t;
+
+/**
+ * A function callback to handle protocol (black channel) messages.
+ */
+typedef pstop_error_t (* protocol_handle_message_t)(pstop_machine_t *machine, const pstop_msg_t *req, pstop_msg_t **resp);
+
+/**
+ * A function callback to handle new messages that have arrived.
+ *
+ * @Param machine The machine object
+ * @Param req     The request message
+ * @Param resp    A pointer to a response message. If NULL then no response is sent back.
+ */
+typedef pstop_error_t (* machine_handle_message_t)(pstop_machine_t *machine, const pstop_msg_t *req, pstop_msg_t **resp);
+
+/**
+ * A function callback that is called to check the heartbeats of the connected PSTOPs
+ *
+ * @Param machine The machine object
+ */
+typedef pstop_error_t (* pstop_check_heartbeats_t)(pstop_machine_t *machine);
+
+#define ROBOT_STATE_OK      0
+#define ROBOT_STATE_STOPPED 1
+
+#define ROBOT_RESTART_STATE_OK 0
+#define ROBOT_RESTART_STATE_NEED_STOP 1
+#define ROBOT_RESTART_STATE_STOP_RECEIVED 2
+
+typedef struct {
+
+    /* Is the robot stopped or OK? */
+    int robot_state;
+
+    /**
+     * The client ID that caused the robot to stop if stopped.
+     * 0 if robot is not stopped or if applicaton just started
+     * Or the client ID that has started the stop/ok cycle to get robot moving.
+     */
+    uint32_t client_stop_id;
+
+    /**
+     * 0 = everything is OK
+     * 1 = need stop message
+     * 2 = stop received
+     */
+    int restart_state;
+} robot_state_t;
+
+typedef struct pstop_machine_t {
+
+    protocol_handle_message_t handle_protocol_message_cb;
+
+    machine_handle_message_t handle_machine_message_cb;
+
+    pstop_check_heartbeats_t check_heartbeats_cb;
+
+    pstop_clients_t pstops;
+
+    pstop_application_t application;
+
+    robot_state_t robot_state;
+
+} pstop_machine_t;
+
+void machine_init(pstop_machine_t *machine, const pstop_application_t *app, pstop_client_data_t *clients, uint16_t max_clients);
+
+void machine_stop_robot(pstop_machine_t *machine, pstop_client_data_t *client);
+
+#endif /* PSTOP_MACHINE_H */

pstop_c/pstop/include/pstop/protocol.h

@@ -0,0 +1,10 @@
+#ifndef PSTOP_PROTOCOL_H
+#define PSTOP_PROTOCOL_H
+
+#include "pstop/error.h"
+#include "pstop/pstop_msg.h"
+#include "pstop/machine.h"
+
+pstop_error_t protocol_handle_message(pstop_machine_t *machine, const pstop_msg_t *req, pstop_msg_t **resp);
+
+#endif /* PSTOP_PROTOCOL_H */

pstop_c/pstop/include/pstop/pstop_application.h

@@ -0,0 +1,68 @@
+#ifndef PSTOP_PSTOP_APPLICATION_H
+#define PSTOP_PSTOP_APPLICATION_H
+
+#include <stdint.h>
+
+#include "pstop/error.h"
+#include "pstop/device_id.h"
+
+typedef enum {
+    PSTOP_STATUS_OK = 0,
+    PSTOP_STATUS_STOP = 1
+} pstop_status_message_t;
+
+typedef uint64_t (* get_current_time_t)(void);
+typedef int      (* is_operator_allowed_t)(const device_id_t *device_id);
+typedef int      (* pstop_status_t)(pstop_status_message_t status);
+
+typedef void     (* log_message_t)(pstop_error_t error, const char *message);
+
+typedef struct {
+    /**
+     * Default timeout for all new clients.
+     */
+    uint64_t default_timeout_ms;
+
+    uint16_t max_lost_messages;
+
+    uint16_t max_missed_heartbeats;
+
+} pstop_application_config_t;
+
+typedef struct pstop_application_t {
+
+    /**
+     * Callback to return the current time.
+     */
+    get_current_time_t get_time_cb;
+
+    /**
+     * The device ID for this machine
+     */
+    device_id_t machine_device_id;
+
+    /**
+     * Simple access control to determine if an operator
+     * is allowed to connect to this machine.
+     *
+     * @Return 1 if operator is allowed, 0 otherwise
+     */
+    is_operator_allowed_t operator_allowed_cb;
+
+    /**
+     * Notifies the underlying hardware about the status of this PSTOP.
+     */
+    pstop_status_t status_cb;
+
+    /**
+     * Callback to handle any errors in the system.
+     */
+    log_message_t log_message_cb;
+
+    pstop_application_config_t app_config;
+
+} pstop_application_t;
+
+void pstop_application_init(pstop_application_t *app);
+
+#endif /* PSTOP_PSTOP_APPLICATION_H */

pstop_c/pstop/include/pstop/pstop_client.h

@@ -0,0 +1,91 @@
+#ifndef PSTOP_PSTOP_CLIENT_H
+#define PSTOP_PSTOP_CLIENT_H
+
+#include <stdint.h>
+
+#include "pstop/device_id.h"
+#include "pstop/constants.h"
+
+typedef enum {
+
+    PSTOP_CLIENT_BONDED = 1,
+    PSTOP_CLIENT_UNBONDED = 2,
+    PSTOP_CLIENT_STOPPED = 3,
+    PSTOP_CLIENT_FAILURE = 4,
+    PSTOP_CLIENT_UNKNOWN = 255
+
+} pstop_client_state_t;
+
+typedef struct {
+
+    uint32_t local_client_id;
+
+    device_id_t client_id;
+
+    // last time we've heard from this client
+    uint64_t last_timestamp;
+
+    // last time we've heard from this client
+    uint64_t last_received_heartbeat;
+
+    // how frequently we should hear from this client
+    uint64_t heartbeat_ms;
+
+    // the counter indicating each message we are sending
+    uint32_t msg_counter;
+
+    uint32_t last_counter;
+
+    // approx how far off is the client's clock from this clock
+    // compare the incoming pstop_msg.stamp to this clock.
+    // Ignoring network transit time
+    int32_t clock_drift;
+
+    // how many messages have we lost so far?
+    uint16_t lost_message_counter;
+
+    // how many heartbeats have we missed?
+    uint16_t missed_heartbeats_counter;
+
+    pstop_client_state_t client_state;
+
+} pstop_client_data_t;
+
+typedef struct {
+
+    pstop_client_data_t *clients;
+    uint16_t max_clients;
+    uint16_t num_clients;
+
+} pstop_clients_t;
+
+/**
+ * Initialize the pstop_client_data_t structure to default values.
+ */
+void pstop_client_init(pstop_client_data_t *client);
+
+/**
+ * Initializes a pstop_clients object to default values.
+ */
+void pstop_clients_init(pstop_clients_t *clients);
+
+/**
+ * Returns an empty pstop_client if one is available
+ *
+ * @Return the client data if successful, NULL if no space available.
+ */
+pstop_client_data_t *pstop_client_get_free_client(pstop_clients_t *clients);
+
+/**
+ * Removes the specified pstop client by the device ID.
+ */
+void pstop_client_remove(pstop_clients_t *clients, const device_id_t *client_id);
+
+/**
+ * Finds the specified pstop client by device ID.
+ *
+ * @Return the requsted client if found or NULL if not found.
+ */
+pstop_client_data_t *pstop_client_get(pstop_clients_t *clients, const device_id_t *client_id);
+
+#endif /* PSTOP_PSTOP_CLIENT_H */