From 16711182995cb44be8d0ce52b22105babad2f976 Mon Sep 17 00:00:00 2001 From: Alexander Shtuchkin Date: Sun, 19 Mar 2017 05:06:55 -0700 Subject: [PATCH] Added new platform: Particle Photon board. --- .gitignore | 3 + CMakeLists.txt | 76 ++----- cmake-variants.json | 27 ++- {src => include}/cycle_phase_classifier.h | 2 +- {src => include}/data_frame_decoder.h | 2 +- {src => include}/debug_node.h | 6 +- {src => include}/formatters.h | 8 +- {src => include}/geometry.h | 12 +- {src => include}/input.h | 13 +- {src => include}/led_state.h | 0 {src => include}/messages.h | 0 {src => include}/outputs.h | 26 +-- {src => include}/primitives/circular_buffer.h | 0 {src => include}/primitives/float16.h | 0 {src => include}/primitives/hash.h | 0 .../primitives/producer_consumer.h | 0 include/primitives/static_registration.h | 36 ++++ {src => include}/primitives/static_utils.h | 0 {src => include}/primitives/string_utils.h | 16 +- {src => include}/primitives/timestamp.h | 5 +- {src => include}/primitives/vector.h | 0 {src => include}/primitives/workers.h | 31 ++- {src => include}/print_helpers.h | 28 +-- {src => include}/pulse_processor.h | 2 +- {src => include}/settings.h | 13 +- {src => include}/vive_sensors_pipeline.h | 5 +- platform-particle/CMakeLists.txt | 65 ++++++ platform-particle/build.mk | 4 + platform-particle/input_tim.cpp | 198 ++++++++++++++++++ platform-particle/input_tim.h | 26 +++ platform-particle/main.cpp | 27 +++ platform-particle/notes.md | 120 +++++++++++ platform-particle/output_serial.cpp | 52 +++++ platform-particle/output_serial.h | 28 +++ platform-particle/platform.cpp | 87 ++++++++ platform-particle/stm32f2xx.toolchain.cmake | 54 +++++ platform-teensy/CMakeLists.txt | 18 ++ {src => platform-teensy}/input_cmp.cpp | 12 +- {src => platform-teensy}/input_cmp.h | 4 +- {src => platform-teensy}/led_state.cpp | 0 platform-teensy/main.cpp | 16 ++ platform-teensy/output_serial.cpp | 52 +++++ platform-teensy/output_serial.h | 28 +++ {src => platform-teensy}/platform.cpp | 59 +++++- platform-teensy/teensy-arm.toolchain.cmake | 57 +++++ platform-teensy/teensy-targets.cmake | 83 ++++++++ platform-teensy/timestamp.cpp | 23 ++ src/CMakeLists.txt | 24 +++ src/cycle_phase_classifier.cpp | 3 +- src/data_frame_decoder.cpp | 2 +- src/debug_node.cpp | 32 +-- src/formatters.cpp | 10 +- src/geometry.cpp | 12 +- src/input.cpp | 25 +-- src/main.cpp | 22 -- src/mavlink.cpp | 12 +- src/message_logging.h | 23 +- src/outputs.cpp | 44 +--- src/platform.h | 10 - src/primitives/string_utils.cpp | 30 ++- src/primitives/timestamp.cpp | 23 +- src/pulse_processor.cpp | 2 +- src/settings.cpp | 101 +++++---- src/ublox.cpp | 4 +- src/vive_sensors_pipeline.cpp | 1 - teensy-arm.toolchain.cmake | 162 -------------- test/32bit.toolchain.cmake | 10 + test/CMakeLists.txt | 21 ++ test/platform_mocks.cpp | 30 +-- 69 files changed, 1366 insertions(+), 561 deletions(-) rename {src => include}/cycle_phase_classifier.h (97%) rename {src => include}/data_frame_decoder.h (97%) rename {src => include}/debug_node.h (81%) rename {src => include}/formatters.h (91%) rename {src => include}/geometry.h (87%) rename {src => include}/input.h (79%) rename {src => include}/led_state.h (100%) rename {src => include}/messages.h (100%) rename {src => include}/outputs.h (66%) rename {src => include}/primitives/circular_buffer.h (100%) rename {src => include}/primitives/float16.h (100%) rename {src => include}/primitives/hash.h (100%) rename {src => include}/primitives/producer_consumer.h (100%) create mode 100644 include/primitives/static_registration.h rename {src => include}/primitives/static_utils.h (100%) rename {src => include}/primitives/string_utils.h (84%) rename {src => include}/primitives/timestamp.h (97%) rename {src => include}/primitives/vector.h (100%) rename {src => include}/primitives/workers.h (78%) rename {src => include}/print_helpers.h (73%) rename {src => include}/pulse_processor.h (97%) rename {src => include}/settings.h (79%) rename {src => include}/vive_sensors_pipeline.h (79%) create mode 100644 platform-particle/CMakeLists.txt create mode 100644 platform-particle/build.mk create mode 100644 platform-particle/input_tim.cpp create mode 100644 platform-particle/input_tim.h create mode 100644 platform-particle/main.cpp create mode 100644 platform-particle/notes.md create mode 100644 platform-particle/output_serial.cpp create mode 100644 platform-particle/output_serial.h create mode 100644 platform-particle/platform.cpp create mode 100644 platform-particle/stm32f2xx.toolchain.cmake create mode 100644 platform-teensy/CMakeLists.txt rename {src => platform-teensy}/input_cmp.cpp (96%) rename {src => platform-teensy}/input_cmp.h (87%) rename {src => platform-teensy}/led_state.cpp (100%) create mode 100644 platform-teensy/main.cpp create mode 100644 platform-teensy/output_serial.cpp create mode 100644 platform-teensy/output_serial.h rename {src => platform-teensy}/platform.cpp (77%) create mode 100644 platform-teensy/teensy-arm.toolchain.cmake create mode 100644 platform-teensy/teensy-targets.cmake create mode 100644 platform-teensy/timestamp.cpp create mode 100644 src/CMakeLists.txt delete mode 100644 src/main.cpp delete mode 100644 src/platform.h delete mode 100644 teensy-arm.toolchain.cmake create mode 100644 test/32bit.toolchain.cmake create mode 100644 test/CMakeLists.txt diff --git a/.gitignore b/.gitignore index 4a24823..e7bfec0 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,6 @@ # CMake build build/* + +# External lib files +libs/particle-firmware diff --git a/CMakeLists.txt b/CMakeLists.txt index d4077fe..a0be18f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,70 +1,26 @@ cmake_minimum_required(VERSION 3.2) -set(CMAKE_DISABLE_SOURCE_CHANGES ON) -#set(CMAKE_VERBOSE_MAKEFILE TRUE) -set(CMAKE_CXX_STANDARD 14) - -set(SOURCE_FILES - src/cycle_phase_classifier.cpp - src/data_frame_decoder.cpp - src/formatters.cpp - src/geometry.cpp - src/input.cpp - src/mavlink.cpp - src/pulse_processor.cpp - src/primitives/string_utils.cpp -) - -set(PLATFORM_SPECIFIC_SOURCE_FILES - src/main.cpp - src/settings.cpp - src/debug_node.cpp - src/input_cmp.cpp - src/led_state.cpp - src/outputs.cpp - src/platform.cpp - src/vive_sensors_pipeline.cpp - src/primitives/timestamp.cpp -) - -set(TEST_SOURCE_FILES - test/main_test.cpp - test/platform_mocks.cpp - test/test_pulse_processor.cpp -) - -if (NOT TEST_MODE) - set(CMAKE_TOOLCHAIN_FILE "teensy-arm.toolchain.cmake") - +if (${PLATFORM} MATCHES "Teensy") + set(CMAKE_TOOLCHAIN_FILE "platform-teensy/teensy-arm.toolchain.cmake") project(vive-diy-position-sensor) + add_subdirectory(platform-teensy) + add_subdirectory(src) - add_executable(vive-diy-position-sensor "${SOURCE_FILES}" "${PLATFORM_SPECIFIC_SOURCE_FILES}") - add_firmware_targets(vive-diy-position-sensor) - - import_cmsis_dsp_library(vive-diy-position-sensor) - import_arduino_library(vive-diy-position-sensor mavlink_v2) +elseif (${PLATFORM} MATCHES "Particle") + set(CMAKE_TOOLCHAIN_FILE "platform-particle/stm32f2xx.toolchain.cmake") + project(vive-diy-position-sensor) + add_subdirectory(platform-particle) + add_subdirectory(src) -else() +elseif (${PLATFORM} MATCHES "Host_Test") + # Need to define project at the top level to be able to call ctest from build directory + set(CMAKE_TOOLCHAIN_FILE "test/32bit.toolchain.cmake") project(vive-diy-position-sensor) include(CTest) + add_subdirectory(test) + add_subdirectory(src) - # Compile 32 bit code - add_compile_options("-m32") - link_libraries("-m32") - if (CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options("-Wno-deprecated-register" "-Wno-unknown-attributes") - endif() - - # Compile CMSIS as a library. - set(CMSIS_ROOT "${CMAKE_SOURCE_DIR}/libs/CMSIS/CMSIS" CACHE PATH "Path to the CMSIS root directory") - file(GLOB_RECURSE CMSIS_CORE_FILES "${CMSIS_ROOT}/DSP_Lib/Source/*_f32.c") - add_library(CMSIS "${CMSIS_CORE_FILES}" "${CMSIS_ROOT}/DSP_Lib/Source/CommonTables/arm_common_tables.c") - target_compile_definitions(CMSIS PUBLIC "-DARM_MATH_CM4") - link_libraries(CMSIS) +else() + message(FATAL_ERROR "Please provide PLATFORM env variable") - # We have only one test executable - add_definitions("-DNEW_H") # Don't include new.h header as it defines non-standard operator new(). - include_directories("libs/Catch" "libs/CMSIS/CMSIS/Include" "libs/cores/teensy3" "libs/mavlink_v2" "src") - add_executable(main-test "${TEST_SOURCE_FILES}" "${SOURCE_FILES}") - add_test(main-test main-test) endif() diff --git a/cmake-variants.json b/cmake-variants.json index a817aac..e2d41b1 100644 --- a/cmake-variants.json +++ b/cmake-variants.json @@ -1,23 +1,30 @@ { "buildType": { - "default$": "debug", + "default$": "teensy", "description$": "the build type to use", - "debug": { - "oneWordSummary$": "Debug", - "description$": "Enable optimizations, include debug info", - "buildType": "Debug" + "teensy": { + "oneWordSummary$": "Teensy", + "description$": "Create firmware for Teensy", + "buildType": "Debug", + "settings": { + "PLATFORM": "Teensy" + } }, - "release": { - "oneWordSummary$": "Release", - "description$": "Enable optimizations, omit debug info", - "buildType": "Release" + "particle": { + "oneWordSummary$": "Photon", + "description$": "Create library for Particle Photon", + "buildType": "Debug", + "settings": { + "PLATFORM": "Particle", + "PARTICLE_PLATFORM": "photon" + } }, "test": { "oneWordSummary$": "Test", "description$": "Test the project", "buildType": "Debug", "settings": { - "TEST_MODE": "TRUE", + "PLATFORM": "Host_Test", "CMAKE_C_COMPILER": "/usr/local/bin/gcc-6", "CMAKE_CXX_COMPILER": "/usr/local/bin/g++-6" } diff --git a/src/cycle_phase_classifier.h b/include/cycle_phase_classifier.h similarity index 97% rename from src/cycle_phase_classifier.h rename to include/cycle_phase_classifier.h index 2763e07..f3892dd 100644 --- a/src/cycle_phase_classifier.h +++ b/include/cycle_phase_classifier.h @@ -33,7 +33,7 @@ class CyclePhaseClassifier { // Print debug information. virtual bool debug_cmd(HashedWord *input_words); - virtual void debug_print(Print& stream); + virtual void debug_print(PrintStream &stream); private: float expected_pulse_len(bool skip, bool data, bool axis); diff --git a/src/data_frame_decoder.h b/include/data_frame_decoder.h similarity index 97% rename from src/data_frame_decoder.h rename to include/data_frame_decoder.h index 5859c6e..f536bdf 100644 --- a/src/data_frame_decoder.h +++ b/include/data_frame_decoder.h @@ -38,7 +38,7 @@ class DataFrameDecoder virtual void consume(const DataFrameBit &bit); virtual bool debug_cmd(HashedWord *input_words); - virtual void debug_print(Print& stream); + virtual void debug_print(PrintStream &stream); private: void reset(); diff --git a/src/debug_node.h b/include/debug_node.h similarity index 81% rename from src/debug_node.h rename to include/debug_node.h index 1808597..5c4a5f0 100644 --- a/src/debug_node.h +++ b/include/debug_node.h @@ -1,6 +1,7 @@ #pragma once #include "primitives/workers.h" #include "primitives/producer_consumer.h" +#include "primitives/string_utils.h" #include "print_helpers.h" // This node calls debug_cmd and debug_print for all pipeline nodes periodically, @@ -16,7 +17,7 @@ class DebugNode virtual void consume_line(char *line, Timestamp time); virtual void do_work(Timestamp cur_time); virtual bool debug_cmd(HashedWord *input_words); - virtual void debug_print(Print &stream); + virtual void debug_print(PrintStream &stream); private: void set_output_attached(bool attached); @@ -28,3 +29,6 @@ class DebugNode bool output_attached_; bool print_debug_memory_; }; + +// This function needs to be defined by the platform. +void print_platform_memory_info(PrintStream &stream); diff --git a/src/formatters.h b/include/formatters.h similarity index 91% rename from src/formatters.h rename to include/formatters.h index 833557f..5d48a18 100644 --- a/src/formatters.h +++ b/include/formatters.h @@ -24,8 +24,8 @@ struct FormatterDef { CoordSysType coord_sys_type; CoordSysDef coord_sys_params; - void print_def(uint32_t idx, Print &stream); - bool parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream); + void print_def(uint32_t idx, PrintStream &stream); + bool parse_def(uint32_t idx, HashedWord *input_words, PrintStream &err_stream); }; @@ -35,7 +35,7 @@ class FormatterNode , public Producer { public: virtual bool debug_cmd(HashedWord *input_words); - virtual void debug_print(Print& stream); + virtual void debug_print(PrintStream &stream); protected: FormatterNode(uint32_t idx, const FormatterDef &def); @@ -79,7 +79,7 @@ class GeometryMavlinkFormatter : public GeometryFormatter { virtual void consume(const ObjectPosition& f); virtual bool debug_cmd(HashedWord *input_words); - virtual void debug_print(Print& stream); + virtual void debug_print(PrintStream &stream); private: bool position_valid(const ObjectPosition& g); diff --git a/src/geometry.h b/include/geometry.h similarity index 87% rename from src/geometry.h rename to include/geometry.h index 8aa555e..46d448a 100644 --- a/src/geometry.h +++ b/include/geometry.h @@ -13,8 +13,8 @@ struct BaseStationGeometryDef { float mat[9]; // Normalized rotation matrix. vec3d origin; // Origin point - void print_def(uint32_t idx, Print &stream); - bool parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream); + void print_def(uint32_t idx, PrintStream &stream); + bool parse_def(uint32_t idx, HashedWord *input_words, PrintStream &err_stream); }; struct SensorLocalGeometry { @@ -26,8 +26,8 @@ struct SensorLocalGeometry { struct GeometryBuilderDef { Vector sensors; - void print_def(uint32_t idx, Print &stream); - bool parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream); + void print_def(uint32_t idx, PrintStream &stream); + bool parse_def(uint32_t idx, HashedWord *input_words, PrintStream &err_stream); }; // Parent, abstract class for GeometryBuilders. @@ -54,7 +54,7 @@ class PointGeometryBuilder : public GeometryBuilder { virtual void do_work(Timestamp cur_time); virtual bool debug_cmd(HashedWord *input_words); - virtual void debug_print(Print& stream); + virtual void debug_print(PrintStream &stream); private: ObjectPosition pos_; @@ -89,7 +89,7 @@ class CoordinateSystemConverter virtual void consume(const ObjectPosition& geo); virtual bool debug_cmd(HashedWord *input_words); - virtual void debug_print(Print& stream); + virtual void debug_print(PrintStream &stream); private: float mat_[9]; diff --git a/src/input.h b/include/input.h similarity index 79% rename from src/input.h rename to include/input.h index fe29dcd..bdc1124 100644 --- a/src/input.h +++ b/include/input.h @@ -2,13 +2,14 @@ #include "primitives/workers.h" #include "primitives/producer_consumer.h" #include "primitives/circular_buffer.h" +#include "primitives/static_registration.h" #include "messages.h" // We can use different Teensy hardware features to measure pulse timing, each with different pros and cons. // Look into each input type's header for details. enum class InputType { kCMP = 0, // Comparator - kFTM = 1, // Flexible Timer Module interrupts + kTimer = 1,// Timer Module interrupts kPort = 2, // Digital input interrupt }; constexpr int kInputTypeCount = 3; @@ -20,8 +21,8 @@ struct InputDef { InputType input_type; uint32_t initial_cmp_threshold; - void print_def(uint32_t idx, Print &stream); - bool parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream); + void print_def(uint32_t idx, PrintStream &stream); + bool parse_def(uint32_t idx, HashedWord *input_words, PrintStream &err_stream); }; @@ -32,10 +33,11 @@ class InputNode public: // Create input node of needed type from given configuration. static std::unique_ptr create(uint32_t input_idx, const InputDef &def); + typedef StaticRegistrar CreatorRegistrar; virtual void do_work(Timestamp cur_time); virtual bool debug_cmd(HashedWord *input_words); - virtual void debug_print(Print& stream); + virtual void debug_print(PrintStream &stream); protected: InputNode(uint32_t input_idx); @@ -49,6 +51,3 @@ class InputNode static constexpr int pulses_buffer_len = 32; CircularBuffer pulses_buf_; }; - -// Create functions for each supported input types. -std::unique_ptr createInputCmpNode(uint32_t input_idx, const InputDef &input_def); diff --git a/src/led_state.h b/include/led_state.h similarity index 100% rename from src/led_state.h rename to include/led_state.h diff --git a/src/messages.h b/include/messages.h similarity index 100% rename from src/messages.h rename to include/messages.h diff --git a/src/outputs.h b/include/outputs.h similarity index 66% rename from src/outputs.h rename to include/outputs.h index 15d3ebb..1cb6f31 100644 --- a/src/outputs.h +++ b/include/outputs.h @@ -3,6 +3,7 @@ #pragma once #include "primitives/workers.h" #include "primitives/producer_consumer.h" +#include "primitives/static_registration.h" #include "messages.h" // Currently supported: usb serial + 3x hardware serials. @@ -12,12 +13,10 @@ struct OutputDef { bool active; uint32_t bitrate; - void print_def(uint32_t idx, Print &stream); - bool parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream); + void print_def(uint32_t idx, PrintStream &stream); + bool parse_def(uint32_t idx, HashedWord *input_words, PrintStream &err_stream); }; -class Stream; - class OutputNode : public WorkerNode , public Consumer @@ -25,6 +24,7 @@ class OutputNode , public Producer { public: static std::unique_ptr create(uint32_t idx, const OutputDef& def); + typedef StaticRegistrar CreatorRegistrar; // Common methods that do i/o with the stream_ object. virtual void consume(const DataChunk &chunk); @@ -32,23 +32,15 @@ class OutputNode virtual void do_work(Timestamp cur_time); protected: - OutputNode(uint32_t idx, const OutputDef& def, Stream &stream); + OutputNode(uint32_t idx, const OutputDef& def); + + virtual size_t write(const uint8_t *buffer, size_t size) = 0; + virtual int read() = 0; + uint32_t node_idx_; OutputDef def_; - Stream &stream_; DataChunk chunk_; bool exclusive_mode_; uint32_t exclusive_stream_idx_; }; - -class UsbSerialOutputNode : public OutputNode { -public: - UsbSerialOutputNode(uint32_t idx, const OutputDef& def); -}; - -class HardwareSerialOutputNode : public OutputNode { -public: - HardwareSerialOutputNode(uint32_t idx, const OutputDef& def); - virtual void start(); -}; diff --git a/src/primitives/circular_buffer.h b/include/primitives/circular_buffer.h similarity index 100% rename from src/primitives/circular_buffer.h rename to include/primitives/circular_buffer.h diff --git a/src/primitives/float16.h b/include/primitives/float16.h similarity index 100% rename from src/primitives/float16.h rename to include/primitives/float16.h diff --git a/src/primitives/hash.h b/include/primitives/hash.h similarity index 100% rename from src/primitives/hash.h rename to include/primitives/hash.h diff --git a/src/primitives/producer_consumer.h b/include/primitives/producer_consumer.h similarity index 100% rename from src/primitives/producer_consumer.h rename to include/primitives/producer_consumer.h diff --git a/include/primitives/static_registration.h b/include/primitives/static_registration.h new file mode 100644 index 0000000..93d8d85 --- /dev/null +++ b/include/primitives/static_registration.h @@ -0,0 +1,36 @@ +#pragma once + +// Class to help with (almost) compile-time class registration. +// To use, add static instance of this class to the class that needs registering and provide creation +// function. Then, you'll be able to iterate over all creation functions. +template +struct StaticRegistrar { + StaticRegistrar(const T &val) + : val_(val) { + next_ = instance; + instance = this; + } + struct iterator { + const T &operator*() const { return cur->val_; } + iterator &operator++() { cur = cur->next_; return *this; } + bool operator!=(const iterator &it) const { return cur != it.cur; } + friend StaticRegistrar; + protected: + iterator(const StaticRegistrar *val): cur(val) {} + private: + const StaticRegistrar *cur; + }; + struct iteration_range { + iterator begin() { return instance; } + iterator end() { return nullptr; } + }; + static iteration_range iterate() { return {}; } + +private: + const T val_; + StaticRegistrar *next_; + static StaticRegistrar *instance; +}; + +template +StaticRegistrar *StaticRegistrar::instance = nullptr; diff --git a/src/primitives/static_utils.h b/include/primitives/static_utils.h similarity index 100% rename from src/primitives/static_utils.h rename to include/primitives/static_utils.h diff --git a/src/primitives/string_utils.h b/include/primitives/string_utils.h similarity index 84% rename from src/primitives/string_utils.h rename to include/primitives/string_utils.h index 8ad3183..b1c9fbb 100644 --- a/src/primitives/string_utils.h +++ b/include/primitives/string_utils.h @@ -1,20 +1,18 @@ #pragma once #include #include -#include "../primitives/hash.h" - -class Stream; -class Print; -template class Vector; +#include "hash.h" // Input string length constants. constexpr int max_input_str_len = 256; constexpr int max_words = 64; - -// Non-blocking version of Stream.readBytesUntil('\n', ...). Returns line if found, or NULL if no line. -// 'buf' argument needs to be provided to keep the accumulated data between calls. -char *read_line(Stream &stream, Vector *buf); +// Very simple virtual printer class. +class PrintStream { +public: + virtual size_t write(const char *buffer, size_t size) = 0; + int printf(const char *format, ...); +}; // Parses provided string to null-terminated array of trimmed strings. // NOTE: Provided string is changed - null characters are added after words. diff --git a/src/primitives/timestamp.h b/include/primitives/timestamp.h similarity index 97% rename from src/primitives/timestamp.h rename to include/primitives/timestamp.h index 5f7df1e..53fd31d 100644 --- a/src/primitives/timestamp.h +++ b/include/primitives/timestamp.h @@ -66,10 +66,12 @@ class Timestamp { // Get adjusted value of this timestamp in provided time unit. // We try to "extend" the value outside of regular period of timestamp using current time in millis. - // NOTE: This function is specialized for tu=1 below. // TODO: Add 64bit version of get_value. uint32_t get_value(TimeUnit tu) const; + // Get raw value in 'ticks'. + uint32_t get_raw_value() { return time_; } + // Static getters. static Timestamp cur_time(); // Implementation will try to get the best resolution possible. @@ -92,6 +94,7 @@ class Timestamp { private: constexpr Timestamp(uint32_t time): time_(time) {} + static uint32_t cur_time_millis(); // Helper function for get_value(). uint32_t time_; // Think about this as some global time mod 2^32. }; diff --git a/src/primitives/vector.h b/include/primitives/vector.h similarity index 100% rename from src/primitives/vector.h rename to include/primitives/vector.h diff --git a/src/primitives/workers.h b/include/primitives/workers.h similarity index 78% rename from src/primitives/workers.h rename to include/primitives/workers.h index 7cbd496..92a95ca 100644 --- a/src/primitives/workers.h +++ b/include/primitives/workers.h @@ -21,7 +21,7 @@ class WorkerNode { virtual bool debug_cmd(HashedWord *input_words) { return false; }; // Print current debugging information about this module. This function is called ~ every second. - virtual void debug_print(Print &stream) {}; + virtual void debug_print(PrintStream &stream) {}; // Virtual destructor to help with correct deletions. virtual ~WorkerNode() = default; @@ -31,6 +31,10 @@ class WorkerNode { // Pipeline 'owns' all the nodes and all of them will be deleted when the pipeline is deleted. class Pipeline : public WorkerNode { public: + Pipeline() + : stop_requested_(false) + {} + // Adding WorkerNodes to this pipeline. Note, after this Pipeline starts to own them and will // destruct them when needed. Suggested usage: // SpecializedNode *node = pipeline->add_front(std::make_unique()); @@ -48,6 +52,26 @@ class Pipeline : public WorkerNode { return res; } + // Helper function to run this pipeline. + void run() { + // Setup all hardware changes needed to run this pipeline. + start(); + + // Process incoming work until finished. + while (!stop_requested_) + do_work(Timestamp::cur_time()); + + // TODO someday: create method end(), symmetrical to start(). + } + + void stop() { + stop_requested_ = true; + } + + bool is_stop_requested() { + return stop_requested_; + } + // Define WorkerNode functions to work on all nodes in order. virtual void do_work(Timestamp cur_time) { for (auto& node : nodes_) @@ -63,7 +87,7 @@ class Pipeline : public WorkerNode { return true; return false; } - virtual void debug_print(Print &stream) { + virtual void debug_print(PrintStream &stream) { for (auto& node : nodes_) node->debug_print(stream); } @@ -71,4 +95,7 @@ class Pipeline : public WorkerNode { protected: // Owning list of nodes. All nodes here will have the same lifecycle as the pipeline itself. std::list> nodes_; + + // Flag that this pipeline should be stopped. + bool stop_requested_; }; diff --git a/src/print_helpers.h b/include/print_helpers.h similarity index 73% rename from src/print_helpers.h rename to include/print_helpers.h index d54b3e2..97330d6 100644 --- a/src/print_helpers.h +++ b/include/print_helpers.h @@ -2,26 +2,25 @@ #include "primitives/producer_consumer.h" #include "primitives/timestamp.h" #include "messages.h" -#include -// Implements Print interface which sends DataChunks as a Producer. +// Implements PrintStream interface which sends DataChunks as a Producer. // Note, the data is buffered and the last chunk is sent either on flush(), newline (if not binary), // or in destructor. -class DataChunkPrint : public Print { +class DataChunkPrintStream : public PrintStream { public: - DataChunkPrint(Producer *producer, Timestamp cur_time, uint32_t stream_idx = 0, bool binary = false) + DataChunkPrintStream(Producer *producer, Timestamp cur_time, uint32_t stream_idx = 0, bool binary = false) : producer_(producer), binary_(binary), chunk_{} { chunk_.time = cur_time; chunk_.stream_idx = stream_idx; } - ~DataChunkPrint() { + ~DataChunkPrintStream() { flush(); } // Main printing method overrides. We try to accumulate data for meaningful packets. - virtual size_t write(const uint8_t *buffer, size_t size) { + virtual size_t write(const char *buffer, size_t size) { if (!size || !buffer) return 0; @@ -42,10 +41,7 @@ class DataChunkPrint : public Print { flush(true); // Flush line-by-line for texts. return size; } - virtual size_t write(uint8_t b) { - return write(&b, 1); - } - + void flush(bool last_chunk = true) { if (!chunk_.data.empty()) { chunk_.last_chunk = last_chunk; @@ -67,13 +63,18 @@ class DataChunkLineSplitter : public Consumer { virtual void consume(const DataChunk& chunk) { for (uint32_t i = 0; i < chunk.data.size(); i++) { char c = chunk.data[i]; - if (c == '\n') { + if (c >= ' ' && c < 0x7F) { // Printable characters + if (input_str_buf_.size() < input_str_buf_.max_size() - 1) + input_str_buf_.push(c); + } else if (c == '\r' || (c == '\n' && prev_char_ != '\r')) { // Support \n, \r, \r\n line separators. input_str_buf_.push(0); input_str_buf_.clear(); consume_line(&input_str_buf_[0], chunk.time); - } else if (input_str_buf_.size() < input_str_buf_.max_size() - 1) { - input_str_buf_.push(c); + } else if (c == '\b' || c == 0x7F) { // Backspace/Delete symbol + if (input_str_buf_.size() > 0) + input_str_buf_.set_size(input_str_buf_.size()-1); } + prev_char_ = c; } } @@ -83,4 +84,5 @@ class DataChunkLineSplitter : public Consumer { private: Vector input_str_buf_; + char prev_char_; }; diff --git a/src/pulse_processor.h b/include/pulse_processor.h similarity index 97% rename from src/pulse_processor.h rename to include/pulse_processor.h index 368b382..c260f9e 100644 --- a/src/pulse_processor.h +++ b/include/pulse_processor.h @@ -18,7 +18,7 @@ class PulseProcessor virtual void do_work(Timestamp cur_time); virtual bool debug_cmd(HashedWord *input_words); - virtual void debug_print(Print& stream); + virtual void debug_print(PrintStream &stream); private: void process_long_pulse(const Pulse &p); diff --git a/src/settings.h b/include/settings.h similarity index 79% rename from src/settings.h rename to include/settings.h index 6a7b312..7a504f1 100644 --- a/src/settings.h +++ b/include/settings.h @@ -20,14 +20,16 @@ class PersistentSettings { // Settings lifecycle methods PersistentSettings(); bool needs_configuration() { return !is_configured_; } - void initialize_from_user_input(Stream &interactive_stream); void restart_in_configuration_mode(); + std::unique_ptr create_configuration_pipeline(uint32_t stream_idx); + bool process_command(char *input_cmd, PrintStream &stream); + private: - bool validate_setup(Print &error_stream); + bool validate_setup(PrintStream &error_stream); template - void set_value(Vector &arr, uint32_t idx, HashedWord *input_words, Print& stream); + void set_value(Vector &arr, uint32_t idx, HashedWord *input_words, PrintStream &stream); void reset(); bool read_from_eeprom(); @@ -45,3 +47,8 @@ class PersistentSettings { // Singleton to access current settings. extern PersistentSettings settings; + +// Functions to be implemented by platform +void restart_system(); +void eeprom_read(uint32_t eeprom_addr, void *dest, uint32_t len); +void eeprom_write(uint32_t eeprom_addr, const void *src, uint32_t len); diff --git a/src/vive_sensors_pipeline.h b/include/vive_sensors_pipeline.h similarity index 79% rename from src/vive_sensors_pipeline.h rename to include/vive_sensors_pipeline.h index d7c05d1..e613540 100644 --- a/src/vive_sensors_pipeline.h +++ b/include/vive_sensors_pipeline.h @@ -1,8 +1,7 @@ #pragma once +#include "primitives/workers.h" +#include "settings.h" #include -class PersistentSettings; -class Pipeline; - // Create Pipeline specialized for Vive Sensors, using provided configuration settings. std::unique_ptr create_vive_sensor_pipeline(const PersistentSettings &settings); diff --git a/platform-particle/CMakeLists.txt b/platform-particle/CMakeLists.txt new file mode 100644 index 0000000..0006d9e --- /dev/null +++ b/platform-particle/CMakeLists.txt @@ -0,0 +1,65 @@ + +set(PARTICLE_PLATFORM "photon" CACHE STRING "Particle platform to compile for") +set(FIRMWARE_DIR "${CMAKE_SOURCE_DIR}/libs/particle-firmware") +set(APPDIR "${CMAKE_CURRENT_SOURCE_DIR}") +set(TARGET_DIR "${CMAKE_BINARY_DIR}/particle") +set(TARGET_FILE "${PARTICLE_PLATFORM}") +set(MAKE_FLAGS + PLATFORM=${PARTICLE_PLATFORM} MODULAR=n PLATFORM_THREADING=0 + APPDIR=${APPDIR} TARGET_DIR=${TARGET_DIR} TARGET_FILE=${TARGET_FILE} +) +add_definitions(-DPLATFORM_THREADING=0) +add_definitions(-DPLATFORM_NAME=${PARTICLE_PLATFORM}) +if (${PARTICLE_PLATFORM} STREQUAL "photon") + add_definitions(-DPLATFORM_ID=6 -DPRODUCT_ID=6) +else () + # Please define PLATFORM_ID and PRODUCT_ID according to the platform name. + message(FATAL_ERROR "Unsupported particle platform: ${PARTICLE_PLATFORM}") +endif () + +# Download firmware and make deps. +include(ExternalProject) +ExternalProject_Add( + particle-firmware-deps + EXCLUDE_FROM_ALL 1 + SOURCE_DIR "${FIRMWARE_DIR}" + GIT_REPOSITORY "https://github.com/ashtuchkin/spark-firmware.git" + GIT_TAG "enable-exceptions-and-cpp14" + CONFIGURE_COMMAND "" + BINARY_DIR "${FIRMWARE_DIR}/main" + BUILD_COMMAND make build_dependencies -j1 ${MAKE_FLAGS} + INSTALL_COMMAND "" +) + +# CMSIS library +set(CMSIS_ROOT "${CMAKE_SOURCE_DIR}/libs/CMSIS/CMSIS" CACHE PATH "Path to the CMSIS root directory") +add_library(cmsis STATIC IMPORTED GLOBAL) +set_target_properties( + cmsis PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${CMSIS_ROOT}/Include" + INTERFACE_COMPILE_DEFINITIONS "ARM_MATH_CM3" + IMPORTED_LOCATION "${CMSIS_ROOT}/Lib/GCC/libarm_cortexM3l_math.a" +) + +# Build full firmware particle-firmware-deps +add_custom_target(sensor-firmware ALL + DEPENDS sensor-core + WORKING_DIRECTORY "${FIRMWARE_DIR}/main" + COMMAND make all ${MAKE_FLAGS} ADDITIONAL_LIB_DIRS='$ $' ADDITIONAL_LIBS='sensor-core arm_cortexM3l_math' +) + +add_custom_target(sensor-firmware-clean + DEPENDS particle-firmware-deps + WORKING_DIRECTORY "${FIRMWARE_DIR}/main" + COMMAND make clean ${MAKE_FLAGS} +) + +add_custom_target(sensor-firmware-upload + DEPENDS sensor-firmware + COMMAND particle flash --usb ${TARGET_DIR}/${TARGET_FILE}.bin +) + +add_custom_target(sensor-firmware-assembler + DEPENDS sensor-firmware + COMMAND ${CMAKE_OBJDUMP} --demangle --disassemble --source --wide ${TARGET_DIR}/${TARGET_FILE}.elf > ${TARGET_DIR}/${TARGET_FILE}-source.txt +) \ No newline at end of file diff --git a/platform-particle/build.mk b/platform-particle/build.mk new file mode 100644 index 0000000..a4cabf7 --- /dev/null +++ b/platform-particle/build.mk @@ -0,0 +1,4 @@ + +INCLUDE_DIRS += $(SOURCE_PATH) $(SOURCE_PATH)/../include +CPPSRC += $(call target_files,,*.cpp) +CSRC += $(call target_files,,*.c) diff --git a/platform-particle/input_tim.cpp b/platform-particle/input_tim.cpp new file mode 100644 index 0000000..5db3992 --- /dev/null +++ b/platform-particle/input_tim.cpp @@ -0,0 +1,198 @@ +#include "input_tim.h" +#include + +// Reference Manual: http://www.st.com/content/ccc/resource/technical/document/reference_manual/51/f7/f3/06/cd/b6/46/ec/CD00225773.pdf/files/CD00225773.pdf/jcr:content/translations/en.CD00225773.pdf +// 14.3.5 Input capture mode +// See also stm32f2xx_tim.c:1840 + +// Timer CH1 CH2 CH3 CH4 Control Bits Max Frequency +// -------------------------------------------------------------------------------------- +// TIM1 -- TX* RX* -- Advanced 16bit SystemCoreClock (120MHz) +// TIM2 -- LED:R LED:G LED:B Basic 32bit SystemCoreClock/2 +// TIM3 D3*/A4 D2/A5 -- -- Basic 16bit SystemCoreClock/2 +// TIM4 D1* D0 -- -- Basic 16bit SystemCoreClock/2 +// TIM5 WKP* -- -- -- Basic 32bit SystemCoreClock/2 +// TIM8 B1 -- B0 -- Advanced 16bit SystemCoreClock +// NOTE: Recommended pins are marked with a '*' character. +// NOTE: Pins D0-D7 -> 0..7; A0-A7 -> 10..17; RX/TX -> 18/19 +// NOTE: Electron Pins: B0-B5 -> 24..29; C0-C5 -> 30-35 + +struct TimerConfig { + TIM_TypeDef *timer; + uint16_t gpio_af; // Alternative Function + uint16_t rcc_apb; + uint32_t rcc_apb_periph; + uint16_t clock_divider; + uint16_t cc_irq; // # of Capture/Compare IRQ + hal_irq_t irq_by_channel[4]; +}; + +static const uint32_t num_timer_configs = 6; +static const TimerConfig timer_config[num_timer_configs] = { + {TIM1, GPIO_AF_TIM1, 2, RCC_APB2Periph_TIM1, 1, TIM1_CC_IRQn, {SysInterrupt_TIM1_Compare1, SysInterrupt_TIM1_Compare2, SysInterrupt_TIM1_Compare3, SysInterrupt_TIM1_Compare4}}, + {TIM2, GPIO_AF_TIM2, 1, RCC_APB1Periph_TIM2, 2, TIM2_IRQn, {SysInterrupt_TIM2_Compare1, SysInterrupt_TIM2_Compare2, SysInterrupt_TIM2_Compare3, SysInterrupt_TIM2_Compare4}}, + {TIM3, GPIO_AF_TIM3, 1, RCC_APB1Periph_TIM3, 2, TIM3_IRQn, {SysInterrupt_TIM3_Compare1, SysInterrupt_TIM3_Compare2, SysInterrupt_TIM3_Compare3, SysInterrupt_TIM3_Compare4}}, + {TIM4, GPIO_AF_TIM4, 1, RCC_APB1Periph_TIM4, 2, TIM4_IRQn, {SysInterrupt_TIM4_Compare1, SysInterrupt_TIM4_Compare2, SysInterrupt_TIM4_Compare3, SysInterrupt_TIM4_Compare4}}, + {TIM5, GPIO_AF_TIM5, 1, RCC_APB1Periph_TIM5, 2, TIM5_IRQn, {SysInterrupt_TIM5_Compare1, SysInterrupt_TIM5_Compare2, SysInterrupt_TIM5_Compare3, SysInterrupt_TIM5_Compare4}}, + {TIM8, GPIO_AF_TIM8, 2, RCC_APB2Periph_TIM8, 1, TIM8_CC_IRQn, {SysInterrupt_TIM8_Compare1, SysInterrupt_TIM8_Compare2, SysInterrupt_TIM8_Compare3, SysInterrupt_TIM8_Compare4}}, +}; + +struct TimerChannelConfig { + uint16_t channel; + uint16_t paired_channel; + uint16_t it_flag; // TIM_IT_CC1 +}; + +static const uint32_t num_timer_channels = 4; +static const TimerChannelConfig timer_channel_config[num_timer_channels] = { + {TIM_Channel_1, TIM_Channel_2, TIM_IT_CC1}, + {TIM_Channel_2, TIM_Channel_1, TIM_IT_CC2}, + {TIM_Channel_3, TIM_Channel_4, TIM_IT_CC3}, + {TIM_Channel_4, TIM_Channel_3, TIM_IT_CC4}, +}; + +// Track usage of channel pairs (we use 2 channels per pin to get timing of start and end of the pulse) +static uint8_t channel_pair_used_by_pin[num_timer_configs][num_timer_channels/2] = {0}; + + +InputTimNode::InputTimNode(uint32_t input_idx, const InputDef &input_def) + : InputNode(input_idx) + , pin_(input_def.pin) + , pulse_polarity_(input_def.pulse_polarity) { + + // Sanity checks. + assert(input_def.input_type == InputType::kTimer); + assert((SystemCoreClock/2) % sec == 0); // Time resolution must be whole number of timer ticks + + if (pin_ >= TOTAL_PINS) + throw_printf("Pin number too large: %d", pin_); + + // Find the timer that serves given pin. + STM32_Pin_Info& pin_info = HAL_Pin_Map()[pin_]; + timer_ = pin_info.timer_peripheral; + if (!timer_) + throw_printf("Pin %d is not connected to any timer", pin_); + + for (timer_idx_ = 0; timer_idx_ < num_timer_configs; timer_idx_++) + if (timer_config[timer_idx_].timer == timer_) + break; + + assert(timer_idx_ < num_timer_configs); + + // Get the channel id for the pin. Convert from TIM_Channel_N -> 0..3 + channel_idx_ = (pin_info.timer_ch - TIM_Channel_1) / (TIM_Channel_2 - TIM_Channel_1); + + // Check that the channel pair for current pin is still available. + if (channel_pair_used_by_pin[timer_idx_][channel_idx_/2]) + throw_printf("Pin %d conflicts with pin %d (same channel pair).", pin_, channel_pair_used_by_pin[timer_idx_][channel_idx_/2]-1); + + channel_pair_used_by_pin[timer_idx_][channel_idx_/2] = pin_+1; // Store pin+1 to keep 0 as an 'available' flag. +} + +InputNode::CreatorRegistrar InputTimNode::creator_([](uint32_t input_idx, const InputDef &input_def) -> std::unique_ptr { + if (input_def.input_type == InputType::kTimer) + return std::make_unique(input_idx, input_def); + return nullptr; +}); + +InputTimNode::~InputTimNode() { + // Deallocate channel pair. + channel_pair_used_by_pin[timer_idx_][channel_idx_/2] = 0; + + // Remove our interrupt handler. + HAL_Set_System_Interrupt_Handler(timer_config[timer_idx_].irq_by_channel[channel_idx_], NULL, NULL, NULL); +} + + +void InputTimNode::start() { + InputNode::start(); + + // See also HAL_Tone_Start for example of how timers are initialized. + const TimerConfig &conf = timer_config[timer_idx_]; + const TimerChannelConfig &channel_conf = timer_channel_config[channel_idx_]; + + // Enable clock for our timer + switch (conf.rcc_apb) { + case 1: RCC_APB1PeriphClockCmd(conf.rcc_apb_periph, ENABLE); break; + case 2: RCC_APB2PeriphClockCmd(conf.rcc_apb_periph, ENABLE); break; + default: assert(false); + } + + // Enable Pin Alternative Configuration + HAL_Pin_Mode(pin_, AF_OUTPUT_PUSHPULL); // It's actually INPUT, but we do care about AF - alternative function. + const STM32_Pin_Info &pin_info = HAL_Pin_Map()[pin_]; + GPIO_PinAFConfig(pin_info.gpio_peripheral, pin_info.gpio_pin_source, conf.gpio_af); + + // Enable IRQ Channel + NVIC_InitTypeDef NVIC_InitStructure; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; // Highest priority. + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_InitStructure.NVIC_IRQChannel = conf.cc_irq; + NVIC_Init(&NVIC_InitStructure); + + // Attach System Interrupt callback + HAL_InterruptCallback callback = { + [](void *data) { ((InputTimNode *)data)->_irq_handler(); }, this + }; + HAL_Set_System_Interrupt_Handler(conf.irq_by_channel[channel_idx_], &callback, NULL, NULL); + + // Initialize Timer to use our Timestamp resolution via prescaler. + TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct; + TIM_TimeBaseInitStruct.TIM_Prescaler = ((SystemCoreClock / conf.clock_divider) / sec) - 1; + TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInitStruct.TIM_Period = 0xFFFF; + TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseInitStruct.TIM_RepetitionCounter = 0; + TIM_TimeBaseInit(timer_, &TIM_TimeBaseInitStruct); + + // Init Input Capture mode to capture both rising and falling edges, using pair of channels. + TIM_ICInitTypeDef TIM_ICInitStruct; + TIM_ICInitStruct.TIM_Channel = channel_conf.channel; + TIM_ICInitStruct.TIM_ICPolarity = pulse_polarity_ ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising; + TIM_ICInitStruct.TIM_ICSelection = TIM_ICSelection_DirectTI; + TIM_ICInitStruct.TIM_ICPrescaler = TIM_ICPSC_DIV1; + TIM_ICInitStruct.TIM_ICFilter = 0x0; + TIM_ICInit(timer_, &TIM_ICInitStruct); + + TIM_ICInitStruct.TIM_Channel = channel_conf.paired_channel; + TIM_ICInitStruct.TIM_ICPolarity = pulse_polarity_ ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling; + TIM_ICInitStruct.TIM_ICSelection = TIM_ICSelection_IndirectTI; + TIM_ICInit(timer_, &TIM_ICInitStruct); + + // Enable interrupt and timer. + TIM_ITConfig(timer_, channel_conf.it_flag, ENABLE); + TIM_Cmd(timer_, ENABLE); + + // Synchronize timer counter and current Timestamp + TIM_SetCounter(timer_, Timestamp::cur_time().get_raw_value() & 0xFFFF); +} + +void InputTimNode::_irq_handler() { + Timestamp cur_time = Timestamp::cur_time(); + uint16_t pulse_stop = (&timer_->CCR1)[channel_idx_]; + uint16_t pulse_start = (&timer_->CCR1)[channel_idx_ ^ 1]; // paired channel + + // We assume that the pulse length is within one timer period. + uint16_t pulse_len = pulse_stop - pulse_start; + TimeDelta pulse_len_ts(pulse_len, (TimeUnit)1); + + // We also assume that the time between end of pulse and interrupt is also within one period. + // Plus, the timer counter is synchronized with the Timestamp's lower 16 bits. + uint16_t time_from_pulse_stop = (uint16_t)(cur_time.get_raw_value() & 0xFFFF) - pulse_stop; + if (time_from_pulse_stop > 0xFF00) time_from_pulse_stop = 0; // Overflow precaution. + TimeDelta time_from_pulse_stop_ts(time_from_pulse_stop, (TimeUnit)1); + + Timestamp pulse_start_ts(cur_time - time_from_pulse_stop_ts - pulse_len_ts); + InputNode::enqueue_pulse(pulse_start_ts, pulse_len_ts); +} + + +bool InputTimNode::debug_cmd(HashedWord *input_words) { + return InputNode::debug_cmd(input_words); +} + +void InputTimNode::debug_print(PrintStream& stream) { + InputNode::debug_print(stream); +} + diff --git a/platform-particle/input_tim.h b/platform-particle/input_tim.h new file mode 100644 index 0000000..c7b44dc --- /dev/null +++ b/platform-particle/input_tim.h @@ -0,0 +1,26 @@ +#pragma once +#include "input.h" +#include + +// Input node using STM32F2xx TIM timer modules. See description in .cpp file. +class InputTimNode : public InputNode { +public: + InputTimNode(uint32_t input_idx, const InputDef &def); + ~InputTimNode(); + + virtual void start(); + virtual bool debug_cmd(HashedWord *input_words); + virtual void debug_print(PrintStream& stream); + +private: + void _irq_handler(); + + // Denormalized configuration + uint32_t pin_; + bool pulse_polarity_; + TIM_TypeDef *timer_; + uint32_t timer_idx_; + uint32_t channel_idx_; + + static CreatorRegistrar creator_; +}; diff --git a/platform-particle/main.cpp b/platform-particle/main.cpp new file mode 100644 index 0000000..479295a --- /dev/null +++ b/platform-particle/main.cpp @@ -0,0 +1,27 @@ +#include "vive_sensors_pipeline.h" +#include "settings.h" +#include "application.h" + +// This will be either configuration pipeline, or production pipeline. +std::unique_ptr pipeline; + +void loop() { + try { + if (!pipeline || pipeline->is_stop_requested()) { + if (settings.needs_configuration()) { + // Initialize persistent settings interactively from user input, if needed. + pipeline = settings.create_configuration_pipeline(0); + } else { + // Otherwise, create production pipeline. + pipeline = create_vive_sensor_pipeline(settings); + } + pipeline->start(); + } + + pipeline->do_work(Timestamp::cur_time()); + } + catch (const ValidationException &exc) { + Serial.printf("Caught exception: %s", exc.what()); + PANIC(InvalidCase, "Caught exception: %s", exc.what()); + } +} diff --git a/platform-particle/notes.md b/platform-particle/notes.md new file mode 100644 index 0000000..c0208cc --- /dev/null +++ b/platform-particle/notes.md @@ -0,0 +1,120 @@ + +## Build sequence for 'main' project. +dependent modules: user wiring hal system services communication platform wiring_globals + + include ../build/platform-id.mk + PLATFORM_ID = 6 + ARCH=arm + PLATFORM=photon + STM32_DEVICE=STM32F2XX + STM32_DEVICE_LC = $(shell echo $(STM32_DEVICE) | tr A-Z a-z) # Lowercase + PLATFORM_NAME=photon + PLATFORM_MCU=STM32F2xx + PLATFORM_NET=BCM9WCDUSI09 + PRODUCT_DESC=Production Photon + USBD_VID_SPARK=0x2B04 + USBD_PID_DFU=0xD006 + USBD_PID_CDC=0xC006 + PLATFORM_DYNALIB_MODULES=photon + DEFAULT_PRODUCT_ID=6 + PLATFORM_STM32_STDPERIPH_EXCLUDE=y + PLATFORM_DFU ?= 0x08020000 + PLATFORM_THREADING=1 + CFLAGS += -DSTM32_DEVICE -D$(STM32_DEVICE) + PLATFORM_THREADING ?= 0 + CFLAGS += -DPLATFORM_THREADING=$(PLATFORM_THREADING) + CFLAGS += -DPLATFORM_ID=$(PLATFORM_ID) -DPLATFORM_NAME=$(PLATFORM_NAME) + CFLAGS += -DUSBD_VID_SPARK=$(USBD_VID_SPARK) + CFLAGS += -DUSBD_PID_DFU=$(USBD_PID_DFU) + CFLAGS += -DUSBD_PID_CDC=$(USBD_PID_CDC) + + include ../build/top-level-module.mk + PROJECT_ROOT ?= .. + MODULE_PATH=. + COMMON_BUILD=$(PROJECT_ROOT)/build + BUILD_PATH_BASE?=$(COMMON_BUILD)/target + + include ../build/macros.mk + ... + + PLATFORM_DFU_LEAVE = y + BUILD_PATH_EXT = $(BUILD_TARGET_PLATFORM)$(USER_FLAVOR) + USE_PRINTF_FLOAT ?= y + + # It's critical that user comes before wiring so that the setup()/loop() functions are linked in preference + # to the weak functions from wiring + # NOTE: MAKE_DEPENDENCIES will be make-d; DEPENDENCIES will be include.mk-d. + MAKE_DEPENDENCIES = newlib_nano user wiring hal system services communication platform wiring_globals + DEPENDENCIES = $(MAKE_DEPENDENCIES) dynalib + + LIBS += $(MAKE_DEPENDENCIES) + LIB_DEPS += $(USER_LIB_DEP) $(WIRING_LIB_DEP) $(SYSTEM_LIB_DEP) $(SERVICES_LIB_DEP) $(COMMUNICATION_LIB_DEP) $(HAL_LIB_DEP) $(PLATFORM_LIB_DEP) $(WIRING_GLOBALS_LIB_DEP) + LIB_DIRS += $(dir $(LIB_DEPS)) + + # Target this makefile is building. + TARGET=elf bin lst hex size + + include ../build/arm-tlm.mk + include arm-tools.mk + ...toolchain + + include module.mk + SOURCE_PATH ?= $(MODULE_PATH) + include $(MODULE_PATH)/import.mk # import this module's symbols + # .. pull in the include.mk files from each dependency in $(DEPENDENCIES) + include module-defaults.mk + ..CFLAGS, + .. add include dirs INCLUDE_DIRS + .. add linker dirs LIB_DIRS + .. LDFLAGS += $(LIBS_EXT) + .. LDFLAGS += -L$(COMMON_BUILD)/arm/linker + .. WHOLE_ARCHIVE=y + .. set TARGET_NAME, TARGET_PATH, TARGET_BASE, BUILD_PATH, BUILD_TARGET_PLATFORM, + include modules' all build.mk + GLOBAL_DEFINES += MODULE_VERSION=$(MAIN_MODULE_VERSION) + GLOBAL_DEFINES += MODULE_FUNCTION=$(MODULE_FUNCTION_MONO_FIRMWARE) + GLOBAL_DEFINES += MODULE_DEPENDENCY=0,0,0 + .. set TARGET_FILE_NAME, TARGET_DIR_NAME + .. TARGET_MAIN_SRC_PATH, + .. append CPPSRC, CSRC. + .. add GLOBAL_DEFINES as -D. + CFLAGS += -D_WINSOCK_H -D_GNU_SOURCE + .. unused: append CPPSRC, CSRC all .c/.cpp files from MODULE_LIBSV1/MODULE_LIBSV2 (set by user/build.mk) + collect all .c/.cpp in BUILD_PATH to ALLOBJ & ALLDEPS + + goal 'all': TARGET (for main: elf bin lst hex size) + for all of these: $(TARGET_BASE). + + goal 'build_dependencies': $(MAKE_DEPENDENCIES) + goal '*.elf': build_dependencies $(ALLOBJ) $(LIB_DEPS) $(LINKER_DEPS) -> link everything with $(LDFLAGS) + goal '*.bin': objcopy -binary + replace crc block at the end (append sha256 and crc) + goal '*.hex': objcopy -O ihex + + include recurse.mk + make all modules in MAKE_DEPENDENCIES by running make on them, with SUBDIR_GOALS + pass GLOBAL_DEFINES and MODULAR_FIRMWARE to them. + add "make_deps" and "clean_deps" goals. + +## Include directories +-I../user/inc +-I../wiring/inc +-I../hal/inc +-I../hal/shared +-I../hal/src/photon +-I../hal/src/stm32f2xx +-I../hal/src/stm32 +-I../hal/src/photon/api +-I../system/inc +-I../services/inc +-I../communication/src +-I../platform/shared/inc +-I../platform/MCU/STM32F2xx/CMSIS/Include +-I../platform/MCU/STM32F2xx/CMSIS/Device/ST/Include +-I../platform/MCU/STM32F2xx/SPARK_Firmware_Driver/inc +-I../platform/MCU/shared/STM32/inc +-I../platform/MCU/STM32F2xx/STM32_StdPeriph_Driver/inc +-I../platform/MCU/STM32F2xx/STM32_USB_Device_Driver/inc +-I../platform/MCU/STM32F2xx/STM32_USB_Host_Driver/inc +-I../platform/MCU/STM32F2xx/STM32_USB_OTG_Driver/inc +-I../dynalib/inc +-I. diff --git a/platform-particle/output_serial.cpp b/platform-particle/output_serial.cpp new file mode 100644 index 0000000..126c910 --- /dev/null +++ b/platform-particle/output_serial.cpp @@ -0,0 +1,52 @@ +#include "output_serial.h" +#include + + +// ====== OutputNodeStream ================================================== + +OutputNodeStream::OutputNodeStream(uint32_t idx, const OutputDef& def, Stream &stream) + : OutputNode(idx, def) + , stream_(stream) { +} + +size_t OutputNodeStream::write(const uint8_t *buffer, size_t size) { + return stream_.write(buffer, size); +} + +int OutputNodeStream::read() { + return stream_.read(); +} + +// ====== UsbSerialOutputNode =============================================== + +UsbSerialOutputNode::UsbSerialOutputNode(uint32_t idx, const OutputDef& def) + : OutputNodeStream(idx, def, Serial) { + assert(idx == 0); +} + +OutputNode::CreatorRegistrar UsbSerialOutputNode::creator_([](uint32_t idx, const OutputDef& def) -> std::unique_ptr { + if (idx == 0) + return std::make_unique(idx, def); + return nullptr; +}); + + +// ====== HardwareSerialOutputNode ========================================== + +static HardwareSerial *hardware_serials[num_outputs] = {nullptr, &Serial1, nullptr, nullptr}; + +HardwareSerialOutputNode::HardwareSerialOutputNode(uint32_t idx, const OutputDef& def) + : OutputNodeStream(idx, def, *hardware_serials[idx]) { + assert(idx < num_outputs && hardware_serials[idx]); + // TODO: check bitrate and serial format are valid. +} + +void HardwareSerialOutputNode::start() { + reinterpret_cast(&stream_)->begin(def_.bitrate); +} + +OutputNode::CreatorRegistrar HardwareSerialOutputNode::creator_([](uint32_t idx, const OutputDef& def) -> std::unique_ptr { + if (idx > 0 && idx < num_outputs) + return std::make_unique(idx, def); + return nullptr; +}); diff --git a/platform-particle/output_serial.h b/platform-particle/output_serial.h new file mode 100644 index 0000000..02926ff --- /dev/null +++ b/platform-particle/output_serial.h @@ -0,0 +1,28 @@ +#pragma once +#include "outputs.h" + +class Stream; + +class OutputNodeStream : public OutputNode { +public: + OutputNodeStream(uint32_t idx, const OutputDef& def, Stream &stream); + +protected: + virtual size_t write(const uint8_t *buffer, size_t size); + virtual int read(); + + Stream &stream_; +}; + +class UsbSerialOutputNode : public OutputNodeStream { +public: + UsbSerialOutputNode(uint32_t idx, const OutputDef& def); + static CreatorRegistrar creator_; +}; + +class HardwareSerialOutputNode : public OutputNodeStream { +public: + HardwareSerialOutputNode(uint32_t idx, const OutputDef& def); + virtual void start(); + static CreatorRegistrar creator_; +}; diff --git a/platform-particle/platform.cpp b/platform-particle/platform.cpp new file mode 100644 index 0000000..c86d6c9 --- /dev/null +++ b/platform-particle/platform.cpp @@ -0,0 +1,87 @@ +#include "debug_node.h" +#include "led_state.h" +#include "primitives/timestamp.h" +#include "application.h" + + +// ==== Timestamp helpers =================================================== + +extern volatile system_tick_t system_millis, system_millis_clock; +constexpr uint32_t F_CPU = 120*1000000; + +static_assert(F_CPU % sec == 0, "Timestamp tick take whole number of CPU ticks"); + +Timestamp Timestamp::cur_time() { + // Reimplementation of GetSystem1UsTick() to get better precision. + int is = __get_PRIMASK(); + __disable_irq(); + system_tick_t base_millis = system_millis; + system_tick_t base_clock = system_millis_clock; + system_tick_t cur_clock = DWT->CYCCNT; + if ((is & 1) == 0) + __enable_irq(); + + return base_millis * msec + (cur_clock - base_clock) / (F_CPU / sec); +} + +uint32_t Timestamp::cur_time_millis() { + return GetSystem1MsTick(); +} + + +// ==== LED helpers ========================================================= + +LedState cur_led_state = LedState::kNotInitialized; +LEDStatus led_statuses[] = { + [(int)LedState::kNotInitialized] = LEDStatus(RGB_COLOR_ORANGE, LED_PATTERN_SOLID), + [(int)LedState::kConfigMode] = LEDStatus(RGB_COLOR_GREEN, LED_PATTERN_BLINK, LED_SPEED_SLOW), + [(int)LedState::kNoFix] = LEDStatus(RGB_COLOR_BLUE, LED_PATTERN_BLINK, LED_SPEED_SLOW), + [(int)LedState::kFixFound] = LEDStatus(RGB_COLOR_BLUE, LED_PATTERN_BLINK, LED_SPEED_FAST), +}; + +void set_led_state(LedState state) { + if (state == cur_led_state) + return; + if ((uint32_t)state >= sizeof(led_statuses) / sizeof(led_statuses[0])) + return; + led_statuses[(uint32_t)cur_led_state].setActive(false); + cur_led_state = state; + led_statuses[(uint32_t)cur_led_state].setActive(true); +} + +void update_led_pattern(Timestamp cur_time) { + // Do nothing. +} + + +// ==== Debug node helpers ================================================== + +void print_platform_memory_info(PrintStream &stream) { + uint32_t freemem = System.freeMemory(); + stream.printf("RAM: free %d\n", freemem); +} + + +// ==== Configuration helpers =============================================== + +void restart_system() { + System.reset(); +} + +void _eeprom_init_if_needed() { + static bool initialized = false; + if (!initialized) { + HAL_EEPROM_Init(); + initialized = true; + } +} + +void eeprom_read(uint32_t eeprom_addr, void *dest, uint32_t len) { + _eeprom_init_if_needed(); + HAL_EEPROM_Get(eeprom_addr, dest, len); +} + +void eeprom_write(uint32_t eeprom_addr, const void *src, uint32_t len) { + _eeprom_init_if_needed(); + HAL_EEPROM_Put(eeprom_addr, src, len); +} diff --git a/platform-particle/stm32f2xx.toolchain.cmake b/platform-particle/stm32f2xx.toolchain.cmake new file mode 100644 index 0000000..85518f2 --- /dev/null +++ b/platform-particle/stm32f2xx.toolchain.cmake @@ -0,0 +1,54 @@ +# STM32F2xx toolchain, inspired by teensy and particle flags. +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_PROCESSOR arm) +set(CMAKE_CROSSCOMPILING 1) + +set(TOOLCHAIN_TRIPLE "arm-none-eabi-" CACHE STRING "Triple prefix for arm crosscompiling tools") +if(CMAKE_HOST_SYSTEM_NAME STREQUAL "Windows") + set(TOOLCHAIN_SUFFIX ".exe" CACHE STRING "Toolchain executable file extension") +else() + set(TOOLCHAIN_SUFFIX "" CACHE STRING "Toolchain executable file extension") +endif() + +# Search default paths for the GNU ARM Embedded Toolchain (https://developer.arm.com/open-source/gnu-toolchain/gnu-rm) +# This will need to be changed if you have it in a different directory. +find_path(TOOLCHAIN_BIN_PATH "${TOOLCHAIN_TRIPLE}gcc${TOOLCHAIN_SUFFIX}" + PATHS "/usr/local/bin" # Linux, Mac + PATHS "C:/Program Files (x86)/GNU Tools ARM Embedded/*/bin" # Default installation directory on Windows + DOC "Toolchain binaries directory") + +if(NOT TOOLCHAIN_BIN_PATH) + message(FATAL_ERROR "GNU ARM Embedded Toolchain not found. Please install it and provide its /bin directory as a TOOLCHAIN_BIN_PATH variable.") +endif() + +set(CMAKE_C_COMPILER "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}gcc${TOOLCHAIN_SUFFIX}" CACHE PATH "gcc") +set(CMAKE_CXX_COMPILER "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}g++${TOOLCHAIN_SUFFIX}" CACHE PATH "g++") +set(CMAKE_AR "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}ar${TOOLCHAIN_SUFFIX}" CACHE PATH "ar") +set(CMAKE_LINKER "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}ld${TOOLCHAIN_SUFFIX}" CACHE PATH "ld") +set(CMAKE_NM "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}nm${TOOLCHAIN_SUFFIX}" CACHE PATH "nm") +set(CMAKE_OBJCOPY "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}objcopy${TOOLCHAIN_SUFFIX}" CACHE PATH "objcopy") +set(CMAKE_OBJDUMP "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}objdump${TOOLCHAIN_SUFFIX}" CACHE PATH "objdump") +set(CMAKE_STRIP "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}strip${TOOLCHAIN_SUFFIX}" CACHE PATH "strip") +set(CMAKE_PRINT_SIZE "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}size${TOOLCHAIN_SUFFIX}" CACHE PATH "size") +set(CMAKE_RANLIB "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}ranlib${TOOLCHAIN_SUFFIX}" CACHE PATH "ranlib") + +set(TARGET_FLAGS "-mcpu=cortex-m3 -mthumb -mfp16-format=ieee") +set(OPTIMIZE_FLAGS "-O2 -g3 -gdwarf-2" CACHE STRING "Optimization flags") +set(CMAKE_C_FLAGS "${OPTIMIZE_FLAGS} -Wall -ffunction-sections -fdata-sections -fno-strict-aliasing -Wstack-usage=256 ${TARGET_FLAGS}" CACHE STRING "C/C++ flags") +set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} -fno-rtti -fsingle-precision-constant -Woverloaded-virtual" CACHE STRING "C++ flags") + +set(CMAKE_C_FLAGS_RELEASE "-DNDEBUG" CACHE STRING "" FORCE) # Don't do -O3 because it increases the size. Just remove asserts. +set(CMAKE_CXX_FLAGS_RELEASE "-DNDEBUG" CACHE STRING "" FORCE) + +set(LINKER_FLAGS "--gc-sections,--relax,--defsym=__rtc_localtime=0" CACHE STRING "Ld flags") +set(CXX_LINKER_FLAGS "${OPTIMIZE_FLAGS} -Wl,${LINKER_FLAGS} ${TARGET_FLAGS} --specs=nosys.specs") + +set(CMAKE_SHARED_LINKER_FLAGS "${CXX_LINKER_FLAGS}" CACHE STRING "Shared Linker flags" FORCE) +set(CMAKE_MODULE_LINKER_FLAGS "${CXX_LINKER_FLAGS}" CACHE STRING "Module Linker flags" FORCE) +set(CMAKE_EXE_LINKER_FLAGS "${CXX_LINKER_FLAGS}" CACHE STRING "Executable Linker flags" FORCE) + + +add_definitions(-DSTM32_DEVICE -DSTM32F2XX) +add_definitions(-DUSE_STDPERIPH_DRIVER -DDFU_BUILD_ENABLE) +add_definitions(-DRELEASE_BUILD -DSPARK=1 -DPARTICLE=1) + diff --git a/platform-teensy/CMakeLists.txt b/platform-teensy/CMakeLists.txt new file mode 100644 index 0000000..424928f --- /dev/null +++ b/platform-teensy/CMakeLists.txt @@ -0,0 +1,18 @@ +set(CMAKE_CXX_STANDARD 14) + +set(TEENSY_SOURCE_FILES + main.cpp + platform.cpp + timestamp.cpp + led_state.cpp + + input_cmp.cpp + output_serial.cpp +) + +include(teensy-targets.cmake) + +add_executable(vive-diy-position-sensor "${TEENSY_SOURCE_FILES}") +target_link_libraries(vive-diy-position-sensor PRIVATE sensor-core teensy-core) +add_firmware_targets(vive-diy-position-sensor) + diff --git a/src/input_cmp.cpp b/platform-teensy/input_cmp.cpp similarity index 96% rename from src/input_cmp.cpp rename to platform-teensy/input_cmp.cpp index 22bac28..14e0463 100644 --- a/src/input_cmp.cpp +++ b/platform-teensy/input_cmp.cpp @@ -34,7 +34,7 @@ struct ComparatorDef { int input_pins[6]; // CMPx_INy to Teensy digital pin # mapping. DAC0=100, DAC1=101. }; -// Comparator definitions. +// Comparator pin definitions. const static ComparatorDef comparator_defs[] = { #if defined(__MK20DX128__) // Teensy 3.0. Chip 64 LQFP pin numbers in comments. Teensy digital pins as numbers. {&CMP0_CR0, IRQ_CMP0, {/*51 */11, /*52 */12, /*53 */28, /*54 */ 27, /*-- */NA, /*17*/NA}}, @@ -164,9 +164,11 @@ InputCmpNode::InputCmpNode(uint32_t input_idx, const InputDef &input_def) input_cmps[cmp_idx_] = this; } -std::unique_ptr createInputCmpNode(uint32_t input_idx, const InputDef &input_def) { - return std::make_unique(input_idx, input_def); -} +InputNode::CreatorRegistrar InputCmpNode::creator_([](uint32_t input_idx, const InputDef &input_def) -> std::unique_ptr { + if (input_def.input_type == InputType::kCMP) + return std::make_unique(input_idx, input_def); + return nullptr; +}); InputCmpNode::~InputCmpNode() { input_cmps[cmp_idx_] = nullptr; @@ -208,7 +210,7 @@ bool InputCmpNode::debug_cmd(HashedWord *input_words) { return InputNode::debug_cmd(input_words); } -void InputCmpNode::debug_print(Print& stream) { +void InputCmpNode::debug_print(PrintStream& stream) { InputNode::debug_print(stream); } diff --git a/src/input_cmp.h b/platform-teensy/input_cmp.h similarity index 87% rename from src/input_cmp.h rename to platform-teensy/input_cmp.h index 8d7f99d..5f9ab15 100644 --- a/src/input_cmp.h +++ b/platform-teensy/input_cmp.h @@ -12,7 +12,7 @@ class InputCmpNode : public InputNode { virtual void start(); virtual bool debug_cmd(HashedWord *input_words); - virtual void debug_print(Print& stream); + virtual void debug_print(PrintStream& stream); void _isr_handler(volatile uint8_t *scr); @@ -28,4 +28,6 @@ class InputCmpNode : public InputNode { int cmp_input_idx_; volatile ComparatorPorts *ports_; const ComparatorDef *cmp_def_; + + static CreatorRegistrar creator_; }; diff --git a/src/led_state.cpp b/platform-teensy/led_state.cpp similarity index 100% rename from src/led_state.cpp rename to platform-teensy/led_state.cpp diff --git a/platform-teensy/main.cpp b/platform-teensy/main.cpp new file mode 100644 index 0000000..0b19b89 --- /dev/null +++ b/platform-teensy/main.cpp @@ -0,0 +1,16 @@ +#include "vive_sensors_pipeline.h" +#include "settings.h" + +extern "C" int main() { + // Initialize persistent settings interactively from user input, if needed. + if (settings.needs_configuration()) { + settings.create_configuration_pipeline(0)->run(); + } + + // Create worker nodes pipeline from settings, + auto pipeline = create_vive_sensor_pipeline(settings); + + // Register & start input nodes' interrupts, + // Process pulses, cycles and output data. + pipeline->run(); +} diff --git a/platform-teensy/output_serial.cpp b/platform-teensy/output_serial.cpp new file mode 100644 index 0000000..037458d --- /dev/null +++ b/platform-teensy/output_serial.cpp @@ -0,0 +1,52 @@ +#include "output_serial.h" +#include + + +// ====== OutputNodeStream ================================================== + +OutputNodeStream::OutputNodeStream(uint32_t idx, const OutputDef& def, Stream &stream) + : OutputNode(idx, def) + , stream_(stream) { +} + +size_t OutputNodeStream::write(const uint8_t *buffer, size_t size) { + return stream_.write(buffer, size); +} + +int OutputNodeStream::read() { + return stream_.read(); +} + +// ====== UsbSerialOutputNode =============================================== + +UsbSerialOutputNode::UsbSerialOutputNode(uint32_t idx, const OutputDef& def) + : OutputNodeStream(idx, def, Serial) { + assert(idx == 0); +} + +OutputNode::CreatorRegistrar UsbSerialOutputNode::creator_([](uint32_t idx, const OutputDef& def) -> std::unique_ptr { + if (idx == 0) + return std::make_unique(idx, def); + return nullptr; +}); + + +// ====== HardwareSerialOutputNode ========================================== + +static HardwareSerial *hardware_serials[num_outputs] = {nullptr, &Serial1, &Serial2, &Serial3}; + +HardwareSerialOutputNode::HardwareSerialOutputNode(uint32_t idx, const OutputDef& def) + : OutputNodeStream(idx, def, *hardware_serials[idx]) { + assert(idx > 0 && idx < num_outputs); + // TODO: check bitrate and serial format are valid. +} + +void HardwareSerialOutputNode::start() { + reinterpret_cast(&stream_)->begin(def_.bitrate); +} + +OutputNode::CreatorRegistrar HardwareSerialOutputNode::creator_([](uint32_t idx, const OutputDef& def) -> std::unique_ptr { + if (idx > 0 && idx < num_outputs) + return std::make_unique(idx, def); + return nullptr; +}); diff --git a/platform-teensy/output_serial.h b/platform-teensy/output_serial.h new file mode 100644 index 0000000..02926ff --- /dev/null +++ b/platform-teensy/output_serial.h @@ -0,0 +1,28 @@ +#pragma once +#include "outputs.h" + +class Stream; + +class OutputNodeStream : public OutputNode { +public: + OutputNodeStream(uint32_t idx, const OutputDef& def, Stream &stream); + +protected: + virtual size_t write(const uint8_t *buffer, size_t size); + virtual int read(); + + Stream &stream_; +}; + +class UsbSerialOutputNode : public OutputNodeStream { +public: + UsbSerialOutputNode(uint32_t idx, const OutputDef& def); + static CreatorRegistrar creator_; +}; + +class HardwareSerialOutputNode : public OutputNodeStream { +public: + HardwareSerialOutputNode(uint32_t idx, const OutputDef& def); + virtual void start(); + static CreatorRegistrar creator_; +}; diff --git a/src/platform.cpp b/platform-teensy/platform.cpp similarity index 77% rename from src/platform.cpp rename to platform-teensy/platform.cpp index b59381e..7c1bf86 100644 --- a/src/platform.cpp +++ b/platform-teensy/platform.cpp @@ -1,8 +1,12 @@ -#include "platform.h" +#include "debug_node.h" +#include "settings.h" + #include #include #include #include +#include +#include #include // eeprom handling. #include @@ -20,7 +24,7 @@ using namespace abi; // For Teensy, some of them are defined in teensy3/mk20dx128.c but we need to -// ==== 1. Termination ================================ +// ==== 1. Termination ====================================================== // abort() is a C stdlib function to do an abnormal program termination. // It's being called for example when exception handling mechanisms themselves fail. @@ -74,7 +78,7 @@ namespace __cxxabiv1 { // TODO: We might want to re-define _exit() and __cxa_pure_virtual() as well to provide meaningful actions. // TODO: Define correct actions for hard failures like division by zero or invalid memory access (see -fnon-call-exceptions) -// ==== 2. Memory & Stack ================================ +// ==== 2. Memory & Stack =================================================== // Low-level _sbrk() syscall used to allocate memory is defined in teensy3/mk20dx128.c and very simple - just moves // __brkval, current top of the heap. We're rewriting it to add stack clashing check. // malloc() uses _sbrk(). operator new() uses malloc(). @@ -90,6 +94,10 @@ namespace __cxxabiv1 { // NOTE: Stack overflow is not actually checked, but we're guaranteed that heap don't grow into stack of given size. // A proper solution would require setting up MMU correctly, which we don't want to do now. +// Max stack size. +// NOTE: Stack overflow is not checked for now. We do check heap from growing into stack, though. +constexpr int stack_size = 4096; + extern char *__brkval; // top of heap (dynamic ram): grows up towards stack extern char _estack; // bottom of stack, top of ram: stack grows down towards heap @@ -130,7 +138,7 @@ int get_stack_high_water_mark() { return static_stack_fill_checker.get_high_water_mark(); } -// ==== 3. Assertions ================================ +// ==== 3. Assertions ======================================================= [[noreturn]] void __assert_func2(const char *function_name, const char *expression) { // TODO: Write to EEPROM. @@ -152,13 +160,13 @@ int get_stack_high_water_mark() { } -// ==== 4. Writing to file descriptors ================================ +// ==== 4. Writing to file descriptors ======================================= // In Print.cpp we define _write() syscall and then use vdprintf() to write. Print class pointer is used // as file descriptor. -// ==== 5. Stack overflow protection ================================ +// ==== 5. Stack overflow protection ======================================== // -fstack-usage -fdump-rtl-dfinish gcc options can be used to get static stack structure. // See http://stackoverflow.com/questions/6387614/how-to-determine-maximum-stack-usage-in-embedded-system-with-gcc // Unfortunately, this probably doesn't work with virtual functions. @@ -169,6 +177,43 @@ int get_stack_high_water_mark() { // Yet another way is to place stack in the beginning of RAM. That way we'll fail hard. // -// ==== 6. Yield ========================================= +// ==== 6. Yield ============================================================ // This needs to be replaced with empty body to avoid linking to all the Serial-s and save memory. void yield() {} + + +// ==== 7. Printing debug information ======================================== + +// Link-time constant markers. Note, you need the *address* of these. +extern char _sdata; // start of static data +extern char _edata; +extern char _sbss; +extern char _ebss; // end of static data; bottom of heap +extern char _estack; // bottom of stack, top of ram: stack grows down towards heap + +void print_platform_memory_info(PrintStream &stream) { + uint32_t static_data_size = &_ebss - (char*)((uint32_t)&_sdata & 0xFFFFF000); + uint32_t allocated_heap = __brkval - &_ebss; + char c, *top_stack = &c; + int32_t unallocated = (&_estack - stack_size) - __brkval; + int32_t stack_max_used = get_stack_high_water_mark(); + uint32_t stack_used = &_estack - top_stack; + + struct mallinfo m = mallinfo(); + stream.printf("RAM: static %d, heap %d (used %d, free %d), unalloc %d, stack %d (used %d, max %d)\n", + static_data_size, allocated_heap, m.uordblks, m.fordblks, unallocated, stack_size, stack_used, stack_max_used); +} + +// ==== 8. Basic configuration helpers ======================================= + +void restart_system() { + SCB_AIRCR = 0x5FA0004; // Restart Teensy. +} + +void eeprom_read(uint32_t eeprom_addr, void *dest, uint32_t len) { + eeprom_read_block(dest, (void *)eeprom_addr, len); +} + +void eeprom_write(uint32_t eeprom_addr, const void *src, uint32_t len) { + eeprom_write_block(src, (void *)eeprom_addr, len); +} diff --git a/platform-teensy/teensy-arm.toolchain.cmake b/platform-teensy/teensy-arm.toolchain.cmake new file mode 100644 index 0000000..4d0c765 --- /dev/null +++ b/platform-teensy/teensy-arm.toolchain.cmake @@ -0,0 +1,57 @@ +# Teensy 3.1 toolchain, inspired by: +# * https://github.com/xya/teensy-cmake +# * http://playground.arduino.cc/Code/CmakeBuild + +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_PROCESSOR arm) +set(CMAKE_CROSSCOMPILING 1) + +set(TOOLCHAIN_TRIPLE "arm-none-eabi-" CACHE STRING "Triple prefix for arm crosscompiling tools") +if(CMAKE_HOST_SYSTEM_NAME STREQUAL "Windows") + set(TOOLCHAIN_SUFFIX ".exe" CACHE STRING "Toolchain executable file extension") +else() + set(TOOLCHAIN_SUFFIX "" CACHE STRING "Toolchain executable file extension") +endif() + +# Search default paths for the GNU ARM Embedded Toolchain (https://developer.arm.com/open-source/gnu-toolchain/gnu-rm) +# This will need to be changed if you have it in a different directory. +find_path(TOOLCHAIN_BIN_PATH "${TOOLCHAIN_TRIPLE}gcc${TOOLCHAIN_SUFFIX}" + PATHS "/usr/local/bin" # Linux, Mac + PATHS "C:/Program Files (x86)/GNU Tools ARM Embedded/*/bin" # Default installation directory on Windows + DOC "Toolchain binaries directory") + +if(NOT TOOLCHAIN_BIN_PATH) + message(FATAL_ERROR "GNU ARM Embedded Toolchain not found. Please install it and provide its /bin directory as a TOOLCHAIN_BIN_PATH variable.") +endif() + +set(CMAKE_C_COMPILER "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}gcc${TOOLCHAIN_SUFFIX}" CACHE PATH "gcc") +set(CMAKE_CXX_COMPILER "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}g++${TOOLCHAIN_SUFFIX}" CACHE PATH "g++") +set(CMAKE_AR "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}ar${TOOLCHAIN_SUFFIX}" CACHE PATH "ar") +set(CMAKE_LINKER "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}ld${TOOLCHAIN_SUFFIX}" CACHE PATH "ld") +set(CMAKE_NM "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}nm${TOOLCHAIN_SUFFIX}" CACHE PATH "nm") +set(CMAKE_OBJCOPY "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}objcopy${TOOLCHAIN_SUFFIX}" CACHE PATH "objcopy") +set(CMAKE_OBJDUMP "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}objdump${TOOLCHAIN_SUFFIX}" CACHE PATH "objdump") +set(CMAKE_STRIP "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}strip${TOOLCHAIN_SUFFIX}" CACHE PATH "strip") +set(CMAKE_PRINT_SIZE "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}size${TOOLCHAIN_SUFFIX}" CACHE PATH "size") +set(CMAKE_RANLIB "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}ranlib${TOOLCHAIN_SUFFIX}" CACHE PATH "ranlib") + + +set(TEENSY_ROOT "${CMAKE_SOURCE_DIR}/libs/cores/teensy3" CACHE PATH "Path to the Teensy 'cores/teensy3' repository") + +set(TARGET_FLAGS "-mcpu=cortex-m4 -mthumb -mfp16-format=ieee") +set(OPTIMIZE_FLAGS "-O2" CACHE STRING "Optimization flags") +set(CMAKE_C_FLAGS "${OPTIMIZE_FLAGS} -Wall -ffunction-sections -fdata-sections -Wstack-usage=256 ${TARGET_FLAGS}" CACHE STRING "C/C++ flags") +set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} -fno-rtti -fsingle-precision-constant -Woverloaded-virtual" CACHE STRING "C++ flags") + +set(CMAKE_C_FLAGS_RELEASE "-DNDEBUG" CACHE STRING "" FORCE) # Don't do -O3 because it increases the size. Just remove asserts. +set(CMAKE_CXX_FLAGS_RELEASE "-DNDEBUG" CACHE STRING "" FORCE) + +set(LINKER_FLAGS "--gc-sections,--relax,--defsym=__rtc_localtime=0" CACHE STRING "Ld flags") +set(CXX_LINKER_FLAGS "${OPTIMIZE_FLAGS} -Wl,${LINKER_FLAGS} ${TARGET_FLAGS} -T${TEENSY_ROOT}/mk20dx256.ld") + +set(CMAKE_SHARED_LINKER_FLAGS "${CXX_LINKER_FLAGS}" CACHE STRING "Shared Linker flags" FORCE) +set(CMAKE_MODULE_LINKER_FLAGS "${CXX_LINKER_FLAGS}" CACHE STRING "Module Linker flags" FORCE) +set(CMAKE_EXE_LINKER_FLAGS "${CXX_LINKER_FLAGS}" CACHE STRING "Executable Linker flags" FORCE) + + + diff --git a/platform-teensy/teensy-targets.cmake b/platform-teensy/teensy-targets.cmake new file mode 100644 index 0000000..1f75f0d --- /dev/null +++ b/platform-teensy/teensy-targets.cmake @@ -0,0 +1,83 @@ + +# Teensy/Arduino definitions +# /Users//Documents/Arduino/libraries +# /Applications/Arduino.app/Contents/Java/libraries +set(ARDUINO_LIB_ROOT "${CMAKE_SOURCE_DIR}/libs" CACHE PATH "Path to the Arduino library directory") +set(ARDUINO_VERSION "10607" CACHE STRING "Version of the Arduino SDK") + +set(TEENSYDUINO_VERSION "127" CACHE STRING "Version of the Teensyduino SDK") +set(TEENSY_MODEL "MK20DX256" CACHE STRING "Model of the Teensy MCU") +set(TEENSY_FREQUENCY "96" CACHE STRING "Frequency of the Teensy MCU (Mhz)") +set(TEENSY_USB_MODE "SERIAL" CACHE STRING "What kind of USB device the Teensy should emulate") +set_property(CACHE TEENSY_USB_MODE PROPERTY STRINGS SERIAL HID SERIAL_HID MIDI RAWHID FLIGHTSIM) + +add_definitions("-DARDUINO=${ARDUINO_VERSION}") +add_definitions("-DTEENSYDUINO=${TEENSYDUINO_VERSION}") +add_definitions("-D__${TEENSY_MODEL}__") +add_definitions("-DUSB_${TEENSY_USB_MODE}") +add_definitions("-DF_CPU=${TEENSY_FREQUENCY}000000") +add_definitions("-DLAYOUT_US_ENGLISH") + + +# Teensy core library +file(GLOB TEENSY_C_CORE_FILES "${TEENSY_ROOT}/*.c") +list(REMOVE_ITEM TEENSY_C_CORE_FILES "${TEENSY_ROOT}/math_helper.c") # legacy cmsis file - not needed anyway. +file(GLOB TEENSY_CXX_CORE_FILES "${TEENSY_ROOT}/*.cpp") +list(REMOVE_ITEM TEENSY_CXX_CORE_FILES "${TEENSY_ROOT}/new.cpp") # Don't use non-standard operator new. + +add_library(teensy-core STATIC EXCLUDE_FROM_ALL ${TEENSY_C_CORE_FILES} ${TEENSY_CXX_CORE_FILES}) +target_include_directories(teensy-core PUBLIC ${TEENSY_ROOT}) +target_compile_definitions(teensy-core PUBLIC "NEW_H") # Don't include new.h header as it defines non-standard operator new(). + +add_custom_command(TARGET teensy-core POST_BUILD + COMMAND ${CMAKE_OBJCOPY} --weaken-symbol=_sbrk $ + COMMENT Allow replacement of _sbrk() to better control memory allocation +) + + +# CMSIS library +set(CMSIS_ROOT "${CMAKE_SOURCE_DIR}/libs/CMSIS/CMSIS" CACHE PATH "Path to the CMSIS root directory") +add_library(cmsis STATIC IMPORTED GLOBAL) +set_target_properties( + cmsis PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${CMSIS_ROOT}/Include" + INTERFACE_COMPILE_DEFINITIONS "ARM_MATH_CM4" + IMPORTED_LOCATION "${CMSIS_ROOT}/Lib/GCC/libarm_cortexM4l_math.a" +) + + +# Firmare helper targets +function(add_firmware_targets TARGET_NAME) + # Generate the hex firmware files that can be flashed to the MCU. + set(EEPROM_OPTS -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0) + set(HEX_OPTS -O ihex -R .eeprom) + + if(NOT (CMAKE_HOST_SYSTEM_NAME STREQUAL "Windows")) + set(PROCESS_SIZE_CMD_OUTPUT | tail -1 | xargs bash -c [[ TEXT=$0\; DATA=$1\; BSS=$2\; TOTAL_FLASH=262144\; TOTAL_RAM=65536\; FLASH=$((TEXT+DATA))\; RAM=$((DATA+BSS))\; echo "FLASH: $FLASH ($((FLASH*100/TOTAL_FLASH))%), RAM: $RAM ($((RAM*100/TOTAL_RAM))%), Free RAM: $((TOTAL_RAM-RAM))" ]]) + endif() + + add_custom_target(${TARGET_NAME}-firmware ALL + COMMENT "Firmware size:" + COMMAND ${CMAKE_PRINT_SIZE} $ ${PROCESS_SIZE_CMD_OUTPUT} + COMMAND ${CMAKE_OBJCOPY} ${EEPROM_OPTS} $ ${TARGET_NAME}.eep + COMMAND ${CMAKE_OBJCOPY} ${HEX_OPTS} $ ${TARGET_NAME}.hex + VERBATIM + ) + + add_custom_target(${TARGET_NAME}-source + COMMAND ${CMAKE_OBJDUMP} --demangle --disassemble --headers --wide $ > ${CMAKE_SOURCE_DIR}/build/source.S + COMMAND ${CMAKE_OBJDUMP} --demangle --disassemble --source --wide $ > ${CMAKE_SOURCE_DIR}/build/source-with-text.S + COMMAND ${CMAKE_OBJDUMP} --demangle --syms $ | sort > ${CMAKE_SOURCE_DIR}/build/source-symbols.txt + ) + + # See https://github.com/Koromix/ty + find_file(TY_EXECUTABLE tyc + PATHS "/usr/local/bin" + DOC "Path to the 'tyc' executable that can upload programs to the Teensy") + + if(TY_EXECUTABLE) + add_custom_target(${TARGET_NAME}-upload + COMMAND "${TY_EXECUTABLE}" upload ${TARGET_NAME}.hex) + add_dependencies(${TARGET_NAME}-upload ${TARGET_NAME}-firmware) + endif() +endfunction(add_firmware_targets) diff --git a/platform-teensy/timestamp.cpp b/platform-teensy/timestamp.cpp new file mode 100644 index 0000000..6737e59 --- /dev/null +++ b/platform-teensy/timestamp.cpp @@ -0,0 +1,23 @@ +#include "primitives/timestamp.h" +#include + +Timestamp Timestamp::cur_time() { + // Repeat logic inside micros(), but get better precision, up to a single CPU tick. + extern volatile uint32_t systick_millis_count; + __disable_irq(); + uint32_t cpu_ticks = SYST_CVR; + uint32_t millis = systick_millis_count; + uint32_t istatus = SCB_ICSR; // bit 26 indicates if systick exception pending + __enable_irq(); + + if ((istatus & SCB_ICSR_PENDSTSET) && cpu_ticks > 50) millis++; + cpu_ticks = ((F_CPU / 1000) - 1) - cpu_ticks; + + static_assert(F_CPU % sec == 0, "Please choose TimeUnit.usec to be a multiple of CPU cycles to keep timing precise"); + return millis * msec + cpu_ticks / (F_CPU / sec); +}; + +uint32_t Timestamp::cur_time_millis() { + return millis(); +} + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..bbcec16 --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,24 @@ +set(CMAKE_CXX_STANDARD 14) + +set(SOURCE_FILES + cycle_phase_classifier.cpp + data_frame_decoder.cpp + debug_node.cpp + formatters.cpp + geometry.cpp + input.cpp + mavlink.cpp + outputs.cpp + pulse_processor.cpp + settings.cpp + vive_sensors_pipeline.cpp + + primitives/string_utils.cpp + primitives/timestamp.cpp +) + +add_library(sensor-core STATIC EXCLUDE_FROM_ALL ${SOURCE_FILES}) +target_include_directories(sensor-core PRIVATE ${CMAKE_SOURCE_DIR}/src ${CMAKE_SOURCE_DIR}/libs/mavlink_v2) +target_link_libraries(sensor-core PRIVATE cmsis) + +target_include_directories(sensor-core PUBLIC ${CMAKE_SOURCE_DIR}/include) diff --git a/src/cycle_phase_classifier.cpp b/src/cycle_phase_classifier.cpp index ea3237b..a9a70f4 100644 --- a/src/cycle_phase_classifier.cpp +++ b/src/cycle_phase_classifier.cpp @@ -1,5 +1,4 @@ #include "cycle_phase_classifier.h" -#include enum PhaseFixLevels { // Unscoped enum because we use it more like set of constants. kPhaseFixNone = 0, @@ -120,7 +119,7 @@ bool CyclePhaseClassifier::debug_cmd(HashedWord *input_words) { } return false; } -void CyclePhaseClassifier::debug_print(Print &stream) { +void CyclePhaseClassifier::debug_print(PrintStream &stream) { if (debug_print_state_) { stream.printf("CyclePhaseClassifier: fix %d, phase %d, pulse_base_len %f, history 0x%x, avg error %.1f us\n", fix_level_, phase_shift_, pulse_base_len_, phase_history_, average_error_); diff --git a/src/data_frame_decoder.cpp b/src/data_frame_decoder.cpp index 784040f..5b8e63d 100644 --- a/src/data_frame_decoder.cpp +++ b/src/data_frame_decoder.cpp @@ -97,6 +97,6 @@ bool DataFrameDecoder::debug_cmd(HashedWord *input_words) { return false; } -void DataFrameDecoder::debug_print(Print& stream) { +void DataFrameDecoder::debug_print(PrintStream &stream) { producer_debug_print(this, stream); } diff --git a/src/debug_node.cpp b/src/debug_node.cpp index 9b6f0e2..dc0b15d 100644 --- a/src/debug_node.cpp +++ b/src/debug_node.cpp @@ -1,10 +1,8 @@ #include "debug_node.h" #include -#include #include "settings.h" #include "led_state.h" -#include "platform.h" #include "print_helpers.h" @@ -26,7 +24,7 @@ void DebugNode::consume_line(char *input_cmd, Timestamp time) { HashedWord* hashed_words = hash_words(input_cmd); bool res = !*hashed_words || pipeline_->debug_cmd(hashed_words); if (!output_attached_ && !continuous_debug_print_) { - DataChunkPrint printer(this, time, stream_idx_); + DataChunkPrintStream printer(this, time, stream_idx_); if (!res) printer.printf("Unknown command.\n"); else if (print_debug) @@ -39,7 +37,7 @@ void DebugNode::do_work(Timestamp cur_time) { // Print current debug state if continuous printing is enabled. if (continuous_debug_print_ > 0 && throttle_ms(TimeDelta(continuous_debug_print_, ms), cur_time, &continuous_print_period_)) { - DataChunkPrint printer(this, cur_time, stream_idx_); + DataChunkPrintStream printer(this, cur_time, stream_idx_); pipeline_->debug_print(printer); } @@ -86,30 +84,8 @@ bool DebugNode::debug_cmd(HashedWord *input_words) { return false; } - -// ====== System-wide debug metrics ========================================= - -// Link-time constant markers. Note, you need the *address* of these. -extern char _sdata; // start of static data -extern char _edata; -extern char _sbss; -extern char _ebss; // end of static data; bottom of heap -extern char _estack; // bottom of stack, top of ram: stack grows down towards heap - -// Dynamic values from mk20dx128.c -extern char *__brkval; // top of heap (dynamic ram): grows up towards stack - -void DebugNode::debug_print(Print &stream) { +void DebugNode::debug_print(PrintStream &stream) { if (print_debug_memory_) { - uint32_t static_data_size = &_ebss - (char*)((uint32_t)&_sdata & 0xFFFFF000); - uint32_t allocated_heap = __brkval - &_ebss; - char c, *top_stack = &c; - int32_t unallocated = (&_estack - stack_size) - __brkval; - int32_t stack_max_used = get_stack_high_water_mark(); - uint32_t stack_used = &_estack - top_stack; - - struct mallinfo m = mallinfo(); - stream.printf("RAM: static %d, heap %d (used %d, free %d), unalloc %d, stack %d (used %d, max %d)\n", - static_data_size, allocated_heap, m.uordblks, m.fordblks, unallocated, stack_size, stack_used, stack_max_used); + print_platform_memory_info(stream); } } diff --git a/src/formatters.cpp b/src/formatters.cpp index dd4cf61..5e4768c 100644 --- a/src/formatters.cpp +++ b/src/formatters.cpp @@ -17,7 +17,7 @@ bool FormatterNode::debug_cmd(HashedWord *input_words) { } return false; } -void FormatterNode::debug_print(Print& stream) { +void FormatterNode::debug_print(PrintStream &stream) { producer_debug_print(this, stream); } @@ -27,7 +27,7 @@ void SensorAnglesTextFormatter::consume(const SensorAnglesFrame& f) { // Print each sensor on its own line. for (uint32_t i = 0; i < f.sensors.size(); i++) { - DataChunkPrint printer(this, f.time, node_idx_); + DataChunkPrintStream printer(this, f.time, node_idx_); const SensorAngles &angles = f.sensors[i]; printer.printf("ANG%d\t%u\t%d", i, time, f.fix_level); for (uint32_t j = 0; j < num_cycle_phases; j++) { @@ -50,7 +50,7 @@ std::unique_ptr GeometryFormatter::create(uint32_t idx, const // ====== GeometryTextFormatter ============================================= void GeometryTextFormatter::consume(const ObjectPosition& f) { - DataChunkPrint printer(this, f.time, node_idx_); + DataChunkPrintStream printer(this, f.time, node_idx_); printer.printf("OBJ%d\t%u\t%d", f.object_idx, f.time.get_value(msec), f.fix_level); if (f.fix_level >= FixLevel::kStaleFix) { printer.printf("\t%.4f\t%.4f\t%.4f\t%.4f", f.pos[0], f.pos[1], f.pos[2], f.pos_delta); @@ -75,7 +75,7 @@ HashedWord formatter_types[] = { }; -void FormatterDef::print_def(uint32_t idx, Print &stream) { +void FormatterDef::print_def(uint32_t idx, PrintStream &stream) { stream.printf("stream%d ", idx); // Print type & subtype. @@ -109,7 +109,7 @@ void FormatterDef::print_def(uint32_t idx, Print &stream) { stream.printf("> serial%d\n", output_idx); } -bool FormatterDef::parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream) { +bool FormatterDef::parse_def(uint32_t idx, HashedWord *input_words, PrintStream &err_stream) { bool type_found = false; for (uint32_t i = 0; i < sizeof(formatter_types) / sizeof(formatter_types[0]); i++) if (*input_words == formatter_types[i]) { diff --git a/src/geometry.cpp b/src/geometry.cpp index ab91b37..e2a6740 100644 --- a/src/geometry.cpp +++ b/src/geometry.cpp @@ -84,7 +84,7 @@ bool PointGeometryBuilder::debug_cmd(HashedWord *input_words) { } return false; } -void PointGeometryBuilder::debug_print(Print& stream) { +void PointGeometryBuilder::debug_print(PrintStream &stream) { producer_debug_print(this, stream); } @@ -213,7 +213,7 @@ bool CoordinateSystemConverter::debug_cmd(HashedWord *input_words) { } return false; } -void CoordinateSystemConverter::debug_print(Print& stream) { +void CoordinateSystemConverter::debug_print(PrintStream &stream) { producer_debug_print(this, stream); } @@ -221,7 +221,7 @@ void CoordinateSystemConverter::debug_print(Print& stream) { // ======= BaseStationGeometryDef I/O =========================================== // Format: base origin matrix <9x floats> -void BaseStationGeometryDef::print_def(uint32_t idx, Print &stream) { +void BaseStationGeometryDef::print_def(uint32_t idx, PrintStream &stream) { stream.printf("base%d origin", idx); for (int j = 0; j < 3; j++) stream.printf(" %f", origin[j]); @@ -231,7 +231,7 @@ void BaseStationGeometryDef::print_def(uint32_t idx, Print &stream) { stream.printf("\n"); } -bool BaseStationGeometryDef::parse_def(uint32_t idx, HashedWord *input_words, Print &stream) { +bool BaseStationGeometryDef::parse_def(uint32_t idx, HashedWord *input_words, PrintStream &stream) { if (*input_words == "origin"_hash) input_words++; for (int i = 0; i < 3; i++, input_words++) @@ -250,7 +250,7 @@ bool BaseStationGeometryDef::parse_def(uint32_t idx, HashedWord *input_words, Pr // ======= GeometryBuilderDef I/O =========================================== // Format: object [ sensor ]+ -void GeometryBuilderDef::print_def(uint32_t idx, Print &stream) { +void GeometryBuilderDef::print_def(uint32_t idx, PrintStream &stream) { stream.printf("object%d", idx); for (uint32_t i = 0; i < sensors.size(); i++) { const SensorLocalGeometry &sensor = sensors[i]; @@ -259,7 +259,7 @@ void GeometryBuilderDef::print_def(uint32_t idx, Print &stream) { stream.printf("\n"); } -bool GeometryBuilderDef::parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream) { +bool GeometryBuilderDef::parse_def(uint32_t idx, HashedWord *input_words, PrintStream &err_stream) { sensors.clear(); while (*input_words) { SensorLocalGeometry sensor; diff --git a/src/input.cpp b/src/input.cpp index 5161e44..88443a6 100644 --- a/src/input.cpp +++ b/src/input.cpp @@ -3,13 +3,11 @@ // Multiplexer method to create input node of correct type. // Throws exceptions on incorrect values. -std::unique_ptr InputNode::create(uint32_t input_idx, const InputDef &input_def) { - switch (input_def.input_type) { - case InputType::kCMP: return createInputCmpNode(input_idx, input_def); - case InputType::kPort: throw_printf("Port input type not implemented yet"); - case InputType::kFTM: throw_printf("FTM input type not implemented yet"); - default: throw_printf("Unknown input type"); - } +std::unique_ptr InputNode::create(uint32_t idx, const InputDef &def) { + for (auto creator_fn : InputNode::CreatorRegistrar::iterate()) + if (auto node = creator_fn(idx, def)) + return node; + throw_printf("Unknown/unimplemented input type"); } @@ -45,29 +43,28 @@ bool InputNode::debug_cmd(HashedWord *input_words) { return false; } -void InputNode::debug_print(Print& stream) { +void InputNode::debug_print(PrintStream &stream) { producer_debug_print(this, stream); } // ==== InputDef I/O ================================ -#include "Print.h" #include "primitives/string_utils.h" HashedWord input_types[kInputTypeCount] = { - [(int)InputType::kCMP] = { "cmp", "cmp"_hash }, - [(int)InputType::kFTM] = { "ftm", "ftm"_hash }, - [(int)InputType::kPort] = { "port_irq", "port_irq"_hash }, + [(int)InputType::kCMP] = { "cmp", "cmp"_hash }, + [(int)InputType::kTimer] = { "tim", "tim"_hash }, + [(int)InputType::kPort] = { "port_irq", "port_irq"_hash }, }; -void InputDef::print_def(uint32_t idx, Print &stream) { +void InputDef::print_def(uint32_t idx, PrintStream &stream) { stream.printf("sensor%d pin %d %s %s", idx, pin, pulse_polarity ? "positive" : "negative", input_types[(int)input_type].word); if (input_type == InputType::kCMP) stream.printf(" %d", initial_cmp_threshold); stream.printf("\n"); } -bool InputDef::parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream) { +bool InputDef::parse_def(uint32_t idx, HashedWord *input_words, PrintStream &err_stream) { if (*input_words == "pin"_hash) input_words++; // Ignore "pin" word diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index 98f924e..0000000 --- a/src/main.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include "vive_sensors_pipeline.h" -#include "settings.h" -#include - -extern "C" int main() { - // Initialize persistent settings interactively from user input, if needed. - if (settings.needs_configuration()) { - settings.initialize_from_user_input(Serial); - } - - // Create worker nodes pipeline from settings. - auto pipeline = create_vive_sensor_pipeline(settings); - - // Register & start input nodes' interrupts. - pipeline->start(); - - // Main loop. - while (true) { - // Process pulses, cycles and output data. - pipeline->do_work(Timestamp::cur_time()); - } -} diff --git a/src/mavlink.cpp b/src/mavlink.cpp index 0e37879..70809d3 100644 --- a/src/mavlink.cpp +++ b/src/mavlink.cpp @@ -70,9 +70,9 @@ void GeometryMavlinkFormatter::consume(const ObjectPosition& g) { void GeometryMavlinkFormatter::send_message(uint32_t msgid, const char *packet, Timestamp time, uint8_t min_length, uint8_t length, uint8_t crc_extra) { - uint8_t buf[MAVLINK_NUM_HEADER_BYTES]; - uint8_t ck[2]; - uint8_t header_len = MAVLINK_CORE_HEADER_LEN; + char buf[MAVLINK_NUM_HEADER_BYTES]; + char ck[2]; + int header_len = MAVLINK_CORE_HEADER_LEN; bool mavlink1 = false; if (mavlink1) { @@ -105,9 +105,9 @@ void GeometryMavlinkFormatter::send_message(uint32_t msgid, const char *packet, ck[0] = (uint8_t)(checksum & 0xFF); ck[1] = (uint8_t)(checksum >> 8); - DataChunkPrint printer(this, time, node_idx_, true); + DataChunkPrintStream printer(this, time, node_idx_, true); printer.write(buf, header_len+1); - printer.write((const uint8_t *)packet, length); + printer.write(packet, length); printer.write(ck, 2); } @@ -123,7 +123,7 @@ bool GeometryMavlinkFormatter::debug_cmd(HashedWord *input_words) { return false; } -void GeometryMavlinkFormatter::debug_print(Print &stream) { +void GeometryMavlinkFormatter::debug_print(PrintStream &stream) { FormatterNode::debug_print(stream); if (debug_print_state_) { if (debug_late_messages_ > 0) diff --git a/src/message_logging.h b/src/message_logging.h index 954141d..94e9aa4 100644 --- a/src/message_logging.h +++ b/src/message_logging.h @@ -6,20 +6,19 @@ #include "primitives/circular_buffer.h" #include "primitives/hash.h" #include "data_frame_decoder.h" -#include // This function is used to print messages used in Producer-Consumer pattern. // Please specialize this function for your messages below. template -inline void print_value(Print &stream, const T& val); +inline void print_value(PrintStream &stream, const T& val); template<> -inline void print_value(Print &stream, const Pulse& val) { +inline void print_value(PrintStream &stream, const Pulse& val) { stream.printf("\nsensor %d, time %dus, len %d ", val.input_idx, val.start_time.get_value(usec), val.pulse_len.get_value(usec)); } template<> -inline void print_value(Print &stream, const SensorAnglesFrame& val) { +inline void print_value(PrintStream &stream, const SensorAnglesFrame& val) { stream.printf("\n%dms: cycle %u, fix %02d, angles ", val.time.get_value(msec), val.cycle_idx, (int)val.fix_level / 100); for (uint32_t i = 0; i < val.sensors.size(); i++) { @@ -36,13 +35,13 @@ inline void print_value(Print &stream, const SensorAnglesFram } template<> -inline void print_value(Print &stream, const DataFrameBit& val) { +inline void print_value(PrintStream &stream, const DataFrameBit& val) { stream.printf("\n%dms: base %d, cycle %d, bit %d ", val.time.get_value(msec), val.base_station_idx, val.cycle_idx, val.bit); } template<> -inline void print_value(Print &stream, const DataFrame& frame) { +inline void print_value(PrintStream &stream, const DataFrame& frame) { stream.printf("\n%dms: ", frame.time.get_value(msec)); const DecodedDataFrame *df = reinterpret_cast(&frame.bytes[0]); if (frame.bytes.size() == 33 && df->protocol == DecodedDataFrame::cur_protocol) { @@ -62,7 +61,7 @@ inline void print_value(Print &stream, const DataFrame& frame) { } template<> -inline void print_value(Print &stream, const ObjectPosition& val) { +inline void print_value(PrintStream &stream, const ObjectPosition& val) { stream.printf("\n%dms: fix %2d, pos %.4f %.4f %.4f, dist %.4f ", val.time.get_value(msec), (int)val.fix_level/100, val.pos[0], val.pos[1], val.pos[2], val.pos_delta); if (val.q[0] != 1.0f) @@ -70,7 +69,7 @@ inline void print_value(Print &stream, const ObjectPosition& val } template<> -inline void print_value(Print &stream, const DataChunk& chunk) { +inline void print_value(PrintStream &stream, const DataChunk& chunk) { stream.printf("\n%dms: stream %d, data ", chunk.time.get_value(msec), chunk.stream_idx); for (uint32_t i = 0; i < chunk.data.size(); i++) stream.printf("%02x ", chunk.data[i]); @@ -80,7 +79,7 @@ inline void print_value(Print &stream, const DataChunk& chunk) { template class PrintableProduceLogger : public ProduceLogger { public: - virtual void print_logs(Print &stream) = 0; + virtual void print_logs(PrintStream &stream) = 0; }; template @@ -90,7 +89,7 @@ class CountingProducerLogger : public PrintableProduceLogger { virtual void log_produce(const T& val) { counter_++; } - virtual void print_logs(Print &stream) { + virtual void print_logs(PrintStream &stream) { uint32_t has_idx = idx_ != (uint32_t)-1; stream.printf("%s%.*u: %d items\n", name_, has_idx, has_idx && idx_, counter_); counter_ = 0; @@ -112,7 +111,7 @@ class PrintingProducerLogger : public PrintableProduceLogger { log_.enqueue(val); counter_++; } - virtual void print_logs(Print &stream) { + virtual void print_logs(PrintStream &stream) { bool first = true; uint32_t has_idx = idx_ != (uint32_t)-1; stream.printf("%s%.*u: ", name_, has_idx, has_idx && idx_); @@ -151,7 +150,7 @@ bool producer_debug_cmd(Producer *producer, HashedWord *input_words, const ch } template -void producer_debug_print(Producer *producer, Print &stream) { +void producer_debug_print(Producer *producer, PrintStream &stream) { if (PrintableProduceLogger *logger = static_cast*>(producer->logger())) logger->print_logs(stream); } diff --git a/src/outputs.cpp b/src/outputs.cpp index 54e0660..583e625 100644 --- a/src/outputs.cpp +++ b/src/outputs.cpp @@ -1,24 +1,18 @@ #include "outputs.h" -#include -static HardwareSerial *hardware_serials[num_outputs] = {nullptr, &Serial1, &Serial2, &Serial3}; - -OutputNode::OutputNode(uint32_t idx, const OutputDef& def, Stream &stream) +OutputNode::OutputNode(uint32_t idx, const OutputDef& def) : node_idx_(idx) , def_(def) - , stream_(stream) , chunk_{} , exclusive_mode_(false) , exclusive_stream_idx_(0) { } std::unique_ptr OutputNode::create(uint32_t idx, const OutputDef& def) { - if (idx == 0) - return std::make_unique(idx, def); - else if (idx < num_outputs) - return std::make_unique(idx, def); - else - throw_printf("Invalid output index: %d", idx); + for (auto creator_fn : OutputNode::CreatorRegistrar::iterate()) + if (auto node = creator_fn(idx, def)) + return node; + throw_printf("Invalid output with index: %d", idx); } void OutputNode::consume(const DataChunk &chunk) { @@ -26,7 +20,7 @@ void OutputNode::consume(const DataChunk &chunk) { return; // NOTE: This will wait until we can fit data into write buffer. Can cause a delay. - stream_.write(&chunk.data[0], chunk.data.size()); + write(&chunk.data[0], chunk.data.size()); } void OutputNode::consume(const OutputCommand& cmd) { @@ -39,7 +33,7 @@ void OutputNode::consume(const OutputCommand& cmd) { void OutputNode::do_work(Timestamp cur_time) { // Accumulate bytes read from the stream_ in the chunk_. while (!chunk_.data.full()) { - int c = stream_.read(); + int c = read(); if (c < 0) break; chunk_.time = cur_time; // Store the time of the last byte read. @@ -57,30 +51,10 @@ void OutputNode::do_work(Timestamp cur_time) { } -// ====== UsbSerialOutputNode =============================================== - -UsbSerialOutputNode::UsbSerialOutputNode(uint32_t idx, const OutputDef& def) - : OutputNode(idx, def, Serial) { - assert(idx == 0); -} - - -// ====== HardwareSerialOutputNode ========================================== - -HardwareSerialOutputNode::HardwareSerialOutputNode(uint32_t idx, const OutputDef& def) - : OutputNode(idx, def, *hardware_serials[idx]) { - assert(idx > 0 && idx < num_outputs); - // TODO: check bitrate and serial format are valid. -} - -void HardwareSerialOutputNode::start() { - reinterpret_cast(&stream_)->begin(def_.bitrate); -} - // ====== OutputDef I/O ===================================================== -void OutputDef::print_def(uint32_t idx, Print &stream) { +void OutputDef::print_def(uint32_t idx, PrintStream &stream) { if (idx == 0 && !active) { stream.printf("usb_serial off\n"); } else if (idx != 0 && active) { @@ -88,7 +62,7 @@ void OutputDef::print_def(uint32_t idx, Print &stream) { } } -bool OutputDef::parse_def(uint32_t idx, HashedWord *input_words, Print &err_stream) { +bool OutputDef::parse_def(uint32_t idx, HashedWord *input_words, PrintStream &err_stream) { if (idx == 0 || idx == (uint32_t)-1) { // usb_serial: do nothing unless turned off. active = (*input_words != "off"_hash); diff --git a/src/platform.h b/src/platform.h deleted file mode 100644 index 6cfb113..0000000 --- a/src/platform.h +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once - -// Platform-specific functions and constants. - -// Max stack size. -// NOTE: Stack overflow is not checked for now. We do check heap from growing into stack, though. -constexpr int stack_size = 4096; - -// Returns max actually used stack. -int get_stack_high_water_mark(); \ No newline at end of file diff --git a/src/primitives/string_utils.cpp b/src/primitives/string_utils.cpp index d7314ef..d02fc4d 100644 --- a/src/primitives/string_utils.cpp +++ b/src/primitives/string_utils.cpp @@ -1,23 +1,19 @@ -#include "string_utils.h" -#include "vector.h" +#include "primitives/string_utils.h" +#include "primitives/vector.h" #include +#include +#include #include -#include -// Non-blocking version of Stream.readBytesUntil('\n', ...). Returns line if found, or NULL if no line. -char *read_line(Stream &stream, Vector *buf) { - while (true) { - int next_char = stream.read(); - if (next_char < 0) { - return NULL; - } else if (next_char == '\n') { - buf->push(0); - buf->clear(); - return &(*buf)[0]; - } else if (buf->size() < buf->max_size() - 1) { - buf->push(next_char); - } - } +int PrintStream::printf(const char *format, ...) { + static char printf_buf[256]; + va_list args; + va_start(args, format); + int len = vsnprintf(printf_buf, sizeof(printf_buf), format, args); + va_end(args); + if (len > 0) + write(printf_buf, len); + return len; } // Parses provided string to null-terminated array of trimmed strings. diff --git a/src/primitives/timestamp.cpp b/src/primitives/timestamp.cpp index d1ad245..3930858 100644 --- a/src/primitives/timestamp.cpp +++ b/src/primitives/timestamp.cpp @@ -1,27 +1,9 @@ -#include "timestamp.h" -#include - -Timestamp Timestamp::cur_time() { - // Repeat logic inside micros(), but get better precision, up to a single CPU tick. - - extern volatile uint32_t systick_millis_count; - __disable_irq(); - uint32_t cpu_ticks = SYST_CVR; - uint32_t millis = systick_millis_count; - uint32_t istatus = SCB_ICSR; // bit 26 indicates if systick exception pending - __enable_irq(); - - if ((istatus & SCB_ICSR_PENDSTSET) && cpu_ticks > 50) millis++; - cpu_ticks = ((F_CPU / 1000) - 1) - cpu_ticks; - - static_assert(F_CPU % sec == 0, "Please choose TimeUnit.usec to be a multiple of CPU cycles to keep timing precise"); - return millis * msec + cpu_ticks / (F_CPU / sec); -}; +#include "primitives/timestamp.h" uint32_t Timestamp::get_value(TimeUnit tu) const { // To "extend" the period we assume that the timestamp is for close enough point in time to now. // Then, use millis and estimate the number of overflows happened. - uint32_t cur_millis = millis(); + uint32_t cur_millis = Timestamp::cur_time_millis(); Timestamp ts_cur_millis(cur_millis * msec); // This will overflow, but that's what we want. int32_t delta_in_tu = (*this - ts_cur_millis).get_value(tu); @@ -37,4 +19,3 @@ uint32_t Timestamp::get_value(TimeUnit tu) const { } return cur_time_in_tu + delta_in_tu; } - diff --git a/src/pulse_processor.cpp b/src/pulse_processor.cpp index 5b9d161..f957262 100644 --- a/src/pulse_processor.cpp +++ b/src/pulse_processor.cpp @@ -222,7 +222,7 @@ bool PulseProcessor::debug_cmd(HashedWord *input_words) { return false; } -void PulseProcessor::debug_print(Print &stream) { +void PulseProcessor::debug_print(PrintStream &stream) { phase_classifier_.debug_print(stream); producer_debug_print(this, stream); producer_debug_print(this, stream); diff --git a/src/settings.cpp b/src/settings.cpp index b10c726..a501a26 100644 --- a/src/settings.cpp +++ b/src/settings.cpp @@ -1,17 +1,14 @@ -// This file manages reading/writing data to EEPROM and configuration mode. +// This file manages project configuration & persisting it to EEPROM. #include "settings.h" #include "primitives/string_utils.h" #include "vive_sensors_pipeline.h" #include "led_state.h" - -#include -#include -#include +#include "print_helpers.h" constexpr uint32_t current_settings_version = 0xbabe0000 + sizeof(PersistentSettings); -constexpr uint32_t *eeprom_addr = 0; +constexpr uint32_t initial_eeprom_addr = 0; -static_assert(sizeof(PersistentSettings) < E2END - 500, "PersistentSettings must fit into EEPROM with some leeway"); +static_assert(sizeof(PersistentSettings) < 1500, "PersistentSettings must fit into EEPROM with some leeway"); static_assert(std::is_trivially_copyable(), "All definitions must be trivially copyable to be bitwise-stored"); /* Example settings @@ -35,10 +32,11 @@ PersistentSettings::PersistentSettings() { } bool PersistentSettings::read_from_eeprom() { - uint32_t eeprom_version = eeprom_read_dword(eeprom_addr); - if (eeprom_version == current_settings_version) { - // Normal initialization. - eeprom_read_block(this, eeprom_addr + 4, sizeof(*this)); + uint32_t eeprom_addr = initial_eeprom_addr, version; + eeprom_read(eeprom_addr, &version, sizeof(version)); eeprom_addr += sizeof(version); + if (version == current_settings_version) { + // Normal initialization: Just copy block of data. + eeprom_read(eeprom_addr, this, sizeof(*this)); return true; } // Unknown version. @@ -46,14 +44,16 @@ bool PersistentSettings::read_from_eeprom() { } void PersistentSettings::write_to_eeprom() { - eeprom_write_dword(eeprom_addr, current_settings_version); - eeprom_write_block(this, eeprom_addr + 4, sizeof(*this)); + uint32_t version = current_settings_version; + uint32_t eeprom_addr = initial_eeprom_addr; + eeprom_write(eeprom_addr, &version, sizeof(version)); eeprom_addr += sizeof(version); + eeprom_write(eeprom_addr, this, sizeof(*this)); } void PersistentSettings::restart_in_configuration_mode() { is_configured_ = false; write_to_eeprom(); - SCB_AIRCR = 0x5FA0004; // Restart Teensy. + restart_system(); } // Initialize settings. @@ -65,7 +65,7 @@ void PersistentSettings::reset() { outputs_[0].active = true; } -bool PersistentSettings::validate_setup(Print &error_stream) { +bool PersistentSettings::validate_setup(PrintStream &error_stream) { try { // Try to create the pipeline and then delete it. std::unique_ptr pipeline = create_vive_sensor_pipeline(*this); @@ -81,7 +81,7 @@ bool PersistentSettings::validate_setup(Print &error_stream) { } template -void PersistentSettings::set_value(Vector &arr, uint32_t idx, HashedWord *input_words, Print& stream) { +void PersistentSettings::set_value(Vector &arr, uint32_t idx, HashedWord *input_words, PrintStream &stream) { if (idx <= arr.size() && idx < arr_len) { T def; if (def.parse_def(idx, input_words, stream)) { @@ -106,26 +106,9 @@ void PersistentSettings::set_value(Vector &arr, uint32_t idx, Hashed stream.printf("Index too large. Next available index: %d.\n", arr.size()); } - -#pragma GCC diagnostic push -#pragma GCC diagnostic warning "-Wstack-usage=512" // Allow slightly higher stack usage for this function. - -void PersistentSettings::initialize_from_user_input(Stream &stream) { - Vector input_buf{}; - while (true) { - stream.print("config> "); - char *input_cmd = nullptr; - while (!input_cmd) { - input_cmd = read_line(stream, &input_buf); - - set_led_state(LedState::kConfigMode); - update_led_pattern(Timestamp::cur_time()); - } - - HashedWord *input_words = hash_words(input_cmd); - if (!input_words->word || input_words->word[0] == '#') // Do nothing on comments and empty lines. - continue; - +bool PersistentSettings::process_command(char *input_cmd, PrintStream &stream) { + HashedWord *input_words = hash_words(input_cmd); + if (*input_words && input_words->word[0] != '#') { // Do nothing on comments and empty lines. uint32_t idx = input_words->idx; switch (*input_words++) { case "view"_hash: @@ -193,12 +176,54 @@ void PersistentSettings::initialize_from_user_input(Stream &stream) { case "continue"_hash: if (!validate_setup(stream)) break; - return; + return false; default: stream.printf("Unknown command '%s'. Valid commands: view, , reset, reload, write, validate, continue.\n", (input_words-1)->word); } } + + // Print prompt for the next command. + stream.printf("config> "); + return true; } -#pragma GCC diagnostic pop + +// == Configuration pipeline ================================================== + +class SettingsReaderWriterNode + : public WorkerNode + , public DataChunkLineSplitter + , public Producer { +public: + SettingsReaderWriterNode(PersistentSettings *settings, Pipeline *pipeline) + : settings_(settings) + , pipeline_(pipeline) { + set_led_state(LedState::kConfigMode); + } + +private: + virtual void consume_line(char *line, Timestamp time) { + DataChunkPrintStream stream(this, time); + if (!settings_->process_command(line, stream)) + pipeline_->stop(); + } + + virtual void do_work(Timestamp cur_time) { + update_led_pattern(cur_time); + } + + PersistentSettings *settings_; + Pipeline *pipeline_; +}; + +std::unique_ptr PersistentSettings::create_configuration_pipeline(uint32_t stream_idx) { + auto pipeline = std::make_unique(); + + auto reader = pipeline->add_back(std::make_unique(this, pipeline.get())); + auto output_node = pipeline->add_back(OutputNode::create(stream_idx, OutputDef{})); + reader->pipe(output_node); + output_node->pipe(reader); + + return pipeline; +} diff --git a/src/ublox.cpp b/src/ublox.cpp index 93c1328..8c8c38b 100644 --- a/src/ublox.cpp +++ b/src/ublox.cpp @@ -103,7 +103,7 @@ enum ubx_nav_status_bits { NAV_STATUS_FIX_VALID = 1 }; -void send_ublox_packet(Stream &stream, uint8_t msg_class, uint8_t msg_id, const void *payload, uint8_t payload_len) { +void send_ublox_packet(PrintStream &stream, uint8_t msg_class, uint8_t msg_id, const void *payload, uint8_t payload_len) { ubx_packet packet = {{PREAMBLE1, PREAMBLE2, msg_class, msg_id, payload_len}, {}}; memcpy(packet.payload, payload, payload_len); @@ -121,7 +121,7 @@ void send_ublox_packet(Stream &stream, uint8_t msg_class, uint8_t msg_id, const } // all arguments in mm and mm/s -void send_ublox_ned_position(Stream &stream, bool fix_valid, float *pos, float *vel) { +void send_ublox_ned_position(PrintStream &stream, bool fix_valid, float *pos, float *vel) { const static int origin_lat = 1000000; // .1 degree const static int origin_lon = 1000000; // .1 degree const static int accuracy = 10; // mm diff --git a/src/vive_sensors_pipeline.cpp b/src/vive_sensors_pipeline.cpp index fe6181a..d81967f 100644 --- a/src/vive_sensors_pipeline.cpp +++ b/src/vive_sensors_pipeline.cpp @@ -7,7 +7,6 @@ #include "input.h" #include "outputs.h" #include "pulse_processor.h" -#include "settings.h" #include diff --git a/teensy-arm.toolchain.cmake b/teensy-arm.toolchain.cmake deleted file mode 100644 index 3a18562..0000000 --- a/teensy-arm.toolchain.cmake +++ /dev/null @@ -1,162 +0,0 @@ -# Teensy 3.1 toolchain, inspired by: -# * https://github.com/xya/teensy-cmake -# * http://playground.arduino.cc/Code/CmakeBuild - -set(CMAKE_SYSTEM_NAME Generic) -set(CMAKE_SYSTEM_PROCESSOR arm) -set(CMAKE_CROSSCOMPILING 1) - -set(TOOLCHAIN_TRIPLE "arm-none-eabi-" CACHE STRING "Triple prefix for arm crosscompiling tools") -if(CMAKE_HOST_SYSTEM_NAME STREQUAL "Windows") - set(TOOLCHAIN_SUFFIX ".exe" CACHE STRING "Toolchain executable file extension") -else() - set(TOOLCHAIN_SUFFIX "" CACHE STRING "Toolchain executable file extension") -endif() - -# Search default paths for the GNU ARM Embedded Toolchain (https://developer.arm.com/open-source/gnu-toolchain/gnu-rm) -# This will need to be changed if you have it in a different directory. -find_path(TOOLCHAIN_BIN_PATH "${TOOLCHAIN_TRIPLE}gcc${TOOLCHAIN_SUFFIX}" - PATHS "/usr/local/bin" # Linux, Mac - PATHS "C:/Program Files (x86)/GNU Tools ARM Embedded/*/bin" # Default installation directory on Windows - DOC "Toolchain binaries directory") - -if(NOT TOOLCHAIN_BIN_PATH) - message(FATAL_ERROR "GNU ARM Embedded Toolchain not found. Please install it and provide its /bin directory as a TOOLCHAIN_BIN_PATH variable.") -endif() - -set(CMAKE_C_COMPILER "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}gcc${TOOLCHAIN_SUFFIX}" CACHE PATH "gcc") -set(CMAKE_CXX_COMPILER "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}g++${TOOLCHAIN_SUFFIX}" CACHE PATH "g++") -set(CMAKE_AR "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}ar${TOOLCHAIN_SUFFIX}" CACHE PATH "ar") -set(CMAKE_LINKER "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}ld${TOOLCHAIN_SUFFIX}" CACHE PATH "ld") -set(CMAKE_NM "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}nm${TOOLCHAIN_SUFFIX}" CACHE PATH "nm") -set(CMAKE_OBJCOPY "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}objcopy${TOOLCHAIN_SUFFIX}" CACHE PATH "objcopy") -set(CMAKE_OBJDUMP "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}objdump${TOOLCHAIN_SUFFIX}" CACHE PATH "objdump") -set(CMAKE_STRIP "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}strip${TOOLCHAIN_SUFFIX}" CACHE PATH "strip") -set(CMAKE_PRINT_SIZE "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}size${TOOLCHAIN_SUFFIX}" CACHE PATH "size") -set(CMAKE_RANLIB "${TOOLCHAIN_BIN_PATH}/${TOOLCHAIN_TRIPLE}ranlib${TOOLCHAIN_SUFFIX}" CACHE PATH "ranlib") - - -# Teensy 3.1 settings - -set(TEENSY_ROOT "${CMAKE_SOURCE_DIR}/libs/cores/teensy3" CACHE PATH "Path to the Teensy 'cores/teensy3' repository") -set(CMSIS_ROOT "${CMAKE_SOURCE_DIR}/libs/CMSIS/CMSIS" CACHE PATH "Path to the CMSIS root directory") - -# /Users//Documents/Arduino/libraries -# /Applications/Arduino.app/Contents/Java/libraries -set(ARDUINO_LIB_ROOT "${CMAKE_SOURCE_DIR}/libs" CACHE PATH "Path to the Arduino library directory") -set(ARDUINO_VERSION "10607" CACHE STRING "Version of the Arduino SDK") - -set(TEENSYDUINO_VERSION "127" CACHE STRING "Version of the Teensyduino SDK") -set(TEENSY_MODEL "MK20DX256" CACHE STRING "Model of the Teensy MCU") - -set(TEENSY_FREQUENCY "96" CACHE STRING "Frequency of the Teensy MCU (Mhz)") -set_property(CACHE TEENSY_FREQUENCY PROPERTY STRINGS 96 72 48 24 16 8 4 2) - -set(TEENSY_USB_MODE "SERIAL" CACHE STRING "What kind of USB device the Teensy should emulate") -set_property(CACHE TEENSY_USB_MODE PROPERTY STRINGS SERIAL HID SERIAL_HID MIDI RAWHID FLIGHTSIM) - - -set(TARGET_FLAGS "-mcpu=cortex-m4 -mthumb -mfp16-format=ieee") -set(OPTIMIZE_FLAGS "-O2" CACHE STRING "Optimization flags") # Remember to reset cache and rebuild cmake when changing this. -set(CMAKE_C_FLAGS "${OPTIMIZE_FLAGS} -Wall -ffunction-sections -fdata-sections -Wstack-usage=256 ${TARGET_FLAGS}" CACHE STRING "C/C++ flags") -set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} -fno-rtti -fsingle-precision-constant -Woverloaded-virtual" CACHE STRING "C++ flags") - -set(CMAKE_C_FLAGS_RELEASE "-DNDEBUG" CACHE STRING "" FORCE) # Don't do -O3 because it increases the size. Just remove asserts. -set(CMAKE_CXX_FLAGS_RELEASE "-DNDEBUG" CACHE STRING "" FORCE) - -link_libraries(m) -set(LINKER_FLAGS "--gc-sections,--relax,--defsym=__rtc_localtime=0" CACHE STRING "Ld flags") -set(CXX_LINKER_FLAGS "${OPTIMIZE_FLAGS} -Wl,${LINKER_FLAGS} ${TARGET_FLAGS} -T${TEENSY_ROOT}/mk20dx256.ld") - -set(CMAKE_SHARED_LINKER_FLAGS "${CXX_LINKER_FLAGS}" CACHE STRING "Shared Linker flags" FORCE) -set(CMAKE_MODULE_LINKER_FLAGS "${CXX_LINKER_FLAGS}" CACHE STRING "Module Linker flags" FORCE) -set(CMAKE_EXE_LINKER_FLAGS "${CXX_LINKER_FLAGS}" CACHE STRING "Executable Linker flags" FORCE) - - -add_definitions("-DARDUINO=${ARDUINO_VERSION}") -add_definitions("-DTEENSYDUINO=${TEENSYDUINO_VERSION}") -add_definitions("-D__${TEENSY_MODEL}__") -add_definitions("-DUSB_${TEENSY_USB_MODE}") -add_definitions("-DF_CPU=${TEENSY_FREQUENCY}000000") -add_definitions("-DLAYOUT_US_ENGLISH") -add_definitions("-DNEW_H") # Don't include new.h header as it defines non-standard operator new(). -add_definitions("-MMD") - -# Define target for the Teensy 'core' library. -# CMake does a lot of compiler checks which include toolchain file, so be careful to define it only once and only -# for the actual source (not sample programs). -if (NOT TARGET TeensyCore AND NOT ${CMAKE_SOURCE_DIR} MATCHES "CMakeTmp") - file(GLOB TEENSY_C_CORE_FILES "${TEENSY_ROOT}/*.c") - list(REMOVE_ITEM TEENSY_C_CORE_FILES "${TEENSY_ROOT}/math_helper.c") # legacy cmsis file - not needed anyway. - file(GLOB TEENSY_CXX_CORE_FILES "${TEENSY_ROOT}/*.cpp") - list(REMOVE_ITEM TEENSY_CXX_CORE_FILES "${TEENSY_ROOT}/new.cpp") # Don't use non-standard operator new. - add_library(TeensyCore ${TEENSY_C_CORE_FILES} ${TEENSY_CXX_CORE_FILES}) - link_libraries(TeensyCore) - include_directories(${TEENSY_ROOT}) - add_custom_command(TARGET TeensyCore POST_BUILD - COMMAND ${CMAKE_OBJCOPY} --weaken-symbol=_sbrk $ - COMMENT Allow replacement of _sbrk() to better control memory allocation - ) -endif (NOT TARGET TeensyCore AND NOT ${CMAKE_SOURCE_DIR} MATCHES "CMakeTmp") - - -function(add_firmware_targets TARGET_NAME) - target_include_directories(${TARGET_NAME} PRIVATE "${TEENSY_ROOT}") - - # Generate the hex firmware files that can be flashed to the MCU. - set(EEPROM_OPTS -O ihex -j .eeprom --set-section-flags=.eeprom=alloc,load --no-change-warnings --change-section-lma .eeprom=0) - set(HEX_OPTS -O ihex -R .eeprom) - - if(NOT (CMAKE_HOST_SYSTEM_NAME STREQUAL "Windows")) - set(PROCESS_SIZE_CMD_OUTPUT | tail -1 | xargs bash -c [[ TEXT=$0\; DATA=$1\; BSS=$2\; TOTAL_FLASH=262144\; TOTAL_RAM=65536\; FLASH=$((TEXT+DATA))\; RAM=$((DATA+BSS))\; echo "FLASH: $FLASH ($((FLASH*100/TOTAL_FLASH))%), RAM: $RAM ($((RAM*100/TOTAL_RAM))%), Free RAM: $((TOTAL_RAM-RAM))" ]]) - endif() - - add_custom_target(${TARGET_NAME}_Firmware ALL - COMMENT "Firmware size:" - COMMAND ${CMAKE_PRINT_SIZE} $ ${PROCESS_SIZE_CMD_OUTPUT} - COMMAND ${CMAKE_OBJCOPY} ${EEPROM_OPTS} $ ${TARGET_NAME}.eep - COMMAND ${CMAKE_OBJCOPY} ${HEX_OPTS} $ ${TARGET_NAME}.hex - VERBATIM - ) - - add_custom_target(${TARGET_NAME}_Assembler - COMMAND ${CMAKE_OBJDUMP} --demangle --disassemble --headers --wide $ > ${CMAKE_SOURCE_DIR}/build/source.S - COMMAND ${CMAKE_OBJDUMP} --demangle --disassemble --source --wide $ > ${CMAKE_SOURCE_DIR}/build/source-with-text.S - COMMAND ${CMAKE_OBJDUMP} --demangle --syms $ | sort > ${CMAKE_SOURCE_DIR}/build/source-symbols.txt - ) - - # See https://github.com/Koromix/ty - find_file(TY_EXECUTABLE tyc - PATHS "/usr/local/bin" - DOC "Path to the 'tyc' executable that can upload programs to the Teensy") - - if(TY_EXECUTABLE) - add_custom_target(${TARGET_NAME}_Upload - COMMAND "${TY_EXECUTABLE}" upload ${TARGET_NAME}.hex) - add_dependencies(${TARGET_NAME}_Upload ${TARGET_NAME}_Firmware) - endif() -endfunction(add_firmware_targets) - - -function(import_cmsis_dsp_library TARGET_NAME) - target_include_directories(${TARGET_NAME} BEFORE PRIVATE "${CMSIS_ROOT}/Include") # Make it a priority over old-version arm_math.h in teensy. - target_link_libraries(${TARGET_NAME} ${CMSIS_ROOT}/Lib/GCC/libarm_cortexM4l_math.a) - target_compile_definitions(${TARGET_NAME} PRIVATE -DARM_MATH_CM4) -endfunction(import_cmsis_dsp_library) - -function(import_arduino_library TARGET_NAME LIB_NAME) - # Check if we can find the library. - set(LIB_DIR "${ARDUINO_LIB_ROOT}/${LIB_NAME}") - if(NOT EXISTS "${LIB_DIR}") - message(FATAL_ERROR "Could not find the directory for library '${LIB_NAME}'") - endif(NOT EXISTS "${LIB_DIR}") - - # Add it to the include path. - target_include_directories(${TARGET_NAME} PRIVATE "${LIB_DIR}") - - # Mark source files to be built along with the sketch code. - file(GLOB SOURCES_CPP ABSOLUTE "${LIB_DIR}" "${LIB_DIR}/*.cpp") - file(GLOB SOURCES_C ABSOLUTE "${LIB_DIR}" "${LIB_DIR}/*.c") - target_sources(${TARGET_NAME} PRIVATE "${SOURCES_CPP}") - target_sources(${TARGET_NAME} PRIVATE "${SOURCES_C}") -endfunction(import_arduino_library) diff --git a/test/32bit.toolchain.cmake b/test/32bit.toolchain.cmake new file mode 100644 index 0000000..7f5fc9c --- /dev/null +++ b/test/32bit.toolchain.cmake @@ -0,0 +1,10 @@ +set(CMAKE_SYSTEM_NAME Linux) + +# Compile as 32 bit code +add_compile_options("-m32") +link_libraries("-m32") + +if (CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options("-Wno-deprecated-register" "-Wno-unknown-attributes") +endif() + diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..3265fe1 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,21 @@ + +set(TEST_SOURCE_FILES + main_test.cpp + platform_mocks.cpp + test_pulse_processor.cpp +) + +# Compile CMSIS as a library. +set(CMSIS_ROOT "${CMAKE_SOURCE_DIR}/libs/CMSIS/CMSIS" CACHE PATH "Path to the CMSIS root directory") +file(GLOB_RECURSE CMSIS_CORE_FILES "${CMSIS_ROOT}/DSP_Lib/Source/*_f32.c") +set(CMSIS_CORE_FILES "${CMSIS_CORE_FILES}" "${CMSIS_ROOT}/DSP_Lib/Source/CommonTables/arm_common_tables.c") +add_library(cmsis STATIC EXCLUDE_FROM_ALL "${CMSIS_CORE_FILES}") +target_compile_definitions(cmsis PUBLIC "ARM_MATH_CM4") +target_include_directories(cmsis PUBLIC "${CMSIS_ROOT}/Include") + +# We have only one test executable +add_executable(main-test "${TEST_SOURCE_FILES}") +target_include_directories(main-test PUBLIC "../libs/Catch") +target_link_libraries(main-test sensor-core) + +add_test(NAME test COMMAND main-test) diff --git a/test/platform_mocks.cpp b/test/platform_mocks.cpp index cb326ea..12f30a8 100644 --- a/test/platform_mocks.cpp +++ b/test/platform_mocks.cpp @@ -4,40 +4,18 @@ #include "input.h" #include #include -#include void set_led_state(LedState state) { // Do nothing } - -Timestamp Timestamp::cur_time() { - return 0; +void update_led_pattern(Timestamp cur_time) { + } -uint32_t Timestamp::get_value(TimeUnit tu) const { - // TODO: Make platform-independent. +Timestamp Timestamp::cur_time() { return 0; } -size_t Print::write(const uint8_t *buffer, size_t size) { - // Do nothing +uint32_t Timestamp::cur_time_millis() { return 0; } - -int Print::printf(const char *format, ...) { - char buf[256]; - va_list args; - va_start(args, format); - int res = vsnprintf(buf, sizeof(buf), format, args); - if (res > 0) { - write(buf, res); - } - va_end(args); - return res; -} - -std::unique_ptr createInputCmpNode(uint32_t input_idx, const InputDef &input_def) { - return nullptr; -} - -