hyperverse 0.0.1__py3-none-any.whl

hyperdrone/_vendor/rl-tools/include/rl_tools/inference/applications/l2f/l2f.h

@@ -0,0 +1,98 @@
+#include "../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_INFERENCE_APPLICATIONS_L2F_L2F_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_INFERENCE_APPLICATIONS_L2F_L2F_H
+
+#include "../../executor/executor.h"
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools::inference::applications{
+    namespace l2f{
+        template <typename T_TYPE_POLICY, typename T_TI, typename T_TIMESTAMP, T_TI T_ACTION_HISTORY_LENGTH, T_TI T_OUTPUT_DIM, typename T_POLICY, T_TIMESTAMP T_CONTROL_INTERVAL_INTERMEDIATE_NS, T_TIMESTAMP T_CONTROL_INTERVAL_NATIVE_NS, bool T_FORCE_SYNC_INTERMEDIATE=false, T_TI T_FORCE_SYNC_NATIVE=0, bool T_FORCE_SYNC_NATIVE_RUNTIME=false, typename T_WARNING_LEVELS=executor::WarningLevelsDefault<T_TYPE_POLICY>, bool T_DYNAMIC_ALLOCATION=true, typename T_STATUS_SPEC=executor::StatusSpecification<T_TYPE_POLICY>>
+        struct Specification{
+            using TYPE_POLICY = T_TYPE_POLICY;
+            using T = typename TYPE_POLICY::DEFAULT;
+            using STATUS_SPEC = T_STATUS_SPEC;
+            using TI = T_TI;
+            using TIMESTAMP = T_TIMESTAMP;
+            using POLICY = T_POLICY;
+            static constexpr T_TI ACTION_HISTORY_LENGTH = T_ACTION_HISTORY_LENGTH;
+            static constexpr T_TI OUTPUT_DIM = T_OUTPUT_DIM;
+            static constexpr bool DYNAMIC_ALLOCATION = T_DYNAMIC_ALLOCATION;
+            using EXECUTOR_SPEC = executor::Specification<TYPE_POLICY, TI, TIMESTAMP, POLICY, T_CONTROL_INTERVAL_INTERMEDIATE_NS, T_CONTROL_INTERVAL_NATIVE_NS, T_FORCE_SYNC_INTERMEDIATE, T_FORCE_SYNC_NATIVE, T_FORCE_SYNC_NATIVE_RUNTIME, T_WARNING_LEVELS, T_DYNAMIC_ALLOCATION, T_STATUS_SPEC>;
+        };
+        enum class ObservationComponentType : unsigned char {
+            POSITION = 0,
+            ORIENTATION_ROTATION_MATRIX = 1,
+            ORIENTATION_QUATERNION = 2,
+            LINEAR_VELOCITY = 3,
+            ANGULAR_VELOCITY = 4,
+            ANGULAR_VELOCITY_DELAYED = 5,
+            LINEAR_VELOCITY_DELAYED = 6,
+            ACTION_HISTORY = 7,
+            LINEAR_ACCELERATION_BODY_FRAME = 8,
+            LINEAR_VELOCITY_BODY_FRAME = 9,
+            ROTOR_SPEEDS = 10,
+        };
+        template <typename T_TI>
+        struct ObservationComponent{
+            using TI = T_TI;
+            ObservationComponentType type;
+            TI parameter;
+            TI offset;
+            TI dim;
+        };
+        template <typename T_TI>
+        struct ObservationLayout{
+            using TI = T_TI;
+            static constexpr TI MAX_COMPONENTS = 16;
+            ObservationComponent<TI> components[MAX_COMPONENTS];
+            TI component_count = 0;
+            TI total_dim = 0;
+            TI action_history_length = 0;
+        };
+        template <typename SPEC>
+        struct Observation{
+            using T = typename SPEC::T;
+            T position[3];
+            T orientation[4]; // Quaternion: w, x, y, z
+            T linear_velocity[3];
+            T angular_velocity[3];
+            T previous_action[4];
+            bool position_set = false;
+            bool orientation_set = false;
+            bool linear_velocity_set = false;
+            bool angular_velocity_set = false;
+            bool previous_action_set = false;
+        };
+        template <typename SPEC>
+        struct Action{
+            float action[SPEC::OUTPUT_DIM];
+        };
+    }
+    namespace l2f{
+        template <typename T_POLICY, typename T_T, typename T_TI, T_TI T_INPUT_DIM, T_TI T_OUTPUT_DIM, bool T_DA>
+        struct InputTensorType{
+            using type = Tensor<tensor::Specification<T_T, T_TI, tensor::Shape<T_TI, 1, T_INPUT_DIM>, T_DA>>;
+        };
+        template <typename T_POLICY, typename T_T, typename T_TI, T_TI T_INPUT_DIM, T_TI T_OUTPUT_DIM, bool T_DA>
+        struct OutputTensorType{
+            using type = Tensor<tensor::Specification<T_T, T_TI, tensor::Shape<T_TI, 1, T_OUTPUT_DIM>, T_DA>>;
+        };
+    }
+    template <typename SPEC>
+    struct L2F{
+        using TYPE_POLICY = typename SPEC::TYPE_POLICY;
+        using T = typename TYPE_POLICY::DEFAULT;
+        using TI = typename SPEC::TI;
+        using TIMESTAMP = typename SPEC::TIMESTAMP;
+        T action_history[SPEC::ACTION_HISTORY_LENGTH][SPEC::OUTPUT_DIM];
+        static constexpr TI INPUT_DIM = 18 + SPEC::OUTPUT_DIM * SPEC::ACTION_HISTORY_LENGTH;
+        typename l2f::InputTensorType<typename SPEC::POLICY, T, TI, INPUT_DIM, SPEC::OUTPUT_DIM, SPEC::DYNAMIC_ALLOCATION>::type input;
+        typename l2f::OutputTensorType<typename SPEC::POLICY, T, TI, INPUT_DIM, SPEC::OUTPUT_DIM, SPEC::DYNAMIC_ALLOCATION>::type output;
+        Executor<typename SPEC::EXECUTOR_SPEC> executor;
+        TI steps_since_original_control_step;
+        l2f::ObservationLayout<TI> observation_layout;
+    };
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/inference/applications/l2f/operations_dyn.h

@@ -0,0 +1,18 @@
+#include "../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_INFERENCE_APPLICATIONS_L2F_OPERATIONS_DYN_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_INFERENCE_APPLICATIONS_L2F_OPERATIONS_DYN_H
+
+#include "operations_generic.h"
+#include "../../../dyn/policy_adapter.h"
+#include "../../../dyn/operations_generic.h"
+
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools{
+    template <typename DEVICE, typename TI, typename OBS_TENSOR, typename ACTION_TENSOR, typename RNG, typename MODE>
+    RL_TOOLS_FUNCTION_PLACEMENT void evaluate_step(DEVICE& device, dyn::Layer<TI>& layer, OBS_TENSOR& observation, dyn::PolicyState<TI>& state, ACTION_TENSOR& action, dyn::PolicyBuffer<TI>& buffer, RNG&, MODE){
+        rl_tools::evaluate_step(device, layer, observation, state.inner, action, buffer.inner);
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/inference/applications/l2f/operations_generic.h

@@ -0,0 +1,268 @@
+#include "../../../version.h"
+#if (defined(RL_TOOLS_DISABLE_INCLUDE_GUARDS) || !defined(RL_TOOLS_INFERENCE_APPLICATIONS_L2F_OPERATIONS_GENERIC_H)) && (RL_TOOLS_USE_THIS_VERSION == 1)
+#pragma once
+#define RL_TOOLS_INFERENCE_APPLICATIONS_L2F_OPERATIONS_GENERIC_H
+
+#include "l2f.h"
+#include "../../executor/operations_generic.h"
+#include "../../../utils/string/operations_generic.h"
+#include "../../../dyn/tensor_operations_generic.h"
+RL_TOOLS_NAMESPACE_WRAPPER_START
+namespace rl_tools{
+    template <typename DEVICE, typename SPEC>
+    RL_TOOLS_FUNCTION_PLACEMENT void malloc(DEVICE& device, inference::applications::L2F<SPEC>& executor){
+        malloc(device, executor.input);
+        malloc(device, executor.output);
+        malloc(device, executor.executor);
+    }
+    template <typename DEVICE, typename SPEC>
+    RL_TOOLS_FUNCTION_PLACEMENT void free(DEVICE& device, inference::applications::L2F<SPEC>& executor){
+        free(device, executor.input);
+        free(device, executor.output);
+        free(device, executor.executor);
+    }
+    template <typename DEVICE, typename SPEC, typename POLICY, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT void reset(DEVICE& device, inference::applications::L2F<SPEC>& executor, POLICY& policy, RNG& rng){
+        using T = typename SPEC::T;
+        using TI = typename SPEC::TI;
+        constexpr T HOVERING_THROTTLE = 0.66;
+        for(TI step_i = 0; step_i < SPEC::ACTION_HISTORY_LENGTH; step_i++){
+            for(TI action_i = 0; action_i < SPEC::OUTPUT_DIM; action_i++){
+                executor.action_history[step_i][action_i] = HOVERING_THROTTLE * 2 - 1;
+            }
+        }
+        executor.steps_since_original_control_step = 0;
+        if(executor.observation_layout.component_count == 0){
+            auto& layout = executor.observation_layout;
+            layout.components[0] = {inference::applications::l2f::ObservationComponentType::POSITION, 0, 0, 3};
+            layout.components[1] = {inference::applications::l2f::ObservationComponentType::ORIENTATION_ROTATION_MATRIX, 0, 3, 9};
+            layout.components[2] = {inference::applications::l2f::ObservationComponentType::LINEAR_VELOCITY, 0, 12, 3};
+            layout.components[3] = {inference::applications::l2f::ObservationComponentType::ANGULAR_VELOCITY, 0, 15, 3};
+            layout.components[4] = {inference::applications::l2f::ObservationComponentType::ACTION_HISTORY, SPEC::ACTION_HISTORY_LENGTH, 18, SPEC::OUTPUT_DIM * SPEC::ACTION_HISTORY_LENGTH};
+            layout.component_count = 5;
+            layout.total_dim = 18 + SPEC::OUTPUT_DIM * SPEC::ACTION_HISTORY_LENGTH;
+            layout.action_history_length = SPEC::ACTION_HISTORY_LENGTH;
+        }
+        reset(device, executor.executor, policy, rng);
+    }
+    namespace inference::applications::l2f{
+        template <typename TI>
+        RL_TOOLS_FUNCTION_PLACEMENT bool extract_observation_from_meta(const char* meta_str, TI meta_len, char* obs_out, TI obs_out_capacity, TI& obs_out_len){
+            obs_out_len = 0;
+            bool in_string = false;
+            TI string_start = 0;
+            for(TI i = 0; i < meta_len; i++){
+                char c = meta_str[i];
+                if(in_string){
+                    if(c == '\\' && i + 1 < meta_len){ i++; continue; }
+                    if(c != '"') continue;
+                    in_string = false;
+                    TI string_len = i - string_start;
+                    TI pos = i + 1;
+                    while(pos < meta_len && meta_str[pos] == ' ') pos++;
+                    if(pos >= meta_len || meta_str[pos] != ':') continue;
+                    if(string_len != 11 || !utils::string::compare(meta_str + string_start, "observation", (TI)11)) continue;
+                    pos++;
+                    while(pos < meta_len && meta_str[pos] == ' ') pos++;
+                    if(pos >= meta_len || meta_str[pos] != '"') continue;
+                    pos++;
+                    TI val_start = pos;
+                    while(pos < meta_len){
+                        if(meta_str[pos] == '\\' && pos + 1 < meta_len){ pos += 2; continue; }
+                        if(meta_str[pos] == '"') break;
+                        pos++;
+                    }
+                    obs_out_len = pos - val_start;
+                    if(obs_out_len >= obs_out_capacity) return false;
+                    for(TI k = 0; k < obs_out_len; k++) obs_out[k] = meta_str[val_start + k];
+                    obs_out[obs_out_len] = '\0';
+                    return true;
+                } else {
+                    if(c == '"'){ in_string = true; string_start = i + 1; }
+                }
+            }
+            return false;
+        }
+        template <typename DEVICE, typename TI>
+        RL_TOOLS_FUNCTION_PLACEMENT bool parse_observation_string(DEVICE& device, const char* obs_str, TI obs_str_len, ObservationLayout<TI>& layout, TI output_dim){
+            layout.component_count = 0;
+            layout.total_dim = 0;
+            layout.action_history_length = 0;
+            TI token_start = 0;
+            for(TI i = 0; i <= obs_str_len; i++){
+                char c = (i < obs_str_len) ? obs_str[i] : '\0';
+                if(c == ','){
+                    utils::assert_exit(device, false, "inference::applications::l2f: multi-branch observations not supported");
+                    return false;
+                }
+                if(c != '.' && c != '\0') continue;
+                TI token_len = i - token_start;
+                if(token_len == 0){ token_start = i + 1; continue; }
+                if(layout.component_count >= ObservationLayout<TI>::MAX_COMPONENTS){
+                    utils::assert_exit(device, false, "inference::applications::l2f: too many observation components");
+                    return false;
+                }
+                const char* token = obs_str + token_start;
+                TI name_len = token_len;
+                TI param = 0;
+                for(TI j = 0; j < token_len; j++){
+                    if(token[j] == '('){
+                        name_len = j;
+                        param = utils::string::string_to_int<TI>(token + j + 1, token_len - j - 1);
+                        break;
+                    }
+                }
+                auto& comp = layout.components[layout.component_count];
+                comp.parameter = param;
+                comp.offset = layout.total_dim;
+                bool matched = false;
+                if(name_len == 8 && utils::string::compare(token, "Position", name_len)){
+                    comp.type = ObservationComponentType::POSITION; comp.dim = 3; matched = true;
+                } else if(name_len == 25 && utils::string::compare(token, "OrientationRotationMatrix", name_len)){
+                    comp.type = ObservationComponentType::ORIENTATION_ROTATION_MATRIX; comp.dim = 9; matched = true;
+                } else if(name_len == 21 && utils::string::compare(token, "OrientationQuaternion", name_len)){
+                    comp.type = ObservationComponentType::ORIENTATION_QUATERNION; comp.dim = 4; matched = true;
+                } else if(name_len == 14 && utils::string::compare(token, "LinearVelocity", name_len)){
+                    comp.type = ObservationComponentType::LINEAR_VELOCITY; comp.dim = 3; matched = true;
+                } else if(name_len == 15 && utils::string::compare(token, "AngularVelocity", name_len)){
+                    comp.type = ObservationComponentType::ANGULAR_VELOCITY; comp.dim = 3; matched = true;
+                } else if(name_len == 22 && utils::string::compare(token, "AngularVelocityDelayed", name_len)){
+                    comp.type = ObservationComponentType::ANGULAR_VELOCITY_DELAYED; comp.dim = 3; matched = true;
+                } else if(name_len == 21 && utils::string::compare(token, "LinearVelocityDelayed", name_len)){
+                    comp.type = ObservationComponentType::LINEAR_VELOCITY_DELAYED; comp.dim = 3; matched = true;
+                } else if(name_len == 13 && utils::string::compare(token, "ActionHistory", name_len)){
+                    comp.type = ObservationComponentType::ACTION_HISTORY; comp.dim = output_dim * param; layout.action_history_length = param; matched = true;
+                } else if(name_len == 27 && utils::string::compare(token, "LinearAccelerationBodyFrame", name_len)){
+                    comp.type = ObservationComponentType::LINEAR_ACCELERATION_BODY_FRAME; comp.dim = 3; matched = true;
+                } else if(name_len == 23 && utils::string::compare(token, "LinearVelocityBodyFrame", name_len)){
+                    comp.type = ObservationComponentType::LINEAR_VELOCITY_BODY_FRAME; comp.dim = 3; matched = true;
+                } else if(name_len == 11 && utils::string::compare(token, "RotorSpeeds", name_len)){
+                    comp.type = ObservationComponentType::ROTOR_SPEEDS; comp.dim = output_dim; matched = true;
+                }
+                if(!matched){
+                    utils::assert_exit(device, false, "inference::applications::l2f: unknown observation component");
+                    return false;
+                }
+                layout.total_dim += comp.dim;
+                layout.component_count++;
+                token_start = i + 1;
+            }
+            return layout.component_count > 0;
+        }
+        template <typename DEVICE, typename TI>
+        RL_TOOLS_FUNCTION_PLACEMENT bool validate_observation_layout(DEVICE& device, const ObservationLayout<TI>& layout, TI action_history_capacity){
+            if(layout.component_count == 0) return false;
+            if(layout.action_history_length > action_history_capacity){
+                utils::assert_exit(device, false, "inference::applications::l2f: action history length exceeds capacity");
+                return false;
+            }
+            for(TI i = 0; i < layout.component_count; i++){
+                auto& comp = layout.components[i];
+                switch(comp.type){
+                    case ObservationComponentType::ANGULAR_VELOCITY_DELAYED:
+                    case ObservationComponentType::LINEAR_VELOCITY_DELAYED:
+                        if(comp.parameter > 0){
+                            utils::assert_exit(device, false, "inference::applications::l2f: delayed observations with delay > 0 not supported");
+                            return false;
+                        }
+                        break;
+                    case ObservationComponentType::LINEAR_ACCELERATION_BODY_FRAME:
+                    case ObservationComponentType::LINEAR_VELOCITY_BODY_FRAME:
+                    case ObservationComponentType::ROTOR_SPEEDS:
+                        utils::assert_exit(device, false, "inference::applications::l2f: unsupported observation component");
+                        return false;
+                    default:
+                        break;
+                }
+            }
+            return true;
+        }
+        template <typename DEVICE, typename SPEC, typename OBS_TENSOR>
+        RL_TOOLS_FUNCTION_PLACEMENT bool observe(DEVICE& device, L2F<SPEC>& executor, Observation<SPEC>& observation, OBS_TENSOR& observation_flat){
+            using TI = typename SPEC::TI;
+            auto& layout = executor.observation_layout;
+            for(TI comp_i = 0; comp_i < layout.component_count; comp_i++){
+                auto& comp = layout.components[comp_i];
+                switch(comp.type){
+                    case ObservationComponentType::POSITION:
+                        if(!observation.position_set) return false;
+                        for(TI i = 0; i < 3; i++) rl_tools::set(device, observation_flat, observation.position[i], 0, comp.offset + i);
+                        break;
+                    case ObservationComponentType::ORIENTATION_ROTATION_MATRIX:{
+                        if(!observation.orientation_set) return false;
+                        float qw = observation.orientation[0], qx = observation.orientation[1], qy = observation.orientation[2], qz = observation.orientation[3];
+                        rl_tools::set(device, observation_flat, (1 - 2*qy*qy - 2*qz*qz), 0, comp.offset + 0);
+                        rl_tools::set(device, observation_flat, (    2*qx*qy - 2*qw*qz), 0, comp.offset + 1);
+                        rl_tools::set(device, observation_flat, (    2*qx*qz + 2*qw*qy), 0, comp.offset + 2);
+                        rl_tools::set(device, observation_flat, (    2*qx*qy + 2*qw*qz), 0, comp.offset + 3);
+                        rl_tools::set(device, observation_flat, (1 - 2*qx*qx - 2*qz*qz), 0, comp.offset + 4);
+                        rl_tools::set(device, observation_flat, (    2*qy*qz - 2*qw*qx), 0, comp.offset + 5);
+                        rl_tools::set(device, observation_flat, (    2*qx*qz - 2*qw*qy), 0, comp.offset + 6);
+                        rl_tools::set(device, observation_flat, (    2*qy*qz + 2*qw*qx), 0, comp.offset + 7);
+                        rl_tools::set(device, observation_flat, (1 - 2*qx*qx - 2*qy*qy), 0, comp.offset + 8);
+                        break;
+                    }
+                    case ObservationComponentType::ORIENTATION_QUATERNION:
+                        if(!observation.orientation_set) return false;
+                        for(TI i = 0; i < 4; i++) rl_tools::set(device, observation_flat, observation.orientation[i], 0, comp.offset + i);
+                        break;
+                    case ObservationComponentType::LINEAR_VELOCITY:
+                    case ObservationComponentType::LINEAR_VELOCITY_DELAYED:
+                        if(!observation.linear_velocity_set) return false;
+                        for(TI i = 0; i < 3; i++) rl_tools::set(device, observation_flat, observation.linear_velocity[i], 0, comp.offset + i);
+                        break;
+                    case ObservationComponentType::ANGULAR_VELOCITY:
+                    case ObservationComponentType::ANGULAR_VELOCITY_DELAYED:
+                        if(!observation.angular_velocity_set) return false;
+                        for(TI i = 0; i < 3; i++) rl_tools::set(device, observation_flat, observation.angular_velocity[i], 0, comp.offset + i);
+                        break;
+                    case ObservationComponentType::ACTION_HISTORY:
+                        for(TI step_i = 0; step_i < comp.parameter; step_i++){
+                            for(TI action_i = 0; action_i < SPEC::OUTPUT_DIM; action_i++){
+                                rl_tools::set(device, observation_flat, executor.action_history[step_i][action_i], 0, comp.offset + step_i * SPEC::OUTPUT_DIM + action_i);
+                            }
+                        }
+                        break;
+                    default:
+                        return false;
+                }
+            }
+            return true;
+        }
+    }
+    template <typename DEVICE, typename SPEC, typename POLICY, typename RNG>
+    RL_TOOLS_FUNCTION_PLACEMENT auto control(DEVICE& device, inference::applications::L2F<SPEC>& executor, typename SPEC::TIMESTAMP nanoseconds, POLICY& policy, inference::applications::l2f::Observation<SPEC>& observation, inference::applications::l2f::Action<SPEC>& action, RNG& rng){
+        using TI = typename SPEC::TI;
+        if(executor.steps_since_original_control_step == 0){
+            for(TI action_i = 0; action_i < SPEC::OUTPUT_DIM; action_i++){
+                executor.action_history[0][action_i] = observation.previous_action[action_i];
+            }
+        }
+        else{
+            for(TI action_i = 0; action_i < SPEC::OUTPUT_DIM; action_i++){
+                executor.action_history[0][action_i] = (executor.action_history[0][action_i] * executor.steps_since_original_control_step + observation.previous_action[action_i]) / (executor.steps_since_original_control_step + 1);
+            }
+        }
+        inference::applications::l2f::observe(device, executor, observation, executor.input);
+        auto status = control(device, executor.executor, nanoseconds, policy, executor.input, executor.output, rng);
+        for (TI output_i=0; output_i < SPEC::OUTPUT_DIM; output_i++){
+            action.action[output_i] = get(device, executor.output, 0, output_i);
+        }
+
+        executor.steps_since_original_control_step++;
+        if(status.source == decltype(status.source)::CONTROL){
+            if(status.step_type == decltype(status.step_type)::NATIVE){
+                static_assert(SPEC::ACTION_HISTORY_LENGTH >= 1);
+                for(TI step_i = SPEC::ACTION_HISTORY_LENGTH-1; step_i > 0; step_i--){
+                    for(TI action_i = 0; action_i < SPEC::OUTPUT_DIM; action_i++){
+                        executor.action_history[step_i][action_i] = executor.action_history[step_i-1][action_i];
+                    }
+                }
+                executor.steps_since_original_control_step = 0;
+            }
+        }
+        return status;
+    }
+}
+RL_TOOLS_NAMESPACE_WRAPPER_END
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/inference/debugging_pool/c_backend.h

@@ -0,0 +1,31 @@
+#ifdef RL_TOOLS_ENABLE_DEBUGGING_POOL
+char rl_tools_debugging_pool_names[RL_TOOLS_DEBUGGING_POOL_NUMBER][RL_TOOLS_DEBUGGING_POOL_NAME_LENGTH];
+float rl_tools_debugging_pool[RL_TOOLS_DEBUGGING_POOL_NUMBER][RL_TOOLS_DEBUGGING_POOL_SIZE];
+uint64_t rl_tools_debugging_pool_indices[RL_TOOLS_DEBUGGING_POOL_NUMBER];
+uint64_t rl_tools_debugging_pool_index = 0;
+bool rl_tools_debugging_pool_locked = false;
+bool rl_tools_debugging_pool_updated = false;
+#endif
+
+static void rl_tools_reset_debuging_pool(){
+    if(!rl_tools_debugging_pool_locked){
+        rl_tools_debugging_pool_index = 0;
+        rl_tools_debugging_pool_updated = true;
+    }
+}
+static void rl_tools_add_to_debuging_pool(const char* name, const float* values, uint64_t num){
+    if(!rl_tools_debugging_pool_locked && rl_tools_debugging_pool_index < RL_TOOLS_DEBUGGING_POOL_NUMBER){
+        uint32_t size = (num < RL_TOOLS_DEBUGGING_POOL_SIZE ? num : RL_TOOLS_DEBUGGING_POOL_SIZE);
+        for(uint64_t i = 0; i < size; i++){
+            rl_tools_debugging_pool[rl_tools_debugging_pool_index][i] = values[i];
+        }
+        if(portable_strlen(name) < RL_TOOLS_DEBUGGING_POOL_NAME_LENGTH){
+            portable_strcpy(rl_tools_debugging_pool_names[rl_tools_debugging_pool_index], name);
+        } else {
+            portable_strcpy(rl_tools_debugging_pool_names[rl_tools_debugging_pool_index], "name too long");
+        }
+        rl_tools_debugging_pool_indices[rl_tools_debugging_pool_index] = size;
+        rl_tools_debugging_pool_index++;
+        rl_tools_debugging_pool_updated = true;
+    }
+}

hyperdrone/_vendor/rl-tools/include/rl_tools/inference/debugging_pool/c_interface.h

@@ -0,0 +1,12 @@
+#define RL_TOOLS_ENABLE_DEBUGGING_POOL
+#ifdef RL_TOOLS_ENABLE_DEBUGGING_POOL
+#define RL_TOOLS_DEBUGGING_POOL_NUMBER 10
+#define RL_TOOLS_DEBUGGING_POOL_SIZE 10
+#define RL_TOOLS_DEBUGGING_POOL_NAME_LENGTH 30
+extern float rl_tools_debugging_pool[RL_TOOLS_DEBUGGING_POOL_NUMBER][RL_TOOLS_DEBUGGING_POOL_SIZE];
+extern char rl_tools_debugging_pool_names[RL_TOOLS_DEBUGGING_POOL_NUMBER][RL_TOOLS_DEBUGGING_POOL_NAME_LENGTH];
+extern uint64_t rl_tools_debugging_pool_indices[RL_TOOLS_DEBUGGING_POOL_NUMBER];
+extern uint64_t rl_tools_debugging_pool_index;
+extern bool rl_tools_debugging_pool_locked;
+extern bool rl_tools_debugging_pool_updated;
+#endif

hyperdrone/_vendor/rl-tools/include/rl_tools/inference/executor/c_backend.h

@@ -0,0 +1,98 @@
+#include <stdio.h>
+#include "executor.h"
+#include "c_interface.h"
+#include "helper.h"
+
+static int portable_strlen(const char* str) {
+    const char* ptr = str;
+    while (*ptr != '\0') {
+        ptr++;
+    }
+    return ptr - str;
+}
+
+static char * portable_strcpy(char *dest, const char *src) {
+    char *original_dest = dest;
+    while ((*dest++ = *src++) != '\0');
+    return original_dest;
+}
+
+static void append(char* target, int target_size, const char* message, int &position){
+    if(position + portable_strlen(message) < target_size){
+        portable_strcpy(target + position, message);
+        position += portable_strlen(message);
+    }
+}
+
+
+void rl_tools_inference_executor_status_message(RLtoolsInferenceExecutorStatus status, char* target, int target_size){
+    int position = 0;
+    if(status.OK){
+        append(target, target_size, "OK", position);
+        if(status.source == RL_TOOLS_INFERENCE_EXECUTOR_STATUS_SOURCE_CONTROL){
+            append(target, target_size, " CONTROL", position);
+            if(status.step_type == RL_TOOLS_INFERENCE_EXECUTOR_STATUS_STEP_TYPE_INTERMEDIATE){
+                append(target, target_size, " INTERMEDIATE", position);
+            }
+            else{
+                append(target, target_size, " NATIVE", position);
+            }
+        }
+    }
+    else{
+        append(target, target_size, "PROBLEM", position);
+        if(status.source == RL_TOOLS_INFERENCE_EXECUTOR_STATUS_SOURCE_CONTROL){
+            append(target, target_size, " CONTROL", position);
+            if(status.step_type == RL_TOOLS_INFERENCE_EXECUTOR_STATUS_STEP_TYPE_INTERMEDIATE){
+                append(target, target_size, " INTERMEDIATE", position);
+            }
+            else{
+                append(target, target_size, " NATIVE", position);
+            }
+        }
+        else{
+            append(target, target_size, " UNKNOWN", position);
+        }
+        if(status.TIMESTAMP_INVALID){
+            append(target, target_size, " TIMESTAMP_INVALID", position);
+        }
+        else{
+            if(!status.timing_bias.OK || !status.timing_jitter.OK){
+                append(target, target_size, " TIMING_ISSUE", position);
+                if(!status.timing_jitter.OK){
+                    append(target, target_size, " JITTER ", position);
+                    char buffer[50];
+                    rl_tools_inference_executor_float_to_str(status.timing_jitter.MAGNITUDE, buffer, 50);
+                    append(target, target_size, buffer, position);
+                }
+                if(!status.timing_bias.OK){
+                    append(target, target_size, " BIAS ", position);
+                    char buffer[50];
+                    rl_tools_inference_executor_float_to_str(status.timing_bias.MAGNITUDE, buffer, 50);
+                    append(target, target_size, buffer, position);
+                }
+            }
+            else{
+                append(target, target_size, " UNKNOWN", position);
+            }
+        }
+
+    }
+}
+
+namespace rl_tools{
+    template <typename SPEC>
+    RLtoolsInferenceExecutorStatus convert(const inference::executor::Status<SPEC>& status){
+        RLtoolsInferenceExecutorStatus output;
+        output.OK = status.OK;
+        output.TIMESTAMP_INVALID = status.TIMESTAMP_INVALID;
+        output.LAST_CONTROL_TIMESTAMP_GREATER_THAN_LAST_OBSERVATION_TIMESTAMP = status.LAST_CONTROL_TIMESTAMP_GREATER_THAN_LAST_OBSERVATION_TIMESTAMP;
+        output.source = status.source == inference::executor::Status<SPEC>::OBSERVATION ? RL_TOOLS_INFERENCE_EXECUTOR_STATUS_SOURCE_OBSERVATION : RL_TOOLS_INFERENCE_EXECUTOR_STATUS_SOURCE_CONTROL;
+        output.step_type = status.step_type == inference::executor::Status<SPEC>::INTERMEDIATE ? RL_TOOLS_INFERENCE_EXECUTOR_STATUS_STEP_TYPE_INTERMEDIATE : RL_TOOLS_INFERENCE_EXECUTOR_STATUS_STEP_TYPE_NATIVE;
+        output.timing_jitter.OK = status.timing_jitter.OK;
+        output.timing_jitter.MAGNITUDE = status.timing_jitter.MAGNITUDE;
+        output.timing_bias.OK = status.timing_bias.OK;
+        output.timing_bias.MAGNITUDE = status.timing_bias.MAGNITUDE;
+        return output;
+    }
+}

hyperdrone/_vendor/rl-tools/include/rl_tools/inference/executor/c_interface.h

@@ -0,0 +1,50 @@
+#ifndef RL_TOOLS_INFERENCE_EXECUTOR_C_INTERFACE_H
+#define RL_TOOLS_INFERENCE_EXECUTOR_C_INTERFACE_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+    typedef uint64_t RLtoolsInferenceTimestamp;
+    // The following structs are generated by copying them from ../../executor/executor.h, removing all templates and replacing all float types "T" with float;
+    typedef struct{
+        bool OK:1;
+        float MAGNITUDE;
+    } RLtoolsInferenceExecutorJitterStatus;
+    typedef struct{
+        bool OK:1;
+        float MAGNITUDE;
+    } RLtoolsInferenceExecutorBiasStatus;
+    typedef enum{
+        RL_TOOLS_INFERENCE_EXECUTOR_STATUS_SOURCE_OBSERVATION,
+        RL_TOOLS_INFERENCE_EXECUTOR_STATUS_SOURCE_CONTROL
+    } RLtoolsInferenceExecutorStatusSource;
+    typedef enum{
+        RL_TOOLS_INFERENCE_EXECUTOR_STATUS_STEP_TYPE_INTERMEDIATE,
+        RL_TOOLS_INFERENCE_EXECUTOR_STATUS_STEP_TYPE_NATIVE
+    } RLtoolsInferenceExecutorStatusStepType;
+    typedef struct{
+        bool reset:1;
+        bool time_diff:1;
+        bool force_sync:1;
+    } RLtoolsInferenceExecutorControlReasons;
+    typedef struct {
+        bool OK:1;
+        bool TIMESTAMP_INVALID:1;
+        bool LAST_CONTROL_TIMESTAMP_GREATER_THAN_LAST_OBSERVATION_TIMESTAMP:1;
+        RLtoolsInferenceExecutorStatusSource source:1;
+        RLtoolsInferenceExecutorStatusStepType step_type:1;
+        RLtoolsInferenceExecutorControlReasons control_reasons_intermediate;
+        RLtoolsInferenceExecutorControlReasons control_reasons_native;
+        RLtoolsInferenceExecutorJitterStatus timing_jitter;
+        RLtoolsInferenceExecutorBiasStatus timing_bias;
+    } RLtoolsInferenceExecutorStatus;
+    void rl_tools_inference_executor_status_message(RLtoolsInferenceExecutorStatus status, char* target, int target_size);
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif