From e62413b1195814c6e470a3348b133188a71048e6 Mon Sep 17 00:00:00 2001 From: nizhnerk Date: Sat, 3 Oct 2026 19:20:18 -0400 Subject: [PATCH 1/7] Initial port: bring Teensy codebase over from sailboat to boat. --- .gitignore | 25 +- teensy/.gitignore | 3 + teensy/README.md | 88 +++++ teensy/platformio.ini | 47 +++ teensy/src/ControlTasks/LedControlTask.cpp | 32 ++ teensy/src/ControlTasks/LedControlTask.hpp | 11 + teensy/src/ControlTasks/ServoControlTask.cpp | 181 ++++++++++ teensy/src/ControlTasks/ServoControlTask.hpp | 23 ++ .../src/ControlTasks/TelemetryControlTask.cpp | 29 ++ .../src/ControlTasks/TelemetryControlTask.hpp | 13 + teensy/src/MainControlLoop.cpp | 17 + teensy/src/MainControlLoop.hpp | 20 ++ teensy/src/Monitors/AnemometerMonitor.cpp | 14 + teensy/src/Monitors/AnemometerMonitor.hpp | 8 + teensy/src/Monitors/RadioSerialMonitor.cpp | 9 + teensy/src/Monitors/RadioSerialMonitor.hpp | 10 + teensy/src/Monitors/SerialMonitorBase.cpp | 15 + teensy/src/Monitors/SerialMonitorBase.hpp | 59 ++++ teensy/src/Monitors/USBSerialMonitor.cpp | 11 + teensy/src/Monitors/USBSerialMonitor.hpp | 10 + teensy/src/constants.hpp | 90 +++++ teensy/src/main.cpp | 12 + teensy/src/sfr.cpp | 27 ++ teensy/src/sfr.hpp | 29 ++ teensy/test/README.md | 106 ++++++ teensy/test/mocks/Arduino.h | 222 ++++++++++++ teensy/test/mocks/Servo.h | 63 ++++ teensy/test/run_tests.sh | 23 ++ teensy/test/test_firmware/anemometer.cpp | 83 +++++ teensy/test/test_firmware/main.cpp | 26 ++ teensy/test/test_firmware/serial_framing.cpp | 257 ++++++++++++++ teensy/test/test_firmware/servo_control.cpp | 322 ++++++++++++++++++ teensy/test/test_firmware/suites.hpp | 13 + teensy/test/test_firmware/telemetry.cpp | 127 +++++++ teensy/test/test_firmware/test_support.h | 173 ++++++++++ 35 files changed, 2187 insertions(+), 11 deletions(-) create mode 100644 teensy/.gitignore create mode 100644 teensy/README.md create mode 100644 teensy/platformio.ini create mode 100644 teensy/src/ControlTasks/LedControlTask.cpp create mode 100644 teensy/src/ControlTasks/LedControlTask.hpp create mode 100644 teensy/src/ControlTasks/ServoControlTask.cpp create mode 100644 teensy/src/ControlTasks/ServoControlTask.hpp create mode 100644 teensy/src/ControlTasks/TelemetryControlTask.cpp create mode 100644 teensy/src/ControlTasks/TelemetryControlTask.hpp create mode 100644 teensy/src/MainControlLoop.cpp create mode 100644 teensy/src/MainControlLoop.hpp create mode 100644 teensy/src/Monitors/AnemometerMonitor.cpp create mode 100644 teensy/src/Monitors/AnemometerMonitor.hpp create mode 100644 teensy/src/Monitors/RadioSerialMonitor.cpp create mode 100644 teensy/src/Monitors/RadioSerialMonitor.hpp create mode 100644 teensy/src/Monitors/SerialMonitorBase.cpp create mode 100644 teensy/src/Monitors/SerialMonitorBase.hpp create mode 100644 teensy/src/Monitors/USBSerialMonitor.cpp create mode 100644 teensy/src/Monitors/USBSerialMonitor.hpp create mode 100644 teensy/src/constants.hpp create mode 100644 teensy/src/main.cpp create mode 100644 teensy/src/sfr.cpp create mode 100644 teensy/src/sfr.hpp create mode 100644 teensy/test/README.md create mode 100644 teensy/test/mocks/Arduino.h create mode 100644 teensy/test/mocks/Servo.h create mode 100755 teensy/test/run_tests.sh create mode 100644 teensy/test/test_firmware/anemometer.cpp create mode 100644 teensy/test/test_firmware/main.cpp create mode 100644 teensy/test/test_firmware/serial_framing.cpp create mode 100644 teensy/test/test_firmware/servo_control.cpp create mode 100644 teensy/test/test_firmware/suites.hpp create mode 100644 teensy/test/test_firmware/telemetry.cpp create mode 100644 teensy/test/test_firmware/test_support.h diff --git a/.gitignore b/.gitignore index e0c092a..2cbb281 100644 --- a/.gitignore +++ b/.gitignore @@ -16,20 +16,23 @@ msg/_*.py build_isolated/ devel_isolated/ -# Generated by dynamic reconfigure +# Generated by dynamic reconfigure. *.cfgc /cfg/cpp/ /cfg/*.py -# Ignore generated docs +# Idea. +.idea/ + +# Generated docs. *.dox *.wikidoc -# eclipse stuff +# Eclipse. .project .cproject -# qcreator stuff +# Qcreator. CMakeLists.txt.user srv/_*.py @@ -44,30 +47,30 @@ qtcreator-* *~ -# Emacs +# Emacs. .#* -# Catkin custom files +# Catkin custom files. CATKIN_IGNORE -# ROS 2 generated artifacts +# ROS2 generated artifacts. install/ log/ __pycache__/ -# ROS workspace metadata +# ROS workspace metadata. .rosinstall .ament_version colcon.meta -# System logs +# System logs. sys/sys.log sys/ros.out -# CV models +# CV models. *.pt -# Miscellaneous +# Miscellaneous. *.DS_Store *.swp *.swo diff --git a/teensy/.gitignore b/teensy/.gitignore new file mode 100644 index 0000000..42814b9 --- /dev/null +++ b/teensy/.gitignore @@ -0,0 +1,3 @@ +.pio/ +.vscode/ +.idea/ \ No newline at end of file diff --git a/teensy/README.md b/teensy/README.md new file mode 100644 index 0000000..18b9d13 --- /dev/null +++ b/teensy/README.md @@ -0,0 +1,88 @@ +# Overview + +The `teensy` directory includes the code for our Teensy 4.0 microcontroller, which serves as the low-level controller +of our sailboat. The code manages the sensors, actuators (such as the servos for the sail and rudder), and communicates +with the Jetson via a serial connection. + +--- + + +## Source Code (`src/`) +This code is structured based on [Lodestar](https://github.com/shihaocao/lodestar), a small scale electric demonstrator for the belly-flop and +tail-sitting control algorithms necessary for SpaceX's Starship. + +### main.cpp +This file is comparable to a `.ino` file you would see in the Arduino IDE (notice setup and loop are exactly the same as +they would be in an Arduino file). + +### MainControlLoop +The MainControlLoop initializes and executes all monitors and control tasks. + +### SFR +SFR stands for State Field Registry. It contains values that should be available to the entire boat (sensor values, +serial buffer data, etc). + +### Monitors +Monitors read input from some source and update corresponding values in the SFR. + +### Control Tasks +Control tasks perform actions based on the current state of the boat or SFR values. + +### constants.hpp +This file contains values that will never be dynamically changed (mainly physical parameters for a specific boat). This +prevents "magic numbers" in the codebase. + + +## Testing (`test/`) +A directory intended for PlatformIO Test Runner and project tests. The tests currently included here run locally, with +no need to have a Teensy plugged in. Run them from the `test/` directory with the command `./run_tests.sh`. + +See [test/README.md](test/README.md) for how more on how this test suite works and what is covered. + +--- + + +## Getting Started +Below are the steps to set up your development environment to upload code and observe serial outputs from the Teensy. + +### Prerequisites: +- [VSCode](https://code.visualstudio.com/download) or [CLion](https://www.jetbrains.com/clion/) is installed. +- The [sailbot](https://github.com/CUSail-Navigation/sailbot) repository is cloned. + +### Steps: +1. In VSCode, click "Extensions" on the left-hand side toolbar and search for PlatformIO IDE. + In CLion, click "File → Plugins" and search for PlatformIO for CLion. +2. Open the `teensy/` folder within the sailbot repository. Make sure the `teensy/` folder is the project root. +3. (VSCode) At the bottom of your screen in the blue toolbar, you should see a check, arrow, and serial monitor icon. + - If you would just like to compile code but not upload to the Teensy, press the check. + - If you would like to upload to the Teensy, press the arrow. + - To view the serial monitor, press the electrical cord icon. + + +## Developing with a Teensy with Docker & WSL on Windows: +The following steps expose a Windows COM port to WSL and then expose the WSL port to the Docker image running in WSL. +This was necessary to set up a test environment with the ROS2 codebase on a Windows 11 computer. + +### Changing Docker Desktop Backend to support WSL +Make sure you have: +- [Docker Desktop](https://docs.docker.com/desktop/setup/install/windows-install/) installed. +- [WSL](https://learn.microsoft.com/en-us/windows/wsl/install) installed. + +1. Open Docker Desktop. +2. Navigate to Settings → General. +3. Check the box "Use the WSL 2 based engine". + +### Expose Windows COM port to WSL +1. Download and install [USBIPD-WIN](https://github.com/dorssel/usbipd-win/) (follow their README.md instructions). +2. Open PowerShell as an administrator. +3. Obtain a list of USB devices using `usbipd list`. +4. Find the bus ID of the device (e.g. 4-4) and use `usbipd bind --busid ` to share it with WSL. +5. Use `usbipd attach --wsl --busid ` to attach the USB port to WSL. +6. In WSL, you can use the command `lsusb` to see the device. + +### Expose WSL port to Docker image +1. Run `ls /dev` or `lsusb` to view ports accessible by WSL. The Teensy will most likely appear as `/dev/ttyACM0`. +2. Run the following command in WSL to expose the shared port with the docker image: +``` +docker run -it --rm --name ros2_container -v $(pwd)/src:/home/ros2_user/ros2_ws/src --device= ros2_humble_custom +``` diff --git a/teensy/platformio.ini b/teensy/platformio.ini new file mode 100644 index 0000000..3ae622b --- /dev/null +++ b/teensy/platformio.ini @@ -0,0 +1,47 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + + +; ---- DEFAULT: only firmware environment built (`pio run` compiles firmware for Teensy; never touches test env). ---- +[platformio] +default_envs = teensy40 + + +; ---- FIRMWARE: what actually gets flashed onto the Teensy. ---- +[env:teensy40] +platform = teensy@4.18 +board = teensy40 +framework = arduino +build_unflags = -std=gnu++14 +build_flags = + -std=gnu++17 + -I .pio/libdeps/native/Unity/src + + +; ---- TESTS: run locally (no board needs to be plugged in). ---- +; Note: `-I test/mocks` puts the `test/mocks/` directory first on the include path, to use for our testing. +[env:native] +platform = native +test_framework = unity +build_flags = + -std=gnu++17 + -Wall + -Wextra + -Wno-unused-parameter + -I src + -I test/mocks + -I .pio/libdeps/native/Unity/src + +; 1) Compile src/ into test binary (off by default) so tests exercise the real firmware code rather than a copy. +; 2) Exclude main.cpp only (has Arduino setup()/loop() entry points + MCL; each test suite supplies its own main()). +; 3) Folders named test_* treated as test suites (test/mocks/ should be ignored and used just as an include directory). +test_build_src = yes +build_src_filter = +<*> - +test_filter = test_* diff --git a/teensy/src/ControlTasks/LedControlTask.cpp b/teensy/src/ControlTasks/LedControlTask.cpp new file mode 100644 index 0000000..283c95b --- /dev/null +++ b/teensy/src/ControlTasks/LedControlTask.cpp @@ -0,0 +1,32 @@ +#include "LedControlTask.hpp" + +LedControlTask::LedControlTask() : LED_PIN(constants::led::LED_PIN) { + pinMode(LED_PIN, OUTPUT); +} + +/** + * Blinks the status LED to indicate the current connectivity/data state of the boat. + */ +void LedControlTask::execute() const { + const bool update_servos = sfr::serial::update_servos_radio || sfr::serial::update_servos_usb; + if (Serial.available() && !update_servos) { + digitalWrite(LED_PIN, LOW); + delay(2000); + digitalWrite(LED_PIN, HIGH); + delay(2000); + } + else if (update_servos) { + digitalWrite(LED_PIN, HIGH); + delay(1000); + } + else if (!Serial.available()) { + digitalWrite(LED_PIN, LOW); + delay(1000); + } + else { + digitalWrite(LED_PIN, LOW); + delay(500); + digitalWrite(LED_PIN, HIGH); + delay(500); + } +} diff --git a/teensy/src/ControlTasks/LedControlTask.hpp b/teensy/src/ControlTasks/LedControlTask.hpp new file mode 100644 index 0000000..cc59876 --- /dev/null +++ b/teensy/src/ControlTasks/LedControlTask.hpp @@ -0,0 +1,11 @@ +#pragma once +#include "sfr.hpp" + +class LedControlTask { +public: + LedControlTask(); + void execute() const; + +private: + const uint8_t LED_PIN; +}; diff --git a/teensy/src/ControlTasks/ServoControlTask.cpp b/teensy/src/ControlTasks/ServoControlTask.cpp new file mode 100644 index 0000000..a628f9e --- /dev/null +++ b/teensy/src/ControlTasks/ServoControlTask.cpp @@ -0,0 +1,181 @@ +#include "ServoControlTask.hpp" + +/** + * Construct a \code ServoControlTask\endcode and initialize all servos to default values. + */ +ServoControlTask::ServoControlTask() { + rudder_servo.attach(constants::servo::RUDDER_PIN, constants::servo::RUDDER_MIN_PULSE, constants::servo::RUDDER_MAX_PULSE); + mainsail_servo.attach(constants::servo::MAINSAIL_PIN, constants::servo::MAINSAIL_MIN_PULSE, constants::servo::MAINSAIL_MAX_PULSE); + jib_port_servo.attach(constants::servo::JIB_PORT_PIN, constants::servo::JIB_PORT_MIN_PULSE, constants::servo::JIB_PORT_MAX_PULSE); + jib_stb_servo.attach(constants::servo::JIB_STB_PIN, constants::servo::JIB_STB_MIN_PULSE, constants::servo::JIB_STB_MAX_PULSE); + + // Pre-set the rudder to center and all sail servos to "all-out" (for manual setup). + actuate_servo(rudder_servo, constants::servo::RUDDER_MID_PULSE); + actuate_servo(mainsail_servo, constants::servo::MAINSAIL_MAX_PULSE); + actuate_servo(jib_port_servo, constants::servo::JIB_PORT_MAX_PULSE); + actuate_servo(jib_stb_servo, constants::servo::JIB_STB_MAX_PULSE); +} + +/** + * Update the servos if valid serial data is ready and waiting.

+ * Note that radio mode ( \code radio_flag != 0\endcode ) takes priority over USB mode. Flags differ between serial + * modes, but the resultant servo behavior should be identical. + * + * - Radio buffer layout: \code [radio_flag, mainsail_angle, rudder_angle, jib_angle, jib_side_flag]\endcode + * - USB buffer layout: \code [mainsail_angle, rudder_angle, jib_angle, jib_side_flag]\endcode + */ +void ServoControlTask::execute() { + if (sfr::serial::radio_flag != 0) { // RADIO MODE. + if (!sfr::serial::update_servos_radio) return; + apply_commands(sfr::serial::radio_buffer[1], sfr::serial::radio_buffer[2], + sfr::serial::radio_buffer[3], sfr::serial::radio_buffer[4]); + sfr::serial::update_servos_radio = false; + } else if (sfr::serial::update_servos_usb) { // USB MODE. + apply_commands(sfr::serial::usb_buffer[0], sfr::serial::usb_buffer[1], + sfr::serial::usb_buffer[2], sfr::serial::usb_buffer[3]); + sfr::serial::update_servos_usb = false; + } +} + +/** A utility method to actually apply values to servos, based on the received data and if checks on the data pass. */ +void ServoControlTask::apply_commands(const uint8_t mainsail_angle, const uint8_t rudder_angle, + const uint8_t jib_angle, const uint8_t jib_side_flag) { + if (mainsail_angle >= constants::servo::MAINSAIL_MIN_ANGLE && mainsail_angle <= constants::servo::MAINSAIL_MAX_ANGLE) { + sfr::servo::mainsail_angle = mainsail_angle; + sfr::servo::mainsail_pwm = mainsail_to_pwm(mainsail_angle); + actuate_servo(mainsail_servo, sfr::servo::mainsail_pwm); + } + if (rudder_angle >= constants::servo::RUDDER_MIN_ANGLE && rudder_angle <= constants::servo::RUDDER_MAX_ANGLE) { + sfr::servo::rudder_angle = rudder_angle; + sfr::servo::rudder_pwm = rudder_to_pwm(rudder_angle); + actuate_servo(rudder_servo, sfr::servo::rudder_pwm); + } + if (jib_angle >= constants::servo::JIB_MIN_ANGLE && jib_angle <= constants::servo::JIB_MAX_ANGLE && + (jib_side_flag == constants::servo::JIB_SIDE_PORT || jib_side_flag == constants::servo::JIB_SIDE_STB)) { + sfr::servo::jib_angle = jib_angle; + sfr::servo::jib_side_flag = jib_side_flag; + + if (jib_side_flag == constants::servo::JIB_SIDE_PORT) { + sfr::servo::jib_stb_pwm = constants::servo::JIB_STB_MAX_PULSE; + actuate_servo(jib_stb_servo, constants::servo::JIB_STB_MAX_PULSE); + sfr::servo::jib_port_pwm = jib_to_pwm(jib_angle, jib_side_flag); + actuate_servo(jib_port_servo, sfr::servo::jib_port_pwm); + } else { + sfr::servo::jib_port_pwm = constants::servo::JIB_PORT_MAX_PULSE; + actuate_servo(jib_port_servo, constants::servo::JIB_PORT_MAX_PULSE); + sfr::servo::jib_stb_pwm = jib_to_pwm(jib_angle, jib_side_flag); + actuate_servo(jib_stb_servo, sfr::servo::jib_stb_pwm); + } + } +} + +/** Maps a goal rudder angle to a \code rudder_servo\endcode PWM. + * + * @param angle the goal angle to set the rudder to. + * @return the PWM to actuate \code rudder_servo\endcode to. + */ +uint32_t ServoControlTask::rudder_to_pwm(const uint8_t angle) { + return map(angle, constants::servo::RUDDER_MIN_ANGLE, constants::servo::RUDDER_MAX_ANGLE, + constants::servo::RUDDER_MIN_PULSE,constants::servo::RUDDER_MAX_PULSE); +} + +/** Maps a goal mainsail angle to a \code mainsail_servo\endcode PWM. + * + * @param angle the goal angle to set the mainsail to. + * @return the PWM to actuate \code mainsail_servo\endcode to. + */ +uint32_t ServoControlTask::mainsail_to_pwm(const uint8_t angle) { + const uint32_t new_pwm = law_of_cos_map(angle, + constants::servo::TWO_BOOM_LEN_SQD_CM, + constants::servo::MAINSAIL_PULSE_PER_TURN, + constants::servo::MAINSAIL_WHEEL_CIRCUM_CM, + true + ) + constants::servo::MAINSAIL_MIN_PULSE; + return new_pwm <= constants::servo::MAINSAIL_MAX_PULSE ? new_pwm : constants::servo::MAINSAIL_MAX_PULSE; +} + +/** Maps a goal sail angle for the jib to a PWM value (for one of the two jib servos). + * + * @param angle the goal angle to set the jib to (on the appropriate side of the boat). + * @param jib_side_flag which servo we are calculating a PWM value for (which side we are setting the jib to). + * @return the PWM to actuate the appropriate servo to. + */ +uint32_t ServoControlTask::jib_to_pwm(const uint8_t angle, const uint8_t jib_side_flag) { + const float wheel_circum = (jib_side_flag == constants::servo::JIB_SIDE_PORT) ? + constants::servo::JIB_PORT_WHEEL_CIRCUM_CM : constants::servo::JIB_STB_WHEEL_CIRCUM_CM; + const uint32_t min_pulse = (jib_side_flag == constants::servo::JIB_SIDE_PORT) ? + constants::servo::JIB_PORT_MIN_PULSE : constants::servo::JIB_STB_MIN_PULSE; + const uint32_t max_pulse = (jib_side_flag == constants::servo::JIB_SIDE_PORT) ? + constants::servo::JIB_PORT_MAX_PULSE : constants::servo::JIB_STB_MAX_PULSE; + const uint32_t new_pwm = law_of_cos_map(angle, + constants::servo::TWO_JIB_FOOT_LEN_SQD_CM, + constants::servo::JIB_PULSE_PER_TURN, + wheel_circum, + false + ) + min_pulse; + return new_pwm <= max_pulse ? new_pwm : max_pulse; +} + +/** Send \code pwm\endcode to \code servo\endcode, thereby changing \code servo\endcode 's angle. */ +void ServoControlTask::actuate_servo(Servo& servo, const uint32_t pwm) { + servo.write(static_cast(pwm)); +} + +/** A utility method that uses the law of cosines to map a goal sail angle to a servo PWM value. + * Used for both the mainsail servo and the jib servos. + * + * Extended explanation as follows: + * - The law of cosines states \code c^2 = a^2 + b^2 - 2ab*cos(angle)\endcode + * - In an isosceles triangle where a = b, and "angle" is the angle between these two legs, this simplifies to + * \code c^2 = 2b^2(1 - cos(angle))\endcode + * - Consider the mainsail, namely looking down at the boom from high above the boat: + * - "angle" refers to the angle that the boom makes with the centerline of the boat; namely, the goal angle we want + * the mainsail to be set to. + * - Pivoting the boom around the mast traces out an isosceles triangle: the length of the legs ("b") is the length + * of the section of the boom from the mast to where the mainsheet attaches. + * - "c" is the component of the mainsheet length in the plane that the boom rotates in. + * - We must take the initial length of the mainsheet, \code MAINSAIL_INITIAL_CM\endcode, into account; we therefore + * apply the Pythagorean theorem on the right triangle formed by "c", the initial distance from the deck to the + * boom, and the actual mainsheet, to obtain the actual goal length of the mainsheet. + * - We model similarly for the jib (note that this is less accurate as there is no "boom" for the jib, and it is + * therefore harder to quantify a triangle or angles in general. This is why we multiply by a slight fractional offset + * when trimming the jib). + * - "angle" refers to the goal angle we want the jib to be set to. + * - We approximate "b" as the length of the foot of the jib. + * - Here, "c" on its own is approximately the length of the sheet, which is what we want to solve for. + * - In either case, this final \code sheet_len\endcode value will map to a PWM value based on the specific servo and + * the circumference of its wheel. + * + * Note that this method assumes that the max PWM value of the servo in question allows for the sail to be set to + * \code angle\endcode. If this is not the case, it will return a value greater than the servo's PWM range. + */ +uint32_t ServoControlTask::law_of_cos_map(const uint8_t angle, const uint32_t two_b_sqd, const float PWM_per_turn, + const float wheel_circum, const bool mainsail) { + const float c_squared = static_cast(two_b_sqd) * (1 - cosf(static_cast(angle) * 0.017453f)); // 0.017453 = pi/180. + const float sheet_len = mainsail ? sqrtf(c_squared + constants::servo::MAINSAIL_INITIAL_SQD_CM) - constants::servo::MAINSAIL_INITIAL_CM + : sqrtf(c_squared) * constants::servo::JIB_CALIBRATION; + + return static_cast(PWM_per_turn * (sheet_len / wheel_circum)); // PWM: (PWM_per_turn * turns_needed). +} + + +/** + * A testing utility used for mapping goal angles to specific servo PWM values. + * Prints these values in the format : which can be directly copy-pasted into the 2025-2026 servo testbench. + */ /* +#include +[[noreturn]] int main() { + while (true) { + std::string servo; + int angle = 0; + + std::cout << "Enter servo and angle: "; + std::cin >> servo; + std::cin >> angle; + + if (servo == "mainsail") std::cout << "1:" << ServoControlTask::mainsail_to_pwm(angle) << std::endl; + else if (servo == "rudder") std::cout << "2:" << ServoControlTask::rudder_to_pwm(angle + 45) << std::endl; + else if (servo == "jib_port") std::cout << "3:" << ServoControlTask::jib_to_pwm(angle, 0) << std::endl; + else if (servo == "jib_stb") std::cout << "4:" << ServoControlTask::jib_to_pwm(angle, 1) << std::endl; + } +} */ diff --git a/teensy/src/ControlTasks/ServoControlTask.hpp b/teensy/src/ControlTasks/ServoControlTask.hpp new file mode 100644 index 0000000..fbb4709 --- /dev/null +++ b/teensy/src/ControlTasks/ServoControlTask.hpp @@ -0,0 +1,23 @@ +#pragma once +#include +#include "sfr.hpp" + +class ServoControlTask { +public: + ServoControlTask(); + void execute(); + +private: + Servo rudder_servo; + Servo mainsail_servo; + Servo jib_port_servo; + Servo jib_stb_servo; + + static uint32_t rudder_to_pwm(uint8_t angle); + static uint32_t mainsail_to_pwm(uint8_t angle); + static uint32_t jib_to_pwm(uint8_t angle, uint8_t jib_side_flag); + static void actuate_servo(Servo &servo, uint32_t pwm); + + void apply_commands(uint8_t mainsail_angle, uint8_t rudder_angle, uint8_t jib_angle, uint8_t jib_side_flag); + static uint32_t law_of_cos_map(uint8_t angle, uint32_t two_b_sqd, float PWM_per_turn, float wheel_circum, bool mainsail); +}; diff --git a/teensy/src/ControlTasks/TelemetryControlTask.cpp b/teensy/src/ControlTasks/TelemetryControlTask.cpp new file mode 100644 index 0000000..4c7d2e4 --- /dev/null +++ b/teensy/src/ControlTasks/TelemetryControlTask.cpp @@ -0,0 +1,29 @@ +#include "TelemetryControlTask.hpp" + +TelemetryControlTask::TelemetryControlTask() : last_telemetry_send_time(0), current_time(0), send_telemetry(false) {} + +/** + * Sends a telemetry packet of SFR data to the computer connected via \code Serial\endcode every + * \code TX_PERIOD_MS\endcode. + */ +void TelemetryControlTask::execute() { + if (current_time - last_telemetry_send_time >= constants::serial::TX_PERIOD_MS) send_telemetry = true; + + if (send_telemetry) { + const uint8_t data[] = { + constants::serial::TX_START_FLAG, + static_cast(sfr::anemometer::wind_angle >> 8), + static_cast(sfr::anemometer::wind_angle & 0xFF), + sfr::servo::mainsail_angle, + sfr::servo::rudder_angle, + sfr::servo::jib_angle, + sfr::servo::jib_side_flag, + sfr::serial::dropped_packets, + constants::serial::TX_END_FLAG + }; + last_telemetry_send_time = millis(); + Serial.write(data, sizeof(data)); + send_telemetry = false; + } + current_time = millis(); +} diff --git a/teensy/src/ControlTasks/TelemetryControlTask.hpp b/teensy/src/ControlTasks/TelemetryControlTask.hpp new file mode 100644 index 0000000..06c62d7 --- /dev/null +++ b/teensy/src/ControlTasks/TelemetryControlTask.hpp @@ -0,0 +1,13 @@ +#pragma once +#include "sfr.hpp" + +class TelemetryControlTask { +public: + TelemetryControlTask(); + void execute(); + +private: + uint32_t last_telemetry_send_time; + uint32_t current_time; + bool send_telemetry; +}; diff --git a/teensy/src/MainControlLoop.cpp b/teensy/src/MainControlLoop.cpp new file mode 100644 index 0000000..ea7b489 --- /dev/null +++ b/teensy/src/MainControlLoop.cpp @@ -0,0 +1,17 @@ +#include "MainControlLoop.hpp" + +/** Delay startup by 1 second to let hardware/peripherals stabilize before the main loop begins. */ +MainControlLoop::MainControlLoop() { + delay(1000); +} + +/** + * Runs a single iteration of the boat's control loop, in the given order. + */ +void MainControlLoop::execute() { + anemometer_monitor.execute(); + radio_serial_monitor.execute(); + usb_serial_monitor.execute(); + servo_control_task.execute(); + telemetry_control_task.execute(); +} diff --git a/teensy/src/MainControlLoop.hpp b/teensy/src/MainControlLoop.hpp new file mode 100644 index 0000000..b5ada88 --- /dev/null +++ b/teensy/src/MainControlLoop.hpp @@ -0,0 +1,20 @@ +#pragma once +#include "Monitors/AnemometerMonitor.hpp" +#include "Monitors/RadioSerialMonitor.hpp" +#include "Monitors/USBSerialMonitor.hpp" +#include "ControlTasks/ServoControlTask.hpp" +#include "ControlTasks/TelemetryControlTask.hpp" +#include "sfr.hpp" + +class MainControlLoop { +public: + MainControlLoop(); + void execute(); + +protected: + AnemometerMonitor anemometer_monitor; + RadioSerialMonitor radio_serial_monitor; + USBSerialMonitor usb_serial_monitor; + ServoControlTask servo_control_task; + TelemetryControlTask telemetry_control_task; +}; diff --git a/teensy/src/Monitors/AnemometerMonitor.cpp b/teensy/src/Monitors/AnemometerMonitor.cpp new file mode 100644 index 0000000..9787202 --- /dev/null +++ b/teensy/src/Monitors/AnemometerMonitor.cpp @@ -0,0 +1,14 @@ +#include "AnemometerMonitor.hpp" + +AnemometerMonitor::AnemometerMonitor() { + pinMode(constants::anemometer::ANEMOMETER_PIN, INPUT); +} + +/** + * Reads the anemometer's analog voltage and converts it to a wind angle in degrees. + */ +void AnemometerMonitor::execute() { + // Note that 0.3515625 = 360/1024 -- maps the analogRead() range of 0-1023 onto 0-359 degrees. + sfr::anemometer::wind_angle = static_cast(0.3515625 * analogRead(constants::anemometer::ANEMOMETER_PIN)); + //Serial.println(sfr::anemometer::wind_angle); // Print for testing. +} diff --git a/teensy/src/Monitors/AnemometerMonitor.hpp b/teensy/src/Monitors/AnemometerMonitor.hpp new file mode 100644 index 0000000..f31deba --- /dev/null +++ b/teensy/src/Monitors/AnemometerMonitor.hpp @@ -0,0 +1,8 @@ +#pragma once +#include "sfr.hpp" + +class AnemometerMonitor { +public: + AnemometerMonitor(); + void execute(); +}; diff --git a/teensy/src/Monitors/RadioSerialMonitor.cpp b/teensy/src/Monitors/RadioSerialMonitor.cpp new file mode 100644 index 0000000..1085f5d --- /dev/null +++ b/teensy/src/Monitors/RadioSerialMonitor.cpp @@ -0,0 +1,9 @@ +#include "RadioSerialMonitor.hpp" + +/** Reads and assembles incoming radio packets from \code Serial2\endcode into \code radio_buffer\endcode. */ +void RadioSerialMonitor::execute() { + if (read_packet(Serial2, temp_buffer, sfr::serial::radio_buffer, constants::serial::RADIO_BUFFER_LEN)) { + sfr::serial::update_servos_radio = true; + sfr::serial::radio_flag = sfr::serial::radio_buffer[0]; + } +} diff --git a/teensy/src/Monitors/RadioSerialMonitor.hpp b/teensy/src/Monitors/RadioSerialMonitor.hpp new file mode 100644 index 0000000..93de424 --- /dev/null +++ b/teensy/src/Monitors/RadioSerialMonitor.hpp @@ -0,0 +1,10 @@ +#pragma once +#include "SerialMonitorBase.hpp" + +class RadioSerialMonitor : public SerialMonitorBase { +public: + void execute() override; + +private: + uint8_t temp_buffer[constants::serial::RADIO_BUFFER_LEN] = {}; +}; diff --git a/teensy/src/Monitors/SerialMonitorBase.cpp b/teensy/src/Monitors/SerialMonitorBase.cpp new file mode 100644 index 0000000..b42e26a --- /dev/null +++ b/teensy/src/Monitors/SerialMonitorBase.cpp @@ -0,0 +1,15 @@ +#include "SerialMonitorBase.hpp" + +SerialMonitorBase::SerialMonitorBase() : buffer_index(0), packet_started(false), packet_start_time(0) {} + +/** Helper method used to indicate dropping a stale packet. */ +void SerialMonitorBase::drop_packet() { + buffer_index = 0; + packet_started = false; + sfr::serial::dropped_packets++; +} + +/** Returns \code true\endcode when an in-progress packet has exceeded the RX timeout. */ +bool SerialMonitorBase::packet_timed_out() const { + return packet_started && (millis() - packet_start_time > constants::serial::RX_PACKET_TIMEOUT_MS); +} diff --git a/teensy/src/Monitors/SerialMonitorBase.hpp b/teensy/src/Monitors/SerialMonitorBase.hpp new file mode 100644 index 0000000..a1a3ed2 --- /dev/null +++ b/teensy/src/Monitors/SerialMonitorBase.hpp @@ -0,0 +1,59 @@ +#pragma once +#include "sfr.hpp" +#include + +/** An abstract base class: stores shared functionality for serial monitors that assemble packets. */ +class SerialMonitorBase { +public: + virtual void execute() = 0; + +protected: + uint8_t buffer_index; + bool packet_started; + uint32_t packet_start_time; + + SerialMonitorBase(); + virtual ~SerialMonitorBase() = default; + + void drop_packet(); + [[nodiscard]] bool packet_timed_out() const; + + /** + * Reads and assembles incoming serial packets from \code port\endcode into \code sfr_buffer\endcode, byte by byte. + * Returns \code true\endcode if at least one full packet is assembled during this call; the last of such packets is + * reflected in \code sfr_buffer\endcode. + * + * Valid packets must begin with \code RX_START_FLAG\endcode, end with \code RX_END_FLAG\endcode, and contain exactly + * \code len\endcode bytes. Malformed packets that do not follow this structure, or packets that stall for longer + * than \code RX_PACKET_TIMEOUT_MS\endcode, are dropped. + */ + template bool read_packet(Port& port, uint8_t* temp_buffer, uint8_t* sfr_buffer, const uint8_t len) { + if (packet_timed_out()) drop_packet(); + + bool completed = false; + while (port.available()) { + if (packet_timed_out()) drop_packet(); + + if (const uint8_t incoming_byte = port.read(); incoming_byte == constants::serial::RX_START_FLAG) { + buffer_index = 0; + packet_started = true; + packet_start_time = millis(); + } + else if (packet_started && incoming_byte != constants::serial::RX_END_FLAG) { + if (buffer_index < len) temp_buffer[buffer_index++] = incoming_byte; + else drop_packet(); // Packet is incorrect (buffer is full, but we have not reached RX_END_FLAG). + } + else if (packet_started && incoming_byte == constants::serial::RX_END_FLAG) { + if (buffer_index == len) { + std::copy_n(temp_buffer, len, sfr_buffer); + buffer_index = 0; + packet_started = false; + completed = true; + } + else drop_packet(); // Packet is incorrect (buffer is not full, but we have reached RX_END_FLAG). + } + } + + return completed; + } +}; diff --git a/teensy/src/Monitors/USBSerialMonitor.cpp b/teensy/src/Monitors/USBSerialMonitor.cpp new file mode 100644 index 0000000..b11d3eb --- /dev/null +++ b/teensy/src/Monitors/USBSerialMonitor.cpp @@ -0,0 +1,11 @@ +#include "USBSerialMonitor.hpp" + +/** + * Reads and assembles incoming packets from the computer connected via \code Serial\endcode into + * \code usb_buffer\endcode. + */ +void USBSerialMonitor::execute() { + if (read_packet(Serial, temp_buffer, sfr::serial::usb_buffer, constants::serial::USB_BUFFER_LEN)) { + sfr::serial::update_servos_usb = true; + } +} diff --git a/teensy/src/Monitors/USBSerialMonitor.hpp b/teensy/src/Monitors/USBSerialMonitor.hpp new file mode 100644 index 0000000..365fc1f --- /dev/null +++ b/teensy/src/Monitors/USBSerialMonitor.hpp @@ -0,0 +1,10 @@ +#pragma once +#include "SerialMonitorBase.hpp" + +class USBSerialMonitor : public SerialMonitorBase { +public: + void execute() override; + +private: + uint8_t temp_buffer[constants::serial::USB_BUFFER_LEN] = {}; +}; diff --git a/teensy/src/constants.hpp b/teensy/src/constants.hpp new file mode 100644 index 0000000..ce06687 --- /dev/null +++ b/teensy/src/constants.hpp @@ -0,0 +1,90 @@ +#pragma once +#include + +namespace constants { + namespace anemometer { + constexpr uint8_t ANEMOMETER_PIN = 18; + } + /** PHYSICAL SERVO NOTES AND CONVENTIONS FROM 2025-2026 SEASON: + * - All servos can go from 600 PWM (a "SMALL" angle) to 2400 PWM (a "LARGE" angle). + * - In general, we choose ~800 as a baseline PWM to represent a minimum angle so that, just in case something + * physical changes on the boat, we can recalibrate the PWM that corresponds to this angle by dipping below this + * baseline (until reaching \code SERVO_MIN_PULSE\endcode), and avoiding needing to re-screw the servos. + * - Rudder: This servo can turn 0.5 times, but mech did something to cut this in half, so that the rudder will only + * go from -45 degrees (600 PWM) to 45 degrees (2400 PWM). + * - Mainsail: This servo can turn 7.85 times. + * - Jib: We have two servos, one for each side of the boat. Both servos can turn 7.85 times. + * + * NOTE: Any constants that are labeled \code TODO\endcode are ones that we plan to make runtime-changeable over + * the mobile app. This functionality is not yet implemented. + */ + namespace servo { + constexpr uint8_t RUDDER_PIN = 4; + constexpr uint8_t MAINSAIL_PIN = 3; + constexpr uint8_t JIB_PORT_PIN = 5; + constexpr uint8_t JIB_STB_PIN = 9; + + constexpr uint32_t SERVO_MIN_PULSE = 600; + constexpr uint32_t SERVO_MAX_PULSE = 2400; + + constexpr uint32_t RUDDER_MIN_PULSE = 600; //TODO + constexpr uint32_t RUDDER_MAX_PULSE = 2400; //TODO + + constexpr uint32_t JIB_PORT_MIN_PULSE = 750; //TODO + constexpr uint32_t JIB_PORT_MAX_PULSE = 1750; //TODO + + constexpr uint32_t JIB_STB_MIN_PULSE = 850; //TODO + constexpr uint32_t JIB_STB_MAX_PULSE = 2200; //TODO + + constexpr uint32_t MAINSAIL_MIN_PULSE = 700; + constexpr uint32_t MAINSAIL_MAX_PULSE = 1800; + + constexpr uint8_t RUDDER_MIN_ANGLE = 0; // (2025-2026) 0-90 range = -45 to +45 degrees. //TODO + constexpr uint8_t RUDDER_MAX_ANGLE = 90; //TODO + constexpr uint32_t RUDDER_MID_PULSE = (RUDDER_MIN_PULSE + RUDDER_MAX_PULSE) / 2; // Amidships. + + constexpr uint8_t MAINSAIL_MIN_ANGLE = 0; //TODO + constexpr uint8_t MAINSAIL_MAX_ANGLE = 90; //TODO + constexpr uint32_t TWO_BOOM_LEN_SQD_CM = 2 * 92 * 92; // (2025-2026) Boom length (mast to mainsheet) is 92cm. //TODO + constexpr uint32_t MAINSAIL_INITIAL_CM = 18; // (2025-2026) Mainsheet length from deck to end of boom is 18cm. //TODO + constexpr uint32_t MAINSAIL_INITIAL_SQD_CM = MAINSAIL_INITIAL_CM * MAINSAIL_INITIAL_CM; + constexpr float MAINSAIL_WHEEL_CIRCUM_CM = 16.242; // (2025-2026) Diameter: 5.17cm. //TODO + constexpr float MAINSAIL_PULSE_PER_TURN = (SERVO_MAX_PULSE - SERVO_MIN_PULSE) / 7.85; + + constexpr uint8_t JIB_MIN_ANGLE = 0; //TODO + constexpr uint8_t JIB_MAX_ANGLE = 90; //TODO + constexpr uint32_t TWO_JIB_FOOT_LEN_SQD_CM = 2 * 60 * 60; // (2025-2026) Jib foot length is 60cm. //TODO + constexpr float JIB_PORT_WHEEL_CIRCUM_CM = 16.242; // (2025-2026) Diameter: 5.17cm. //TODO + constexpr float JIB_STB_WHEEL_CIRCUM_CM = 16.242; // (2025-2026) Diameter: 5.17cm. //TODO + constexpr float JIB_PULSE_PER_TURN = (SERVO_MAX_PULSE - SERVO_MIN_PULSE) / 7.85; + constexpr float JIB_CALIBRATION = 0.85; + + constexpr uint8_t JIB_SIDE_PORT = 0; + constexpr uint8_t JIB_SIDE_STB = 1; + } + /** SERIAL NOTES FROM 2025-2026 SEASON:

+ * - USB BUFFER FORMAT (which is RX PACKET FORMAT between the start and end flags): + * - [0] = \code mainsail_angle\endcode + * - [1] = \code rudder_angle\endcode + * - [2] = \code jib_angle\endcode + * - [3] = \code jib_side_flag\endcode ( \code JIB_SIDE_PORT\endcode or \code JIB_SIDE_STB\endcode ) + * - TX PACKET FORMAT: [start_flag, wind_hi, wind_lo, mainsail_angle, rudder_angle, + * jib_angle, jib_side_flag, dropped_packets, end_flag]. + */ + namespace serial { + constexpr uint8_t TX_START_FLAG = 0XFF; + constexpr uint8_t TX_END_FLAG = 0xEE; + constexpr uint32_t TX_PERIOD_MS = 500; + + constexpr uint8_t RX_START_FLAG = 0xFF; + constexpr uint8_t RX_END_FLAG = 0xEE; + constexpr uint8_t RX_PACKET_TIMEOUT_MS = 50; + constexpr uint8_t USB_BUFFER_LEN = 4; + constexpr uint8_t RADIO_BUFFER_LEN = 5; + + constexpr uint32_t BAUD_RATE = 9600; + } + namespace led { + constexpr uint8_t LED_PIN = 13; + } +} diff --git a/teensy/src/main.cpp b/teensy/src/main.cpp new file mode 100644 index 0000000..726ce32 --- /dev/null +++ b/teensy/src/main.cpp @@ -0,0 +1,12 @@ +#include "MainControlLoop.hpp" + +MainControlLoop mcl; + +void setup(){ + Serial.begin(constants::serial::BAUD_RATE); // (2025-2026) The Jetson, via USB. + Serial2.begin(constants::serial::BAUD_RATE); // (2025-2026) The XBee, on pins 7/8. +} + +void loop() { + mcl.execute(); +} diff --git a/teensy/src/sfr.cpp b/teensy/src/sfr.cpp new file mode 100644 index 0000000..c0cf3d1 --- /dev/null +++ b/teensy/src/sfr.cpp @@ -0,0 +1,27 @@ +#include "sfr.hpp" + +/** Initialize all fields here; these values will be changed immediately when relevant. */ +namespace sfr { + namespace anemometer { + uint16_t wind_angle = 0; + } + namespace servo { + uint8_t rudder_angle = 0; + uint8_t mainsail_angle = 0; + uint8_t jib_angle = 0; + uint8_t jib_side_flag = 0; + + uint32_t rudder_pwm = 0; + uint32_t mainsail_pwm = 0; + uint32_t jib_port_pwm = 0; + uint32_t jib_stb_pwm = 0; + } + namespace serial { + bool update_servos_usb = false; + bool update_servos_radio = false; + uint8_t usb_buffer[constants::serial::USB_BUFFER_LEN] = {}; + uint8_t radio_buffer[constants::serial::RADIO_BUFFER_LEN] = {}; + uint8_t radio_flag = 1; + uint8_t dropped_packets = 0; + } +} diff --git a/teensy/src/sfr.hpp b/teensy/src/sfr.hpp new file mode 100644 index 0000000..eacf89b --- /dev/null +++ b/teensy/src/sfr.hpp @@ -0,0 +1,29 @@ +#pragma once +#include "constants.hpp" + +/** SFR stands for State Field Registry. It contains all sensor values and + * universal flags that should be available to the entire boat. */ +namespace sfr { + namespace anemometer { + extern uint16_t wind_angle; + } + namespace servo { + extern uint8_t rudder_angle; + extern uint8_t mainsail_angle; + extern uint8_t jib_angle; + extern uint8_t jib_side_flag; + + extern uint32_t rudder_pwm; + extern uint32_t mainsail_pwm; + extern uint32_t jib_port_pwm; + extern uint32_t jib_stb_pwm; + } + namespace serial { + extern bool update_servos_usb; + extern bool update_servos_radio; + extern uint8_t usb_buffer[constants::serial::USB_BUFFER_LEN]; + extern uint8_t radio_buffer[constants::serial::RADIO_BUFFER_LEN]; + extern uint8_t radio_flag; + extern uint8_t dropped_packets; + } +} diff --git a/teensy/test/README.md b/teensy/test/README.md new file mode 100644 index 0000000..4e4d53e --- /dev/null +++ b/teensy/test/README.md @@ -0,0 +1,106 @@ +# Teensy Tests + +Code logic unit testing for the Teensy firmware that runs locally, with no need to have a Teensy plugged in. + +_Note that to run tests locally, having a local C++17 compiler is required: typically `g++` or `clang++` on Linux/macOS, +and MinGW/MSVC on Windows._ + +--- + + +## Running Tests +_Make sure you have an internet connection the first time you run these tests: PlatformIO will auto-download the +`native` platform and the `Unity` test framework._ + +Included in the `test/` directory is a script (`run_tests.sh`) that you can use to run the entire test suite. From the +project root directory, `teensy/`, you can run: +```bash +./test/run_tests.sh +``` +The less robust but functional raw command is simply: +```bash +pio test -e native +``` + +### IDE integration +It is also possible to run the test with one click in an editor, without using the command line. +- VSCode: PlatformIO Icon (in sidebar) → Project Tasks → native → Advanced → Test +- CLion: create a run configuration for a shell script. Set "script path" to `test/run_tests.sh` and + "working directory" to the project root `teensy/`. + +### Troubleshooting +- "`Error: could not find the 'pio' command`": PlatformIO Core is not installed, or is not on `PATH` (`run_tests.sh` + already falls back to `~/.platformio/penv/bin/pio`). +- A compiler error (mentioning `Arduino.h` or `Servo.h`): new source code is using functions that the mocks do not yet + implement. Add these to the appropriate file in (`test/mocks/`); they only cover what the firmware currently calls. +- A test passes alone but fails in the suite: almost always leaked global state. Check that `setUp()` calls +`reset_all()`, and that any new SFR field is reset in `reset_sfr()`. + + +## Organization and Adding Tests +### Structure +All tests build into a single suite, but are split across one file per topic in `test/test_firmware/`. +- PlatformIO runs each folder under `test/` as its own binary needing its own `main()`. +- One folder thus means one build/link/process. Each topic exposes a single `run__tests()` called by `main.cpp`. +- For more information about what is covered by the full suite, read the documentation in each topic file. + +There are also a bunch of shared useful functions organized in the `test_firmware/test_support.h` file. + +### Extending Existing Topics +Add a `static void test_something()` function to the relevant file, put your testing logic in this function, and add a +matching `RUN_TEST()` line in that file's `run__tests()` function. + +### Adding New Topics +Create a new file (`test/test_firmware/.cpp`), following the templates of the topics already in the +`test/test_firmware/` directory. Make sure to declare its run function in `suites.hpp` and call it from `main.cpp` to +have the tests actually run. + +--- + + +## Mocks +The firmware needs and uses `Arduino.h` and `Servo.h` for functions like `millis()`, `Serial`, `analogRead()`. +Those only exist when cross-compiling for the Teensy, implying that tests could only run with hardware attached. + +To avoid this issue and make it possible to test locally, `test/mocks/` contains stand-ins named exactly `Arduino.h` and +`Servo.h`. This means `#include ` in source code resolves to the **mock** when building tests and to the +**genuine Teensy header** when building firmware. In both cases, the production code is compiled unmodified: nothing in +`src/` is aware that the fakes for testing exist. + + +Some advantages that the mocks bring over real hardware: + - A clock we can manipulate by hand: timeouts become instant and deterministic rather than requiring real waiting. + - Serial ports to feed bytes into: `Serial.mock_rx({...})` queues bytes as if the Jetson/XBee had sent them, and + `Serial.mock_tx()` shows exactly what the firmware wrote back. + - We can see what each servo was commanded to do, including whether it moved at all, without needing actual servos. + - All mocks are header-only and use C++17 `inline` variables: there is no separate `.cpp` file to keep in sync and no + multiple-definition problems regardless of how many files include them. +--- + + +## Additional Information +1. **PlatformIO supports testing with a board plugged in, which is more faithful, but less practical.** + - This suite tests code logic and does not require the actual boat or Teensy board. + - Actual hardware-specific behavior (servo movement, real Serial communication) is *not* covered by these tests. + This test suite therefore cannot replace actual boat-testing. + - `MainControlLoop` is untested: while the individual monitors and tasks are tested, the order they run in is not. +2. **For proper encapsulation, there is no direct unit test of a private method anywhere in this suite: private methods + are covered indirectly. This suite is more about testing functionality class by class.** + - The workaround used instead is to reach private methods *through* the public `execute()` -- the `apply_commands()` + method writes every value these helpers compute into the SFR, so a test can stage a command, call `execute()`, and + read the computed PWM back out. + - The mock `Servo` class records what reaches each pin, confirming numbers actually get sent to the right servo. + - Some resulting consequences to be aware of: + - Failures point appear at the wrong place (a broken `law_of_cos_map()` surfaces as a failing `execute()` + assertion; you have to work backwards to the error yourself). + - Coverage is coupled to the caller (if `execute()` ever stops routing through one of these helpers, its tests + might pass quietly while the helper degrades). +3. **Convention: no magic numbers. Use `constants.hpp`.** + - Everything in `constants.hpp` is boat calibration, which changes with a new boat. A test must never hardcode one + of these values, or it would have to be manually updated with every new boat. + - Instead, the tests assert *relationships* that hold under any calibration. For example: + - The minimum angle maps to `MIN_PULSE`; the maximum to `MAX_PULSE`. + - Every servo is expected to be rigged so a larger commanded angle means a larger pulse width. + - PWM output should never escape `[MIN_PULSE, MAX_PULSE]`. + - A serial packet of exactly `_BUFFER_LEN` payload bytes is accepted; anything shorter or longer is dropped. +4. **For more information about PlatformIO Unit Testing, vist this [link](https://docs.platformio.org/en/latest/advanced/unit-testing/index.html).** diff --git a/teensy/test/mocks/Arduino.h b/teensy/test/mocks/Arduino.h new file mode 100644 index 0000000..0e18833 --- /dev/null +++ b/teensy/test/mocks/Arduino.h @@ -0,0 +1,222 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// Pin modes and logic levels. +#define HIGH 1 +#define LOW 0 +#define INPUT 0 +#define OUTPUT 1 +#define INPUT_PULLUP 2 + + +// Simulated clock (firmware's packet timeouts are driven entirely by millis(), so tests advance this manually). +inline uint32_t g_mock_millis = 0; + +/** Returns the current fake time, in milliseconds. */ +inline uint32_t millis() { + return g_mock_millis; +} + +/** Returns the current fake time in microseconds, derived from the millisecond clock. */ +inline uint32_t micros() { + return g_mock_millis * 1000; +} + +/** Jumps the fake clock to an absolute time, \code ms\endcode. */ +inline void mock_set_millis(const uint32_t ms) { + g_mock_millis = ms; +} + +/** Moves the fake clock forward by \code ms\endcode. Use this to trip \code RX_PACKET_TIMEOUT_MS\endcode. */ +inline void mock_advance_millis(const uint32_t ms) { + g_mock_millis += ms; +} + +/** Simulates a delay without sleeping: only moves the fake clock forward by \code ms\endcode. */ +inline void delay(const uint32_t ms) { + g_mock_millis += ms; +} + +/** Accept and ignore this call -- the fake clock has millisecond resolution and cannot represent this. */ +inline void delayMicroseconds(const uint32_t) {} + +/** Accept and ignore this call -- there is no background work to yield to on the host. */ +inline void yield() {} + + +// Digital/analog IO. Writes are recorded so a test can assert on them; reads return whatever the test staged. +inline constexpr size_t MOCK_PIN_COUNT = 64; + +inline int g_mock_pin_modes[MOCK_PIN_COUNT] = {}; +inline int g_mock_pin_levels[MOCK_PIN_COUNT] = {}; +inline int g_mock_analog_values[MOCK_PIN_COUNT] = {}; + +/** Records the mode that a pin was configured to. */ +inline void pinMode(const uint8_t pin, const int mode) { + if (pin < MOCK_PIN_COUNT) g_mock_pin_modes[pin] = mode; +} + +/** Records a logic level written to a pin. */ +inline void digitalWrite(const uint8_t pin, const int level) { + if (pin < MOCK_PIN_COUNT) g_mock_pin_levels[pin] = level; +} + +/** Returns the last logic level written to a pin, or 0 if it was never written. */ +inline int digitalRead(const uint8_t pin) { + return pin < MOCK_PIN_COUNT ? g_mock_pin_levels[pin] : 0; +} + +/** Returns the analog value a test staged for a pin, or 0 if none was staged. */ +inline int analogRead(const uint8_t pin) { + return pin < MOCK_PIN_COUNT ? g_mock_analog_values[pin] : 0; +} + +/** Records an analog value written to a pin, in the same slot \code analogRead()\endcode reads back from. */ +inline void analogWrite(const uint8_t pin, const int value) { + if (pin < MOCK_PIN_COUNT) g_mock_analog_values[pin] = value; +} + +/** Stages the value \code analogRead(pin)\endcode will return, such as a raw anemometer ADC reading. */ +inline void mock_set_analog(const uint8_t pin, const int value) { + if (pin < MOCK_PIN_COUNT) g_mock_analog_values[pin] = value; +} + +/** Reads back the last level written by \code digitalWrite(pin)\endcode, for asserting on pin state. */ +inline int mock_pin_level(const uint8_t pin) { + return pin < MOCK_PIN_COUNT ? g_mock_pin_levels[pin] : 0; +} + + +// Specific implementation for the map() method. +/** + * A faithful port of the Teensy 4 core's \code map()\endcode -- NOT the simpler classic Arduino one, which leaves out + * the round-off correction below and so returns different values. This is one of the few Teensy functions whose + * internals have to be matched exactly here, because \code rudder_to_pwm()\endcode maps angles through it. + */ +template +long map(T _x, A _in_min, B _in_max, C _out_min, D _out_max, + typename std::enable_if::value>::type* = 0) { + long x = _x, in_min = _in_min, in_max = _in_max, out_min = _out_min, out_max = _out_max; + + const long in_range = in_max - in_min; + const long out_range = out_max - out_min; + if (in_range == 0) return out_min + out_range / 2; + + long num = (x - in_min) * out_range; + if (out_range >= 0) num += in_range / 2; // Round off towards zero. + else num -= in_range / 2; + + const long result = num / in_range + out_min; + + // Teensy's fix for "a strange behaviour with negative numbers" (ArduinoCore-API issue #51). + if (out_range >= 0) { + if (in_range * num < 0) return result - 1; + } else { + if (in_range * num >= 0) return result + 1; + } + return result; +} + + +// Serial ports: stand in for both USB port to the Jetson and the hardware UART to the XBee. +class FakeStream { + std::deque rx_queue; + std::vector tx_log; + +public: + /** Accept and ignore this call -- there is no real baud rate to configure. */ + void begin(unsigned long /*baud*/) {} + + /** Accept and ignore this call -- there is no real port to close. */ + void end() {} + + /** Returns how many queued bytes are waiting to be read. */ + int available() { + return static_cast(rx_queue.size()); + } + + /** Pops one queued byte, or returns -1 when empty (matching the real Stream contract). */ + int read() { + if (rx_queue.empty()) return -1; + const int byte = rx_queue.front(); + rx_queue.pop_front(); + return byte; + } + + /** Returns the next queued byte without consuming it, or -1 when empty. */ + int peek() const { + return rx_queue.empty() ? -1 : rx_queue.front(); + } + + /** Appends one byte to the transmit log, reporting the single byte written. */ + size_t write(const uint8_t byte) { + tx_log.push_back(byte); + return 1; + } + + /** Appends \code size\endcode bytes to the transmit log, reporting how many were written. */ + size_t write(const uint8_t* buffer, const size_t size) { + tx_log.insert(tx_log.end(), buffer, buffer + size); + return size; + } + + /** Accept and ignore this call -- writes to the log are never buffered. */ + void flush() {} + + /** Discards printed output; exists only so that debug prints in the firmware still compile under test. */ + template size_t print(const T&) { + return 0; + } + + /** Discards printed output, as \code print()\endcode does. */ + template size_t println(const T&) { + return 0; + } + + /** Discards a bare newline, as \code print()\endcode does. */ + size_t println() { + return 0; + } + + + // Test-only hooks below (how a test feeds bytes in and reads back what the firmware sent out). + /** Queues bytes as if the peer had just transmitted them. */ + void mock_rx(const std::initializer_list bytes) { + for (const uint8_t byte : bytes) rx_queue.push_back(byte); + } + + /** Queues \code count\endcode bytes from a buffer, for feeding in a packet built at runtime. */ + void mock_rx(const uint8_t* bytes, const size_t count) { + for (size_t i = 0; i < count; ++i) rx_queue.push_back(bytes[i]); + } + + /** Queues a single byte, for building up a partial packet one byte at a time. */ + void mock_rx_byte(const uint8_t byte) { rx_queue.push_back(byte); } + + /** Returns everything the firmware has written to this port since the last reset. */ + const std::vector& mock_tx() const { return tx_log; } + + /** Returns how many queued bytes the firmware has not consumed yet. */ + size_t mock_rx_pending() const { return rx_queue.size(); } + + /** Empties both the receive queue and the transmit log, so that the next test starts clean. */ + void mock_clear() { + rx_queue.clear(); + tx_log.clear(); + } +}; + +/** The Jetson link (USB serial): read by \code USBSerialMonitor\endcode, written by \code TelemetryControlTask\endcode. */ +inline FakeStream Serial; + +/** The XBee radio link (hardware UART on pins 7/8): read by \code RadioSerialMonitor\endcode. */ +inline FakeStream Serial2; diff --git a/teensy/test/mocks/Servo.h b/teensy/test/mocks/Servo.h new file mode 100644 index 0000000..94fc335 --- /dev/null +++ b/teensy/test/mocks/Servo.h @@ -0,0 +1,63 @@ +#pragma once +#include "Arduino.h" + +// Per-pin records of what each servo was commanded to do, so tests can observe servo activity. +inline int g_mock_servo_last_write[MOCK_PIN_COUNT] = {}; +inline int g_mock_servo_write_count[MOCK_PIN_COUNT] = {}; + +/** Clears both records above, so one test cannot see the servo commands issued by the previous one. */ +inline void mock_reset_servos() { + for (size_t pin = 0; pin < MOCK_PIN_COUNT; ++pin) { + g_mock_servo_last_write[pin] = 0; + g_mock_servo_write_count[pin] = 0; + } +} + + +// Stand-in for the Arduino Servo class. +class Servo { + int attached_pin = -1; + int last_value = 0; + bool is_attached = false; + +public: + /** Records which pin this servo drives, ignoring the pulse bounds (nothing is able to read them back). */ + void attach(const int pin, const int /*min_pulse*/, const int /*max_pulse*/) { + attach(pin); + } + + /** Records which pin this servo drives. */ + void attach(const int pin) { + attached_pin = pin; + is_attached = true; + } + + /** Marks this servo as no longer driving its pin. */ + void detach() { + is_attached = false; + } + + /** Returns whether \code attach()\endcode has been called without a later \code detach()\endcode. */ + bool attached() const { + return is_attached; + } + + /** Records a commanded pulse width, both on this instance and against the pin it is attached to. */ + void write(const int value) { + last_value = value; + if (attached_pin >= 0 && static_cast(attached_pin) < MOCK_PIN_COUNT) { + g_mock_servo_last_write[attached_pin] = value; + ++g_mock_servo_write_count[attached_pin]; + } + } + + /** Records a pulse width given explicitly in microseconds (identical to \code write()\endcode for this mock). */ + void writeMicroseconds(const int value) { + write(value); + } + + /** Returns the last value passed to \code write()\endcode. */ + int read() const { + return last_value; + } +}; diff --git a/teensy/test/run_tests.sh b/teensy/test/run_tests.sh new file mode 100755 index 0000000..2de9904 --- /dev/null +++ b/teensy/test/run_tests.sh @@ -0,0 +1,23 @@ +#!/usr/bin/env bash +# A script to actually run the Teensy unit test suite. Some options: +# ./run_tests.sh # run everything +# ./run_tests.sh -v # verbose (shows the compiler command lines) +# Note that anything you pass is forwarded to `pio test`. + + +set -euo pipefail # Error handling. +cd "$(dirname "$0")/.." # Script lives in test/, but pio has to run from the project root, where platformio.ini is. + +# PlatformIO setup/verification. Note that it is often installed to ~/.platformio/penv/bin rather than onto PATH. +if command -v pio > /dev/null 2>&1; then + PIO=pio +elif [ -x "$HOME/.platformio/penv/bin/pio" ]; then + PIO="$HOME/.platformio/penv/bin/pio" +else + echo "Error: could not find the 'pio' command." >&2 + echo "Install PlatformIO Core: https://docs.platformio.org/en/latest/core/installation/index.html" >&2 + exit 127 +fi + +# Actually run the tests. "-e native" pins this to host-side environment (never tries to talk to a Teensy). +exec "$PIO" test -e native "$@" diff --git a/teensy/test/test_firmware/anemometer.cpp b/teensy/test/test_firmware/anemometer.cpp new file mode 100644 index 0000000..ae032fa --- /dev/null +++ b/teensy/test/test_firmware/anemometer.cpp @@ -0,0 +1,83 @@ +/** + * ANEMOMETER TESTS -- AnemometerMonitor. + * This file tests the properties of the AnemometerMonitor that the rest of the boat relies on (including that the + * reading is a compass bearing, it never wraps past a full circle, and it tracks the sensor in the right direction). + */ +#include "test_support.h" +#include "suites.hpp" +#include "Monitors/AnemometerMonitor.hpp" + + +// Constants and helper functions. +static constexpr int ADC_MAX = 1023; +static constexpr uint16_t FULL_CIRCLE_DEGREES = 360; + +/** Take one reading, with the ADC staged at \code raw\endcode. */ +static uint16_t read_wind_angle_for(AnemometerMonitor& monitor, const int raw) { + mock_set_analog(constants::anemometer::ANEMOMETER_PIN, raw); + monitor.execute(); + return sfr::anemometer::wind_angle; +} + + +// Tests. +static void test_zero_reading_is_zero_degrees() { + AnemometerMonitor monitor; + TEST_ASSERT_EQUAL_UINT16_MESSAGE(0, read_wind_angle_for(monitor, 0), + "A zero ADC reading should be a zero-degree bearing"); +} + +static void test_every_reading_stays_within_one_revolution() { + AnemometerMonitor monitor; + + for (int raw = 0; raw <= ADC_MAX; raw++) { + const uint16_t angle = read_wind_angle_for(monitor, raw); + if (angle >= FULL_CIRCLE_DEGREES) { + char message[128]; + snprintf(message, sizeof(message), "ADC reading %d produced %u degrees, which is not a valid bearing", + raw, static_cast(angle)); + TEST_FAIL_MESSAGE(message); + } + } +} + +static void test_angle_rises_with_sensor_reading() { + AnemometerMonitor monitor; + std::vector sweep; + + for (int raw = 0; raw <= ADC_MAX; raw += 16) sweep.push_back(read_wind_angle_for(monitor, raw)); + + assert_non_decreasing(sweep, "wind angle"); + TEST_ASSERT_GREATER_THAN_UINT32_MESSAGE(sweep.front(), sweep.back(), + "A full-scale reading should differ from a zero reading"); +} + +static void test_full_scale_reading_covers_almost_whole_circle() { + AnemometerMonitor monitor; + const uint16_t angle = read_wind_angle_for(monitor, ADC_MAX); + + TEST_ASSERT_LESS_THAN_UINT16(FULL_CIRCLE_DEGREES, angle); + TEST_ASSERT_GREATER_THAN_UINT16_MESSAGE(FULL_CIRCLE_DEGREES - 5, angle, + "A full-scale reading should map to nearly a full circle"); +} + +static void test_reading_vane_isolated() { + AnemometerMonitor monitor; + read_wind_angle_for(monitor, ADC_MAX / 2); + + TEST_ASSERT_EQUAL_UINT8(0, sfr::serial::dropped_packets); + TEST_ASSERT_FALSE(sfr::serial::update_servos_usb); + TEST_ASSERT_FALSE(sfr::serial::update_servos_radio); + TEST_ASSERT_EQUAL_UINT32(0, sfr::servo::rudder_pwm); +} + + +// Runner. +void run_anemometer_tests() { + Unity.TestFile = __FILE__; // Report failures against this file, not main.cpp. + RUN_TEST(test_zero_reading_is_zero_degrees); + RUN_TEST(test_every_reading_stays_within_one_revolution); + RUN_TEST(test_angle_rises_with_sensor_reading); + RUN_TEST(test_full_scale_reading_covers_almost_whole_circle); + RUN_TEST(test_reading_vane_isolated); +} diff --git a/teensy/test/test_firmware/main.cpp b/teensy/test/test_firmware/main.cpp new file mode 100644 index 0000000..ef6c0fd --- /dev/null +++ b/teensy/test/test_firmware/main.cpp @@ -0,0 +1,26 @@ +#include "test_support.h" +#include "suites.hpp" + +/** + * This method is the entry point for the Teensy unit tests. + * + * The test framework \code Unity\endcode requires exactly one \code main()\endcode, one \code setup()\endcode, and one +* \code tearDown()\endcode per binary, so they are centrally organized here. The function \code setup()\endcode runs +* before every test in every topic (wipes the SFR and mocks so no test can inherit state from whatever ran before it). + */ +int main(int, char**) { + UNITY_BEGIN(); + + run_serial_framing_tests(); + run_servo_control_tests(); + run_telemetry_tests(); + run_anemometer_tests(); + + return UNITY_END(); +} + +void setUp() { + reset_all(); +} + +void tearDown() {} diff --git a/teensy/test/test_firmware/serial_framing.cpp b/teensy/test/test_firmware/serial_framing.cpp new file mode 100644 index 0000000..78bd1ff --- /dev/null +++ b/teensy/test/test_firmware/serial_framing.cpp @@ -0,0 +1,257 @@ +/** + * SERIAL MONITOR TESTS -- SerialMonitorBase, USBSerialMonitor, RadioSerialMonitor. + * Tests in this file cover the byte-level state machine that turns a stream of serial bytes into a validated packet. + */ +#include "test_support.h" +#include "suites.hpp" +#include "Monitors/USBSerialMonitor.hpp" +#include "Monitors/RadioSerialMonitor.hpp" + + +// Assumptions the framing depends on -- if one of these ever fails, the other tests in this file are meaningless. +static void test_framing_flags_are_distinct() { + TEST_ASSERT_NOT_EQUAL_MESSAGE(constants::serial::RX_START_FLAG, constants::serial::RX_END_FLAG, + "Start and end flags must differ or packets cannot be delimited"); +} + +static void test_buffers_are_non_empty() { + TEST_ASSERT_GREATER_THAN_size_t(0, constants::serial::USB_BUFFER_LEN); + TEST_ASSERT_GREATER_THAN_size_t(0, constants::serial::RADIO_BUFFER_LEN); +} + + +// Tests that validate everything went well. +static void test_usb_valid_packet_is_accepted() { + const std::vector payload = make_payload(constants::serial::USB_BUFFER_LEN); + feed(Serial, frame_packet(payload)); + + USBSerialMonitor monitor; + monitor.execute(); + + TEST_ASSERT_TRUE_MESSAGE(sfr::serial::update_servos_usb, "A well-formed USB packet should raise the update flag"); + TEST_ASSERT_EQUAL_UINT8_ARRAY(payload.data(), sfr::serial::usb_buffer, constants::serial::USB_BUFFER_LEN); + TEST_ASSERT_EQUAL_UINT8_MESSAGE(0, sfr::serial::dropped_packets, "A valid packet must not count as dropped"); +} + +static void test_radio_valid_packet_is_accepted() { + const std::vector payload = make_payload(constants::serial::RADIO_BUFFER_LEN); + feed(Serial2, frame_packet(payload)); + + RadioSerialMonitor monitor; + monitor.execute(); + + TEST_ASSERT_TRUE_MESSAGE(sfr::serial::update_servos_radio, "A well-formed radio packet should raise the flag"); + TEST_ASSERT_EQUAL_UINT8_ARRAY(payload.data(), sfr::serial::radio_buffer, constants::serial::RADIO_BUFFER_LEN); + TEST_ASSERT_EQUAL_UINT8(0, sfr::serial::dropped_packets); +} + +static void test_radio_publishes_mode_flag_from_payload() { + std::vector payload = make_payload(constants::serial::RADIO_BUFFER_LEN); + payload[layout::RADIO_FLAG] = 0; + feed(Serial2, frame_packet(payload)); + + RadioSerialMonitor monitor; + monitor.execute(); + + TEST_ASSERT_EQUAL_UINT8_MESSAGE(0, sfr::serial::radio_flag, + "radio_flag must mirror payload byte 0 so the boat can switch out of radio mode"); +} + +static void test_packet_split_across_execute_calls_still_completes() { + const std::vector payload = make_payload(constants::serial::USB_BUFFER_LEN); + const std::vector packet = frame_packet(payload); + const size_t split = packet.size() / 2; + + USBSerialMonitor monitor; + + // First half arrives; the packet is still mid-flight so nothing should be published yet. + Serial.mock_rx(packet.data(), split); + monitor.execute(); + TEST_ASSERT_FALSE_MESSAGE(sfr::serial::update_servos_usb, "A half-received packet must not be published"); + + // Remainder arrives on a later loop iteration. + Serial.mock_rx(packet.data() + split, packet.size() - split); + monitor.execute(); + + TEST_ASSERT_TRUE_MESSAGE(sfr::serial::update_servos_usb, "The packet should complete once the rest arrives"); + TEST_ASSERT_EQUAL_UINT8_ARRAY(payload.data(), sfr::serial::usb_buffer, constants::serial::USB_BUFFER_LEN); + TEST_ASSERT_EQUAL_UINT8(0, sfr::serial::dropped_packets); +} + +static void test_back_to_back_packets_both_parse() { + const std::vector first = make_payload(constants::serial::USB_BUFFER_LEN); + std::vector second = make_payload(constants::serial::USB_BUFFER_LEN); + second[0] = static_cast(second[0] + 1); // Make the second packet distinguishable. + if (second[0] == constants::serial::RX_START_FLAG || second[0] == constants::serial::RX_END_FLAG) second[0] += 1; + + feed(Serial, frame_packet(first)); + feed(Serial, frame_packet(second)); + + USBSerialMonitor monitor; + monitor.execute(); + + TEST_ASSERT_TRUE(sfr::serial::update_servos_usb); + TEST_ASSERT_EQUAL_UINT8(0, sfr::serial::dropped_packets); + TEST_ASSERT_EQUAL_UINT8_ARRAY_MESSAGE(second.data(), sfr::serial::usb_buffer, constants::serial::USB_BUFFER_LEN, + "The most recent packet should win when several arrive in one pass"); +} + + +// Tests that validate what happens with malformed packets. +static void test_short_packet_is_dropped() { + const std::vector payload = make_payload(constants::serial::USB_BUFFER_LEN - 1); + feed(Serial, frame_packet(payload)); + + USBSerialMonitor monitor; + monitor.execute(); + + TEST_ASSERT_FALSE_MESSAGE(sfr::serial::update_servos_usb, "A truncated packet must never be published"); + TEST_ASSERT_EQUAL_UINT8_MESSAGE(1, sfr::serial::dropped_packets, "Truncated packets should be counted as dropped"); +} + +static void test_overlong_packet_is_dropped() { + const std::vector payload = make_payload(constants::serial::USB_BUFFER_LEN + 1); + feed(Serial, frame_packet(payload)); + + USBSerialMonitor monitor; + monitor.execute(); + + TEST_ASSERT_FALSE_MESSAGE(sfr::serial::update_servos_usb, "An over-long packet must never be published"); + TEST_ASSERT_EQUAL_UINT8_MESSAGE(1, sfr::serial::dropped_packets, "Over-long packets should be counted as dropped"); +} + +static void test_radio_short_packet_is_dropped() { + const std::vector payload = make_payload(constants::serial::RADIO_BUFFER_LEN - 1); + feed(Serial2, frame_packet(payload)); + + RadioSerialMonitor monitor; + monitor.execute(); + + TEST_ASSERT_FALSE(sfr::serial::update_servos_radio); + TEST_ASSERT_EQUAL_UINT8(1, sfr::serial::dropped_packets); +} + +static void test_dropped_packet_counter_accumulates() { + USBSerialMonitor monitor; + + feed(Serial, frame_packet(make_payload(constants::serial::USB_BUFFER_LEN - 1))); + monitor.execute(); + feed(Serial, frame_packet(make_payload(constants::serial::USB_BUFFER_LEN + 1))); + monitor.execute(); + + TEST_ASSERT_EQUAL_UINT8_MESSAGE(2, sfr::serial::dropped_packets, "Each rejected packet bumps the drop counter"); +} + +static void test_stray_bytes_outside_a_packet_are_ignored() { + Serial.mock_rx({safe_payload_byte(0), safe_payload_byte(1), constants::serial::RX_END_FLAG}); + + USBSerialMonitor monitor; + monitor.execute(); + + TEST_ASSERT_FALSE(sfr::serial::update_servos_usb); + TEST_ASSERT_EQUAL_UINT8_MESSAGE(0, sfr::serial::dropped_packets, "Noise outside a packet is not a dropped packet"); +} + +static void test_start_flag_mid_packet_restarts_cleanly() { + const std::vector payload = make_payload(constants::serial::USB_BUFFER_LEN); + + std::vector stream; + stream.push_back(constants::serial::RX_START_FLAG); + stream.push_back(safe_payload_byte(200)); // Represents a partial payload that gets abandoned. + const std::vector restarted = frame_packet(payload); + stream.insert(stream.end(), restarted.begin(), restarted.end()); + feed(Serial, stream); + + USBSerialMonitor monitor; + monitor.execute(); + + TEST_ASSERT_TRUE_MESSAGE(sfr::serial::update_servos_usb, "The restarted packet should parse normally"); + TEST_ASSERT_EQUAL_UINT8_ARRAY(payload.data(), sfr::serial::usb_buffer, constants::serial::USB_BUFFER_LEN); +} + + +// Tests that validate stale packet timeouts. +static void test_stalled_packet_times_out_and_is_dropped() { + USBSerialMonitor monitor; + + // A packet starts arriving but stops partway through. + Serial.mock_rx({constants::serial::RX_START_FLAG, safe_payload_byte(0)}); + monitor.execute(); + TEST_ASSERT_EQUAL_UINT8_MESSAGE(0, sfr::serial::dropped_packets, "Nothing is stale yet"); + + // Time passes with no further bytes, then the rest finally shows up far too late. + mock_advance_millis(constants::serial::RX_PACKET_TIMEOUT_MS + 1); + const std::vector payload = make_payload(constants::serial::USB_BUFFER_LEN); + Serial.mock_rx(payload.data() + 1, payload.size() - 1); + Serial.mock_rx_byte(constants::serial::RX_END_FLAG); + monitor.execute(); + + TEST_ASSERT_EQUAL_UINT8_MESSAGE(1, sfr::serial::dropped_packets, "The stalled packet should be dropped"); + TEST_ASSERT_FALSE_MESSAGE(sfr::serial::update_servos_usb, "Late leftovers must not be assembled into a packet"); +} + +static void test_packet_just_inside_timeout_still_completes() { + USBSerialMonitor monitor; + const std::vector payload = make_payload(constants::serial::USB_BUFFER_LEN); + + Serial.mock_rx_byte(constants::serial::RX_START_FLAG); + monitor.execute(); + + mock_advance_millis(constants::serial::RX_PACKET_TIMEOUT_MS - 1); + Serial.mock_rx(payload.data(), payload.size()); + Serial.mock_rx_byte(constants::serial::RX_END_FLAG); + monitor.execute(); + + TEST_ASSERT_TRUE_MESSAGE(sfr::serial::update_servos_usb, "A packet inside the timeout window should complete"); + TEST_ASSERT_EQUAL_UINT8(0, sfr::serial::dropped_packets); +} + + +// Tests that validate port isolation (different monitors must not consume each other's traffic). +static void test_usb_monitor_ignores_radio_port() { + feed(Serial2, frame_packet(make_payload(constants::serial::RADIO_BUFFER_LEN))); + + USBSerialMonitor monitor; + monitor.execute(); + + TEST_ASSERT_FALSE_MESSAGE(sfr::serial::update_servos_usb, "The USB monitor must not read the radio UART"); + TEST_ASSERT_GREATER_THAN_size_t_MESSAGE(0, Serial2.mock_rx_pending(), + "Radio bytes should still be waiting for the radio monitor"); +} + +static void test_radio_monitor_ignores_usb_port() { + feed(Serial, frame_packet(make_payload(constants::serial::USB_BUFFER_LEN))); + + RadioSerialMonitor monitor; + monitor.execute(); + + TEST_ASSERT_FALSE_MESSAGE(sfr::serial::update_servos_radio, "The radio monitor must not read the USB UART"); + TEST_ASSERT_GREATER_THAN_size_t(0, Serial.mock_rx_pending()); +} + + +// Runner. +void run_serial_framing_tests() { + Unity.TestFile = __FILE__; // Report failures against this file, not main.cpp. + RUN_TEST(test_framing_flags_are_distinct); + RUN_TEST(test_buffers_are_non_empty); + + RUN_TEST(test_usb_valid_packet_is_accepted); + RUN_TEST(test_radio_valid_packet_is_accepted); + RUN_TEST(test_radio_publishes_mode_flag_from_payload); + RUN_TEST(test_packet_split_across_execute_calls_still_completes); + RUN_TEST(test_back_to_back_packets_both_parse); + + RUN_TEST(test_short_packet_is_dropped); + RUN_TEST(test_overlong_packet_is_dropped); + RUN_TEST(test_radio_short_packet_is_dropped); + RUN_TEST(test_dropped_packet_counter_accumulates); + RUN_TEST(test_stray_bytes_outside_a_packet_are_ignored); + RUN_TEST(test_start_flag_mid_packet_restarts_cleanly); + + RUN_TEST(test_stalled_packet_times_out_and_is_dropped); + RUN_TEST(test_packet_just_inside_timeout_still_completes); + + RUN_TEST(test_usb_monitor_ignores_radio_port); + RUN_TEST(test_radio_monitor_ignores_usb_port); +} diff --git a/teensy/test/test_firmware/servo_control.cpp b/teensy/test/test_firmware/servo_control.cpp new file mode 100644 index 0000000..d11f2df --- /dev/null +++ b/teensy/test/test_firmware/servo_control.cpp @@ -0,0 +1,322 @@ +/** + * SERVO CONTROL TESTS -- ServoControlTask. + * Tests in this file cover two main areas of functionality: + * 1. Mode arbitration (radio serial must win over USB serial whenever radio_flag != 0, a fresh radio or USB command + * applies and clears its own update flag, an idle cycle with nothing pending commands no servo at all). + * 2. Angle to PWM mapping. + */ +#include "test_support.h" +#include "suites.hpp" +#include "ControlTasks/ServoControlTask.hpp" + + +static_assert(constants::serial::USB_BUFFER_LEN > layout::USB_JIB_SIDE, "usb_buffer is too small for the documented USB layout"); +static_assert(constants::serial::RADIO_BUFFER_LEN > layout::RADIO_JIB_SIDE, "radio_buffer is too small for the documented radio layout"); + + +// Helper functions. +/** Angles that are valid under any calibration, for the fields a test is not focused on. */ +static uint8_t default_mainsail() { + return mid_angle(constants::servo::MAINSAIL_MIN_ANGLE, constants::servo::MAINSAIL_MAX_ANGLE); +} +static uint8_t default_rudder() { + return mid_angle(constants::servo::RUDDER_MIN_ANGLE, constants::servo::RUDDER_MAX_ANGLE); +} +static uint8_t default_jib() { + return mid_angle(constants::servo::JIB_MIN_ANGLE, constants::servo::JIB_MAX_ANGLE); +} + +/** Stage a USB command in the SFR exactly as \code USBSerialMonitor\endcode would, and select USB mode. */ +static void stage_usb_command(const uint8_t mainsail, const uint8_t rudder, const uint8_t jib, const uint8_t jib_side) { + sfr::serial::usb_buffer[layout::USB_MAINSAIL] = mainsail; + sfr::serial::usb_buffer[layout::USB_RUDDER] = rudder; + sfr::serial::usb_buffer[layout::USB_JIB] = jib; + sfr::serial::usb_buffer[layout::USB_JIB_SIDE] = jib_side; + sfr::serial::update_servos_usb = true; + sfr::serial::radio_flag = 0; +} + +/** Stage a radio command in the SFR exactly as \code RadioSerialMonitor\endcode would, and select radio mode. */ +static void stage_radio_command(const uint8_t mainsail, const uint8_t rudder, const uint8_t jib, const uint8_t jib_side) { + sfr::serial::radio_buffer[layout::RADIO_FLAG] = 1; + sfr::serial::radio_buffer[layout::RADIO_MAINSAIL] = mainsail; + sfr::serial::radio_buffer[layout::RADIO_RUDDER] = rudder; + sfr::serial::radio_buffer[layout::RADIO_JIB] = jib; + sfr::serial::radio_buffer[layout::RADIO_JIB_SIDE] = jib_side; + sfr::serial::update_servos_radio = true; + sfr::serial::radio_flag = 1; +} + +/** Run one USB command through the task and return, so a sweep can read the resulting PWM out of the SFR. */ +static void apply_usb_command(ServoControlTask& task, const uint8_t mainsail, const uint8_t rudder, + const uint8_t jib, const uint8_t jib_side) { + stage_usb_command(mainsail, rudder, jib, jib_side); + task.execute(); +} + + +// Mode arbitration tests. +static void test_radio_mode_ignores_pending_usb_command() { + ServoControlTask task; + mock_reset_servos(); + + // Have a USB packet pending, but radio mode is engaged. USB packet shouldn't process. + stage_usb_command(default_mainsail(), default_rudder(), default_jib(), constants::servo::JIB_SIDE_PORT); + sfr::serial::radio_flag = 1; + sfr::serial::update_servos_radio = false; + task.execute(); + + TEST_ASSERT_EQUAL_UINT8_MESSAGE(0, sfr::servo::mainsail_angle, + "A queued USB command must not reach the servos while radio mode is engaged"); + TEST_ASSERT_TRUE_MESSAGE(sfr::serial::update_servos_usb, + "The USB command should remain pending, not be silently consumed"); + TEST_ASSERT_EQUAL_INT_MESSAGE(0, g_mock_servo_write_count[constants::servo::RUDDER_PIN], + "No servo should be commanded at all on this cycle"); +} + +static void test_radio_mode_applies_a_fresh_radio_command() { + ServoControlTask task; + const uint8_t mainsail = default_mainsail(); + const uint8_t rudder = default_rudder(); + const uint8_t jib = default_jib(); + + stage_radio_command(mainsail, rudder, jib, constants::servo::JIB_SIDE_PORT); + task.execute(); + + TEST_ASSERT_EQUAL_UINT8(mainsail, sfr::servo::mainsail_angle); + TEST_ASSERT_EQUAL_UINT8(rudder, sfr::servo::rudder_angle); + TEST_ASSERT_EQUAL_UINT8(jib, sfr::servo::jib_angle); + TEST_ASSERT_FALSE_MESSAGE(sfr::serial::update_servos_radio, "The radio flag should be cleared once consumed"); +} + +static void test_usb_mode_applies_when_radio_flag_is_zero() { + ServoControlTask task; + const uint8_t mainsail = default_mainsail(); + const uint8_t rudder = default_rudder(); + + apply_usb_command(task, mainsail, rudder, default_jib(), constants::servo::JIB_SIDE_PORT); + + TEST_ASSERT_EQUAL_UINT8(mainsail, sfr::servo::mainsail_angle); + TEST_ASSERT_EQUAL_UINT8(rudder, sfr::servo::rudder_angle); + TEST_ASSERT_FALSE_MESSAGE(sfr::serial::update_servos_usb, "The USB flag should be cleared once consumed"); +} + +static void test_no_pending_command_moves_nothing() { + ServoControlTask task; + mock_reset_servos(); + + sfr::serial::radio_flag = 0; + sfr::serial::update_servos_usb = false; + sfr::serial::update_servos_radio = false; + task.execute(); + + TEST_ASSERT_EQUAL_INT_MESSAGE(0, g_mock_servo_write_count[constants::servo::MAINSAIL_PIN], + "An idle cycle should not command any servo"); +} + + +// Tests for input validation: out-of-range values must be discarded. +static void test_out_of_range_mainsail_angle_is_rejected() { + uint8_t bad_angle; + if (!find_out_of_range_angle(constants::servo::MAINSAIL_MIN_ANGLE, constants::servo::MAINSAIL_MAX_ANGLE, bad_angle)) { + TEST_IGNORE_MESSAGE("Mainsail angle limits span the whole byte range; no invalid value exists to test"); + } + + ServoControlTask task; + apply_usb_command(task, bad_angle, default_rudder(), default_jib(), constants::servo::JIB_SIDE_PORT); + + TEST_ASSERT_EQUAL_UINT8_MESSAGE(0, sfr::servo::mainsail_angle, "An out-of-range mainsail angle must be discarded"); + TEST_ASSERT_EQUAL_UINT8_MESSAGE(default_rudder(), sfr::servo::rudder_angle, + "A bad mainsail angle must not block the valid rudder angle in the same packet"); +} + +static void test_out_of_range_rudder_angle_is_rejected() { + uint8_t bad_angle; + if (!find_out_of_range_angle(constants::servo::RUDDER_MIN_ANGLE, constants::servo::RUDDER_MAX_ANGLE, bad_angle)) { + TEST_IGNORE_MESSAGE("Rudder angle limits span the whole byte range; no invalid value exists to test"); + } + + ServoControlTask task; + apply_usb_command(task, default_mainsail(), bad_angle, default_jib(), constants::servo::JIB_SIDE_PORT); + + TEST_ASSERT_EQUAL_UINT8_MESSAGE(0, sfr::servo::rudder_angle, "An out-of-range rudder angle must be discarded"); +} + +static void test_out_of_range_jib_angle_is_rejected() { + uint8_t bad_angle; + if (!find_out_of_range_angle(constants::servo::JIB_MIN_ANGLE, constants::servo::JIB_MAX_ANGLE, bad_angle)) { + TEST_IGNORE_MESSAGE("Jib angle limits span the whole byte range; no invalid value exists to test"); + } + + ServoControlTask task; + apply_usb_command(task, default_mainsail(), default_rudder(), bad_angle, constants::servo::JIB_SIDE_PORT); + + TEST_ASSERT_EQUAL_UINT8_MESSAGE(0, sfr::servo::jib_angle, "An out-of-range jib angle must be discarded"); +} + +static void test_invalid_jib_side_flag_is_rejected() { + uint8_t bad_flag; + if (!find_invalid_jib_side_flag(bad_flag)) TEST_IGNORE_MESSAGE("Every byte value is a valid jib side flag"); + + ServoControlTask task; + apply_usb_command(task, default_mainsail(), default_rudder(), default_jib(), bad_flag); + + TEST_ASSERT_EQUAL_UINT8_MESSAGE(0, sfr::servo::jib_angle, "Corrupt jib side flag means whole jib command discard"); +} + + +// Tests for rudder mapping. +static void test_rudder_endpoints_match_the_declared_pulse_range() { + ServoControlTask task; + + apply_usb_command(task, default_mainsail(), constants::servo::RUDDER_MIN_ANGLE, default_jib(), + constants::servo::JIB_SIDE_PORT); + TEST_ASSERT_EQUAL_UINT32_MESSAGE(constants::servo::RUDDER_MIN_PULSE, sfr::servo::rudder_pwm, + "The smallest rudder angle should map to RUDDER_MIN_PULSE"); + + apply_usb_command(task, default_mainsail(), constants::servo::RUDDER_MAX_ANGLE, default_jib(), + constants::servo::JIB_SIDE_PORT); + TEST_ASSERT_EQUAL_UINT32_MESSAGE(constants::servo::RUDDER_MAX_PULSE, sfr::servo::rudder_pwm, + "The largest rudder angle should map to RUDDER_MAX_PULSE"); +} + +static void test_rudder_midpoint_is_amidships() { + ServoControlTask task; + apply_usb_command(task, default_mainsail(), default_rudder(), default_jib(), constants::servo::JIB_SIDE_PORT); + + TEST_ASSERT_UINT32_WITHIN_MESSAGE(1, constants::servo::RUDDER_MID_PULSE, sfr::servo::rudder_pwm, + "A mid-range rudder angle should sit at RUDDER_MID_PULSE"); +} + +static void test_rudder_pwm_rises_with_angle_and_stays_in_range() { + ServoControlTask task; + std::vector sweep; + + for (int angle = constants::servo::RUDDER_MIN_ANGLE; angle <= constants::servo::RUDDER_MAX_ANGLE; ++angle) { + apply_usb_command(task, default_mainsail(), static_cast(angle), default_jib(), + constants::servo::JIB_SIDE_PORT); + assert_pwm_in_range(sfr::servo::rudder_pwm, constants::servo::RUDDER_MIN_PULSE, + constants::servo::RUDDER_MAX_PULSE, "rudder"); + sweep.push_back(sfr::servo::rudder_pwm); + } + + assert_strictly_increasing(sweep, "rudder"); +} + + +// Test for sail mappings (law-of-cosines based: exact values are not predictable, but the shape of the curve is). +static void test_mainsail_pwm_never_falls_and_stays_in_range() { + ServoControlTask task; + std::vector sweep; + + for (int angle = constants::servo::MAINSAIL_MIN_ANGLE; angle <= constants::servo::MAINSAIL_MAX_ANGLE; ++angle) { + apply_usb_command(task, static_cast(angle), default_rudder(), default_jib(), + constants::servo::JIB_SIDE_PORT); + assert_pwm_in_range(sfr::servo::mainsail_pwm, constants::servo::MAINSAIL_MIN_PULSE, + constants::servo::MAINSAIL_MAX_PULSE, "mainsail"); + sweep.push_back(sfr::servo::mainsail_pwm); + } + + assert_non_decreasing(sweep, "mainsail"); + TEST_ASSERT_GREATER_THAN_UINT32_MESSAGE(sweep.front(), sweep.back(), + "Sheeting the mainsail all the way out should differ from all the way in"); +} + +static void test_mainsail_minimum_angle_sits_at_minimum_pulse() { + if constexpr (constants::servo::MAINSAIL_MIN_ANGLE != 0) { + TEST_IGNORE_MESSAGE("This anchor only holds when the minimum mainsail angle is 0 degrees"); + } + + ServoControlTask task; + apply_usb_command(task, constants::servo::MAINSAIL_MIN_ANGLE, default_rudder(), default_jib(), + constants::servo::JIB_SIDE_PORT); + + TEST_ASSERT_EQUAL_UINT32_MESSAGE(constants::servo::MAINSAIL_MIN_PULSE, sfr::servo::mainsail_pwm, + "Zero mainsail angle means zero sheet paid out, i.e. MAINSAIL_MIN_PULSE"); +} + +static void test_jib_pwm_never_falls_and_stays_in_range_on_both_sides() { + ServoControlTask task; + + for (const uint8_t side : {constants::servo::JIB_SIDE_PORT, constants::servo::JIB_SIDE_STB}) { + const bool is_port = (side == constants::servo::JIB_SIDE_PORT); + const uint32_t min_pulse = is_port ? constants::servo::JIB_PORT_MIN_PULSE : constants::servo::JIB_STB_MIN_PULSE; + const uint32_t max_pulse = is_port ? constants::servo::JIB_PORT_MAX_PULSE : constants::servo::JIB_STB_MAX_PULSE; + + std::vector sweep; + for (int angle = constants::servo::JIB_MIN_ANGLE; angle <= constants::servo::JIB_MAX_ANGLE; ++angle) { + apply_usb_command(task, default_mainsail(), default_rudder(), static_cast(angle), side); + const uint32_t pwm = is_port ? sfr::servo::jib_port_pwm : sfr::servo::jib_stb_pwm; + assert_pwm_in_range(pwm, min_pulse, max_pulse, is_port ? "jib (port)" : "jib (starboard)"); + sweep.push_back(pwm); + } + assert_non_decreasing(sweep, is_port ? "jib (port)" : "jib (starboard)"); + } +} + +static void test_trimming_port_jib_slacks_the_starboard_sheet() { + ServoControlTask task; + apply_usb_command(task, default_mainsail(), default_rudder(), default_jib(), constants::servo::JIB_SIDE_PORT); + + TEST_ASSERT_EQUAL_UINT32_MESSAGE(constants::servo::JIB_STB_MAX_PULSE, sfr::servo::jib_stb_pwm, + "Trimming to port should let the starboard sheet all the way out"); + assert_pwm_in_range(sfr::servo::jib_port_pwm, constants::servo::JIB_PORT_MIN_PULSE, + constants::servo::JIB_PORT_MAX_PULSE, "jib (port)"); + TEST_ASSERT_EQUAL_UINT8(constants::servo::JIB_SIDE_PORT, sfr::servo::jib_side_flag); +} + +static void test_trimming_starboard_jib_slacks_the_port_sheet() { + ServoControlTask task; + apply_usb_command(task, default_mainsail(), default_rudder(), default_jib(), constants::servo::JIB_SIDE_STB); + + TEST_ASSERT_EQUAL_UINT32_MESSAGE(constants::servo::JIB_PORT_MAX_PULSE, sfr::servo::jib_port_pwm, + "Trimming to starboard should let the port sheet all the way out"); + assert_pwm_in_range(sfr::servo::jib_stb_pwm, constants::servo::JIB_STB_MIN_PULSE, + constants::servo::JIB_STB_MAX_PULSE, "jib (starboard)"); + TEST_ASSERT_EQUAL_UINT8(constants::servo::JIB_SIDE_STB, sfr::servo::jib_side_flag); +} + + +// Simulates wiring: the computed PWM must actually reach the servo on the right pin. +static void test_computed_pwm_reaches_the_correct_servo_pins() { + ServoControlTask task; + mock_reset_servos(); + apply_usb_command(task, default_mainsail(), default_rudder(), default_jib(), constants::servo::JIB_SIDE_PORT); + + TEST_ASSERT_EQUAL_INT_MESSAGE(static_cast(sfr::servo::rudder_pwm), + g_mock_servo_last_write[constants::servo::RUDDER_PIN], + "The rudder servo should receive the PWM recorded in the SFR"); + TEST_ASSERT_EQUAL_INT_MESSAGE(static_cast(sfr::servo::mainsail_pwm), + g_mock_servo_last_write[constants::servo::MAINSAIL_PIN], + "The mainsail servo should receive the PWM recorded in the SFR"); + TEST_ASSERT_EQUAL_INT_MESSAGE(static_cast(sfr::servo::jib_port_pwm), + g_mock_servo_last_write[constants::servo::JIB_PORT_PIN], + "The port jib servo should receive the PWM recorded in the SFR"); +} + + +// Runner. +void run_servo_control_tests() { + Unity.TestFile = __FILE__; // Report failures against this file, not main.cpp. + RUN_TEST(test_radio_mode_ignores_pending_usb_command); + RUN_TEST(test_radio_mode_applies_a_fresh_radio_command); + RUN_TEST(test_usb_mode_applies_when_radio_flag_is_zero); + RUN_TEST(test_no_pending_command_moves_nothing); + + RUN_TEST(test_out_of_range_mainsail_angle_is_rejected); + RUN_TEST(test_out_of_range_rudder_angle_is_rejected); + RUN_TEST(test_out_of_range_jib_angle_is_rejected); + RUN_TEST(test_invalid_jib_side_flag_is_rejected); + + RUN_TEST(test_rudder_endpoints_match_the_declared_pulse_range); + RUN_TEST(test_rudder_midpoint_is_amidships); + RUN_TEST(test_rudder_pwm_rises_with_angle_and_stays_in_range); + + RUN_TEST(test_mainsail_pwm_never_falls_and_stays_in_range); + RUN_TEST(test_mainsail_minimum_angle_sits_at_minimum_pulse); + RUN_TEST(test_jib_pwm_never_falls_and_stays_in_range_on_both_sides); + RUN_TEST(test_trimming_port_jib_slacks_the_starboard_sheet); + RUN_TEST(test_trimming_starboard_jib_slacks_the_port_sheet); + + RUN_TEST(test_computed_pwm_reaches_the_correct_servo_pins); +} diff --git a/teensy/test/test_firmware/suites.hpp b/teensy/test/test_firmware/suites.hpp new file mode 100644 index 0000000..df17276 --- /dev/null +++ b/teensy/test/test_firmware/suites.hpp @@ -0,0 +1,13 @@ +#pragma once + +/** + * This file is the test suite registry. It acts almost like a header file for \code main.cpp\endcode. + * + * PlatformIO builds each directory under \code test/\endcode into one binary with a single \code main()\endcode, so all + * of these tests link together. They are still split across files by topic; each file keeps its own tests file-local, + * \code static\endcode, and exposes just the one function below, which \code main.cpp\endcode calls. + */ +void run_serial_framing_tests(); +void run_servo_control_tests(); +void run_telemetry_tests(); +void run_anemometer_tests(); diff --git a/teensy/test/test_firmware/telemetry.cpp b/teensy/test/test_firmware/telemetry.cpp new file mode 100644 index 0000000..0b38c17 --- /dev/null +++ b/teensy/test/test_firmware/telemetry.cpp @@ -0,0 +1,127 @@ +/** + * TELEMETRY TESTS -- TelemetryControlTask. + * The tests in this file assert the format of the telemetry packet and the send cadence. The byte order is a protocol + * contract shared with the Jetson, so it is tested explicitly; everything else comes from the SFR/constants.hpp. + */ +#include "test_support.h" +#include "suites.hpp" +#include "ControlTasks/TelemetryControlTask.hpp" + + +// Helper functions. +/** + * \code TelemetryControlTask\endcode compares against the timestamp captured at the END of the previous + * \code execute()\endcode, so a freshly constructed task needs two calls before the first frame goes out. + * This lag is harmless in the real loop (runs continuously) but must be reproduced here to observe any telemetry. + */ +static void init_telemetry(TelemetryControlTask& task) { + task.execute(); + task.execute(); +} + +/** The frame the firmware should produce for whatever is currently in the SFR. */ +static std::vector expected_frame() { + return { + constants::serial::TX_START_FLAG, + static_cast(sfr::anemometer::wind_angle >> 8), + static_cast(sfr::anemometer::wind_angle & 0xFF), + sfr::servo::mainsail_angle, + sfr::servo::rudder_angle, + sfr::servo::jib_angle, + sfr::servo::jib_side_flag, + sfr::serial::dropped_packets, + constants::serial::TX_END_FLAG, + }; +} + + +// Tests. +static void test_nothing_is_sent_before_the_period_elapses() { + TelemetryControlTask task; + + mock_set_millis(constants::serial::TX_PERIOD_MS - 1); + init_telemetry(task); + + TEST_ASSERT_EQUAL_size_t_MESSAGE(0, Serial.mock_tx().size(), "Telemetry must not send before TX_PERIOD_MS has passed"); +} + +static void test_frame_layout_matches_the_protocol() { + // Values chosen purely as test input. + sfr::anemometer::wind_angle = 345; + sfr::servo::mainsail_angle = 12; + sfr::servo::rudder_angle = 34; + sfr::servo::jib_angle = 56; + sfr::servo::jib_side_flag = constants::servo::JIB_SIDE_STB; + sfr::serial::dropped_packets = 7; + + TelemetryControlTask task; + mock_set_millis(constants::serial::TX_PERIOD_MS); + init_telemetry(task); + + const std::vector expected = expected_frame(); + const std::vector& actual = Serial.mock_tx(); + + TEST_ASSERT_EQUAL_size_t_MESSAGE(expected.size(), actual.size(), "Telemetry frame is the wrong length"); + TEST_ASSERT_EQUAL_UINT8_ARRAY(expected.data(), actual.data(), expected.size()); +} + +static void test_wind_angle_is_split_big_endian() { + sfr::anemometer::wind_angle = 347; // 0x015B: high byte 0x01, low byte 0x5B. + + TelemetryControlTask task; + mock_set_millis(constants::serial::TX_PERIOD_MS); + init_telemetry(task); + + const std::vector& frame = Serial.mock_tx(); + TEST_ASSERT_GREATER_THAN_size_t(2, frame.size()); + TEST_ASSERT_EQUAL_UINT8_MESSAGE(0x01, frame[1], "Wind angle high byte should come first"); + TEST_ASSERT_EQUAL_UINT8_MESSAGE(0x5B, frame[2], "Wind angle low byte should come second"); + + // A wind angle above 255 is exactly why this field is two bytes; make sure it survives the round trip. + const uint16_t reassembled = static_cast((frame[1] << 8) | frame[2]); + TEST_ASSERT_EQUAL_UINT16(sfr::anemometer::wind_angle, reassembled); +} + +static void test_frame_is_not_repeated_until_another_period_passes() { + TelemetryControlTask task; + mock_set_millis(constants::serial::TX_PERIOD_MS); + init_telemetry(task); + + const size_t after_first = Serial.mock_tx().size(); + TEST_ASSERT_GREATER_THAN_size_t_MESSAGE(0, after_first, "The first frame should have been sent by now"); + + // Another loop iteration immediately afterwards must not produce a second frame. + task.execute(); + TEST_ASSERT_EQUAL_size_t_MESSAGE(after_first, Serial.mock_tx().size(), + "Telemetry should be rate limited, not sent every loop iteration"); + + // Once a full period has passed, the next frame goes out. + mock_advance_millis(constants::serial::TX_PERIOD_MS); + task.execute(); + task.execute(); + TEST_ASSERT_GREATER_THAN_size_t_MESSAGE(after_first, Serial.mock_tx().size(), + "A second frame should follow one TX_PERIOD_MS later"); +} + +static void test_dropped_packet_count_is_reported() { + sfr::serial::dropped_packets = 42; + + TelemetryControlTask task; + mock_set_millis(constants::serial::TX_PERIOD_MS); + init_telemetry(task); + + const std::vector& frame = Serial.mock_tx(); + TEST_ASSERT_EQUAL_size_t(expected_frame().size(), frame.size()); + TEST_ASSERT_EQUAL_UINT8_MESSAGE(42, frame[frame.size() - 2], "Dropped counter belongs just before the end flag"); +} + + +// Runner. +void run_telemetry_tests() { + Unity.TestFile = __FILE__; // Report failures against this file, not main.cpp. + RUN_TEST(test_nothing_is_sent_before_the_period_elapses); + RUN_TEST(test_frame_layout_matches_the_protocol); + RUN_TEST(test_wind_angle_is_split_big_endian); + RUN_TEST(test_frame_is_not_repeated_until_another_period_passes); + RUN_TEST(test_dropped_packet_count_is_reported); +} diff --git a/teensy/test/test_firmware/test_support.h b/teensy/test/test_firmware/test_support.h new file mode 100644 index 0000000..9de8333 --- /dev/null +++ b/teensy/test/test_firmware/test_support.h @@ -0,0 +1,173 @@ +/** + * THIS FILE CONTAINS SHARED TEST HELPERS. It accomplishes two main jobs: + * 1. Reset all scraps of global state between tests to start fresh each time (the SFR and mocks are both global). + * 2. Build serial packets and pick test angles symbolically -- from constants.hpp, never from literals. + */ +#pragma once +#include +#include "../mocks/Arduino.h" +#include "../mocks/Servo.h" +#include "sfr.hpp" + +// Packet layouts: constants for indices defined explicitly (if a format ever gains a field, just make one edit here). +// TODO consider just defining these in constants.hpp (it's a good practice for the rest of the codebase anyway). +namespace layout { + // USB payload: [mainsail_angle, rudder_angle, jib_angle, jib_side_flag] + constexpr size_t USB_MAINSAIL = 0; + constexpr size_t USB_RUDDER = 1; + constexpr size_t USB_JIB = 2; + constexpr size_t USB_JIB_SIDE = 3; + + // Radio payload: [radio_flag, mainsail_angle, rudder_angle, jib_angle, jib_side_flag] + constexpr size_t RADIO_FLAG = 0; + constexpr size_t RADIO_MAINSAIL = 1; + constexpr size_t RADIO_RUDDER = 2; + constexpr size_t RADIO_JIB = 3; + constexpr size_t RADIO_JIB_SIDE = 4; +} + + +// State reset. +/** Restore the SFR to the power-on values defined in \code sfr.cpp\endcode. */ +inline void reset_sfr() { + sfr::anemometer::wind_angle = 0; + + sfr::servo::rudder_angle = 0; + sfr::servo::mainsail_angle = 0; + sfr::servo::jib_angle = 0; + sfr::servo::jib_side_flag = 0; + sfr::servo::rudder_pwm = 0; + sfr::servo::mainsail_pwm = 0; + sfr::servo::jib_port_pwm = 0; + sfr::servo::jib_stb_pwm = 0; + + sfr::serial::update_servos_radio = false; + sfr::serial::update_servos_usb = false; + sfr::serial::dropped_packets = 0; + memset(sfr::serial::usb_buffer, 0, sizeof(sfr::serial::usb_buffer)); + memset(sfr::serial::radio_buffer, 0, sizeof(sfr::serial::radio_buffer)); + sfr::serial::radio_flag = 1; +} + +/** Clear the fake clock, both serial ports, and all recorded pin/servo activity. */ +inline void reset_mocks() { + mock_set_millis(0); + Serial.mock_clear(); + Serial2.mock_clear(); + mock_reset_servos(); + for (size_t pin = 0; pin < MOCK_PIN_COUNT; ++pin) { + g_mock_pin_modes[pin] = 0; + g_mock_pin_levels[pin] = 0; + g_mock_analog_values[pin] = 0; + } +} + +/** Call this function from Unity's \code setUp()\endcode so every test starts from the same baseline. */ +inline void reset_all() { + reset_mocks(); + reset_sfr(); +} + + +// Packet construction. +/** A distinct, deterministic payload byte for slot \code i\endcode that isn't one of the signal flags. */ +inline uint8_t safe_payload_byte(const size_t i) { + uint8_t byte = static_cast(i + 1); + while (byte == constants::serial::RX_START_FLAG || byte == constants::serial::RX_END_FLAG) ++byte; + return byte; +} + +/** Returns \code count\endcode distinct, flag-safe payload bytes. */ +inline std::vector make_payload(const size_t count) { + std::vector payload; + payload.reserve(count); + for (size_t i = 0; i < count; ++i) payload.push_back(safe_payload_byte(i)); + return payload; +} + +/** Wrap a payload in the RX framing flags, producing bytes exactly as they arrive over the wire. */ +inline std::vector frame_packet(const std::vector& payload) { + std::vector packet; + packet.reserve(payload.size() + 2); + packet.push_back(constants::serial::RX_START_FLAG); + packet.insert(packet.end(), payload.begin(), payload.end()); + packet.push_back(constants::serial::RX_END_FLAG); + return packet; +} + +/** Queue bytes on a port as if the peer had just sent them. */ +inline void feed(FakeStream& port, const std::vector& bytes) { + port.mock_rx(bytes.data(), bytes.size()); +} + + +// Angle calculation helper methods, derived from limits that constants.hpp currently declares. +/** The midpoint of an inclusive angle range. */ +inline uint8_t mid_angle(const uint8_t lo, const uint8_t hi) { + return static_cast(lo + (hi - lo) / 2); +} + +/** + * Finds an angle guaranteed to fall outside the inclusive range [lo, hi], for testing bounds rejection. + * Returns \code false\endcode when the range covers the whole \code uint8_t\endcode domain and no invalid value + * exists (the caller should then skip the test rather than assert something impossible). + */ +inline bool find_out_of_range_angle(const uint8_t lo, const uint8_t hi, uint8_t& out) { + if (hi < 255) { out = static_cast(hi + 1); return true; } + if (lo > 0) { out = static_cast(lo - 1); return true; } + return false; +} + +/** Generates an invalid jib side flag (neither port nor stb). */ +inline bool find_invalid_jib_side_flag(uint8_t& out) { + for (unsigned candidate = 0; candidate <= 255; ++candidate) { + const uint8_t flag = static_cast(candidate); + if (flag != constants::servo::JIB_SIDE_PORT && flag != constants::servo::JIB_SIDE_STB) { + out = flag; + return true; + } + } + return false; +} + +/** Asserts that every step of a sweep goes up. Used for the rudder, whose \code map()\endcode is linear with no + * clamping. */ +inline void assert_strictly_increasing(const std::vector& values, const char* what) { + for (size_t i = 1; i < values.size(); ++i) { + if (values[i] <= values[i - 1]) { + char message[192]; + snprintf(message, sizeof(message), + "%s should rise with angle, but step %zu went %lu -> %lu", + what, i, static_cast(values[i - 1]), static_cast(values[i])); + TEST_FAIL_MESSAGE(message); + } + } +} + +/** + * Asserts that a sweep never goes down. Used for the sails, whose law-of-cosines mapping rises but then flattens once + * it clamps at \code MAX_PULSE\endcode, so consecutive values are allowed to be equal but never to reverse. + */ +inline void assert_non_decreasing(const std::vector& values, const char* what) { + for (size_t i = 1; i < values.size(); ++i) { + if (values[i] < values[i - 1]) { + char message[192]; + snprintf(message, sizeof(message), + "%s should never fall as angle rises, but step %zu went %lu -> %lu", + what, i, static_cast(values[i - 1]), static_cast(values[i])); + TEST_FAIL_MESSAGE(message); + } + } +} + +/** Asserts that a PWM value sits inside the servo's declared pulse range. */ +inline void assert_pwm_in_range(const uint32_t pwm, const uint32_t min_pulse, const uint32_t max_pulse, + const char* what) { + if (pwm < min_pulse || pwm > max_pulse) { + char message[160]; + snprintf(message, sizeof(message), "%s PWM %lu outside [%lu, %lu]", + what, static_cast(pwm), + static_cast(min_pulse), static_cast(max_pulse)); + TEST_FAIL_MESSAGE(message); + } +} From 1fb34c45a4aacad5fd0f7507404db9218fc0ad69 Mon Sep 17 00:00:00 2001 From: nizhnerk Date: Sat, 3 Oct 2026 19:24:40 -0400 Subject: [PATCH 2/7] Allow Teensy lib directory into git --- .gitignore | 2 +- teensy/lib/README | 44 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 45 insertions(+), 1 deletion(-) create mode 100644 teensy/lib/README diff --git a/.gitignore b/.gitignore index 2cbb281..b4dd7b0 100644 --- a/.gitignore +++ b/.gitignore @@ -2,7 +2,7 @@ devel/ logs/ build/ bin/ -lib/ +#lib/ msg_gen/ srv_gen/ msg/*Action.msg diff --git a/teensy/lib/README b/teensy/lib/README new file mode 100644 index 0000000..e80d800 --- /dev/null +++ b/teensy/lib/README @@ -0,0 +1,44 @@ +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) { + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html \ No newline at end of file From fbf466127148efd37fb79e18c76b34645a5ca7ae Mon Sep 17 00:00:00 2001 From: nizhnerk Date: Sat, 3 Oct 2026 19:24:55 -0400 Subject: [PATCH 3/7] Put lib back --- .gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index b4dd7b0..2cbb281 100644 --- a/.gitignore +++ b/.gitignore @@ -2,7 +2,7 @@ devel/ logs/ build/ bin/ -#lib/ +lib/ msg_gen/ srv_gen/ msg/*Action.msg From 7997e3f11f4d103e36de5589eb28c5dedee14650 Mon Sep 17 00:00:00 2001 From: nizhnerk Date: Sat, 3 Oct 2026 19:43:02 -0400 Subject: [PATCH 4/7] Fix stale references from sailbot repository. --- teensy/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/teensy/README.md b/teensy/README.md index 18b9d13..e8cafa4 100644 --- a/teensy/README.md +++ b/teensy/README.md @@ -47,12 +47,12 @@ Below are the steps to set up your development environment to upload code and ob ### Prerequisites: - [VSCode](https://code.visualstudio.com/download) or [CLion](https://www.jetbrains.com/clion/) is installed. -- The [sailbot](https://github.com/CUSail-Navigation/sailbot) repository is cloned. +- The [boat](https://github.com/CUSail-Navigation/boat) repository is cloned. ### Steps: 1. In VSCode, click "Extensions" on the left-hand side toolbar and search for PlatformIO IDE. In CLion, click "File → Plugins" and search for PlatformIO for CLion. -2. Open the `teensy/` folder within the sailbot repository. Make sure the `teensy/` folder is the project root. +2. Open the `teensy/` folder within the boat repository. Make sure the `teensy/` folder is the project root. 3. (VSCode) At the bottom of your screen in the blue toolbar, you should see a check, arrow, and serial monitor icon. - If you would just like to compile code but not upload to the Teensy, press the check. - If you would like to upload to the Teensy, press the arrow. @@ -84,5 +84,5 @@ Make sure you have: 1. Run `ls /dev` or `lsusb` to view ports accessible by WSL. The Teensy will most likely appear as `/dev/ttyACM0`. 2. Run the following command in WSL to expose the shared port with the docker image: ``` -docker run -it --rm --name ros2_container -v $(pwd)/src:/home/ros2_user/ros2_ws/src --device= ros2_humble_custom +docker run -it --rm --name boat-dev -v "$(pwd)/src:/home/ros2_user/ros2_ws/src" --device= boat-local ``` From 77e8794b015ce1b946550fe10c5448330a131aac Mon Sep 17 00:00:00 2001 From: nizhnerk Date: Sat, 3 Oct 2026 21:36:06 -0400 Subject: [PATCH 5/7] Add Teensy build checks and tests to CI --- .github/workflows/ci.yml | 27 ++++++++++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f5bcfe6..9ae9aaa 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,4 +1,4 @@ -name: ROS CI +name: Boat CI on: push: @@ -49,3 +49,28 @@ jobs: colcon test-result --verbose exit "$test_status" ' + + teensy: + runs-on: ubuntu-24.04 + timeout-minutes: 15 + defaults: + run: + working-directory: teensy + steps: + - uses: actions/checkout@v7 + with: + persist-credentials: false + - uses: actions/setup-python@v5 + with: + python-version: "3.12" + - name: Cache PlatformIO + uses: actions/cache@v4 + with: + path: ~/.platformio + key: pio-${{ runner.os }}-${{ hashFiles('teensy/platformio.ini') }} + - name: Install PlatformIO + run: pip install --upgrade platformio + - name: Build firmware + run: pio run -e teensy40 + - name: Run unit tests + run: ./test/run_tests.sh From 302cde3d322f5e71ed95fd844c457ecaaa8588ae Mon Sep 17 00:00:00 2001 From: nizhnerk Date: Sat, 3 Oct 2026 21:50:47 -0400 Subject: [PATCH 6/7] Clean up .gitignore - ROS2 codebase with colcon/ament won't produce lots of old ROS1 stuff + nobody uses some of the other stuff. --- .gitignore | 78 +++++++++++------------------------------------------- 1 file changed, 15 insertions(+), 63 deletions(-) diff --git a/.gitignore b/.gitignore index 2cbb281..3b02099 100644 --- a/.gitignore +++ b/.gitignore @@ -1,74 +1,24 @@ -devel/ -logs/ +# ROS2/colcon build artifacts. build/ -bin/ -lib/ -msg_gen/ -srv_gen/ -msg/*Action.msg -msg/*ActionFeedback.msg -msg/*ActionGoal.msg -msg/*ActionResult.msg -msg/*Feedback.msg -msg/*Goal.msg -msg/*Result.msg -msg/_*.py -build_isolated/ -devel_isolated/ - -# Generated by dynamic reconfigure. -*.cfgc -/cfg/cpp/ -/cfg/*.py - -# Idea. -.idea/ - -# Generated docs. -*.dox -*.wikidoc - -# Eclipse. -.project -.cproject - -# Qcreator. -CMakeLists.txt.user - -srv/_*.py -*.pcd -*.pyc -qtcreator-* -*.user - -/planning/cfg -/planning/docs -/planning/src - -*~ - -# Emacs. -.#* - -# Catkin custom files. -CATKIN_IGNORE - -# ROS2 generated artifacts. install/ log/ -__pycache__/ - -# ROS workspace metadata. -.rosinstall +logs/ .ament_version colcon.meta -# System logs. -sys/sys.log -sys/ros.out +# Python. +__pycache__/ +*.pyc + +# IDE. +.vscode/ +.idea/ -# CV models. +# Project-specific. +/lib/ *.pt +sys/sys.log +sys/ros.out # Miscellaneous. *.DS_Store @@ -76,3 +26,5 @@ sys/ros.out *.swo *.bak *.tmp +*~ +.#* From 2898ddf375802f442000b2e9b0d6c71bab067e6e Mon Sep 17 00:00:00 2001 From: nizhnerk Date: Sat, 3 Oct 2026 21:52:32 -0400 Subject: [PATCH 7/7] - --- .gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 3b02099..ad4914d 100644 --- a/.gitignore +++ b/.gitignore @@ -14,7 +14,7 @@ __pycache__/ .vscode/ .idea/ -# Project-specific. +# Project specific. /lib/ *.pt sys/sys.log