Repository: tinymovr/Tinymovr (language: python)

Issue:
Support for Tinymovr R53



Discussion / hints:

Fix patch:
diff --git a/.readthedocs.yaml b/.readthedocs.yaml
index 9712e405..7f5014e3 100644
--- a/.readthedocs.yaml
+++ b/.readthedocs.yaml
@@ -20,3 +20,8 @@ sphinx:
 python:
   install:
   - requirements: docs/requirements.txt
+
+# Build PDF & ePub
+formats:
+  - epub
+  - pdf
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 47a4c674..ebd476f2 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -8,7 +8,8 @@
             "defines": [
                 "_DEBUG",
                 "UNICODE",
-                "_UNICODE"
+                "_UNICODE",
+                "__CC_ARM"
             ],
             "cStandard": "c11",
             "cppStandard": "c++17",
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 9e3ad64e..73380aa3 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -3,7 +3,32 @@
     "files.associations": {
         "random": "c",
         "limits": "c",
-        "common.h": "c"
+        "common.h": "c",
+        "__hash_table": "c",
+        "__tuple": "c",
+        "array": "c",
+        "type_traits": "c",
+        "ratio": "c",
+        "tuple": "c",
+        "variant": "c",
+        "chrono": "c",
+        "compare": "c",
+        "cstddef": "c",
+        "__locale": "c",
+        "__bit_reference": "c",
+        "__node_handle": "c",
+        "atomic": "c",
+        "bitset": "c",
+        "__memory": "c",
+        "locale": "c",
+        "optional": "c",
+        "system_error": "c",
+        "vector": "c",
+        "motor.h": "c",
+        "trajectory_planner.h": "c",
+        "xf1.h": "c",
+        "controller.h": "c",
+        "observer.h": "c"
     },
     "python.testing.unittestArgs": [
         "-v",
diff --git a/.vscode/tasks.json b/.vscode/tasks.json
index 26620eec..ea32fcfe 100644
--- a/.vscode/tasks.json
+++ b/.vscode/tasks.json
@@ -96,9 +96,10 @@
                 "R50",
                 "R51",
                 "R52",
+                "R53",
                 "M51"
             ],
-            "default": "R52"
+            "default": "R53"
         }
     ]
 }
\ No newline at end of file
diff --git a/README.md b/README.md
index 78c873b6..2a8d2b3f 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,7 @@
 ![FW Check Build](https://github.com/yconst/tinymovr/workflows/Tinymovr%20Firmware%20Check%2FBuild/badge.svg)
 ![Studio Lint Test](https://github.com/yconst/tinymovr/workflows/Tinymovr%20Studio%20Lint%2FTest/badge.svg)
 ![Docs Build](https://github.com/yconst/tinymovr/workflows/Tinymovr%20Docs%20Build/badge.svg)
-[![Discord](https://img.shields.io/discord/742400176664084535)](https://discord.gg/CzcCaXbU)
+[![Discord](https://img.shields.io/discord/742400176664084535)](https://discord.gg/UnmfuVzKuQ)
 
 [Tinymovr is an affordable motor controller](https://tinymovr.com) with integrated encoder and CAN bus for precise control of 3-phase brushless motors (PMSMs). 
 
diff --git a/avlos_config.yaml b/avlos_config.yaml
index 7e0989fb..7ac383bd 100644
--- a/avlos_config.yaml
+++ b/avlos_config.yaml
@@ -5,15 +5,17 @@ generators:
   generator_c:
     enabled: true
     paths:
+      output_enums: ./firmware/src/tm_enums.h
       output_header: ./firmware/src/can/can_endpoints.h
       output_impl: ./firmware/src/can/can_endpoints.c
     header_includes:
     - src/common.h
+    - src/tm_enums.h
     impl_includes:
     - <string.h>
     - src/adc/adc.h
     - src/system/system.h
-    - src/encoder/encoder.h
+    - src/sensor/sensors.h
     - src/observer/observer.h
     - src/motor/motor.h
     - src/scheduler/scheduler.h
diff --git a/docs/hardware/errata.rst b/docs/hardware/errata.rst
index ce8a4123..99bdb02e 100644
--- a/docs/hardware/errata.rst
+++ b/docs/hardware/errata.rst
@@ -3,7 +3,7 @@ Hardware Errata
 
 .. _alpha-erratum-1:
 
-Tinymovr Alpha CAN Bus Connector Erratum
+Tinymovr Alpha CAN Bus Connector
 ****************************************
 
 The CANine v1 Adapter that came with Tinymovr alpha had the DF-13 pins reversed and as such is not compatible with regular DF-13 cables. Alpha users are advised to use the alternative pin header on the board and the included DF-13 to 2.54mm pin converter cable to communicate with Tinymovr, as shown in the diagram below:
@@ -16,7 +16,7 @@ Note above that the red wire should stay disconnected and to the left side of th
 
 .. _alpha-erratum-2:
 
-Tinymovr Alpha USB Micro Connector Erratum
+Tinymovr Alpha USB Micro Connector
 ******************************************
 
 The USB Micro connector used in the v1 adapter was unfortunately not very robust. In order to ensure that there is a good contact between the board and the USB cable, please ensure the male connector of the cable is firmly seated in the female connector of the board.
@@ -33,6 +33,17 @@ Tinymovr R5 UART Silkscreen Reversed
 
 The silkscreen next to the UART port on the rear of Tinymovr R5 has the order of pins reversed. The correct pins are provided in the figure below.
 
+This does not affect R5.2 and above.
+
 .. image:: erratum_R51.png
   :width: 600
-  :alt: Tinymovr R5 reversed silkscreen on UART port
\ No newline at end of file
+  :alt: Tinymovr R5 reversed silkscreen on UART port
+
+.. _r52-erratum-1:
+
+Tinymovr R5.2 FLEX2 Port as SPI
+************************************
+
+Even though Tinymovr R5.2 has the FLEX2 port which can function as SPI, due to a hardware incompatibility this port does not implement SPI correctly. As such, external sensors are only supported on the M5.x series at the moment.
+
+Tinymovr R5.3 and above does not have this issue and supports external SPI sensors normally.
\ No newline at end of file
diff --git a/docs/hardware/gimbal.rst b/docs/hardware/gimbal.rst
index 012b1a7d..2e034b92 100644
--- a/docs/hardware/gimbal.rst
+++ b/docs/hardware/gimbal.rst
@@ -1,3 +1,5 @@
+.. _gimbal-motors:
+
 *************
 Gimbal Motors
 *************
@@ -28,6 +30,9 @@ To achieve closed-loop current control, motor controllers such as Tinymovr use c
 
 The gimbal mode of Tinymovr disables closed-loop current control. The commanded currents are converted to voltages using basic resistance and inductance formulas. 
 
+
+.. _enabling-gimbal-mode:
+
 Enabling Gimbal Mode
 --------------------
 
diff --git a/docs/hardware/guide.rst b/docs/hardware/guide.rst
index 59aaff11..b1296991 100644
--- a/docs/hardware/guide.rst
+++ b/docs/hardware/guide.rst
@@ -36,6 +36,26 @@ Most gimbal motors can be used with Tinymovr M5. Gimbal motors are essentially o
 
    * Motors with trapezoidal back-EMF can also be controlled, but control will be sub-optimal. The trapezoidal back-EMF will appear as residual in the dq frame, as a result it will be much harder for the current controller to regulate phase currents. The tangible result is that the motor may exhibit increased noise and vibration while running.
 
+Limitations
+***********
+
+Tinymovr controllers are designed with certain limitations in terms of motor internal resistance and inductance, to ensure optimal closed loop current control performance.
+
+.. list-table:: Motor Limits
+   :header-rows: 1
+
+   * - Controller Version
+     - Phase Resistance (ohms)
+     - Phase Inductance (henries)
+   * - Tinymovr R5.x and R3.x
+     - 0.01 to 1.0
+     - 5e-6 to 1e-3
+   * - Tinymovr M5.x
+     - 0.5 to 20.0
+     - 1e-5 to 1e-2
+
+Note that motors with parameters outside this range will result in calibration failing. Ensure that the motors you select adhere to these parameters for the best operational experience. You can still use motors outside this range with open loop current control, through :ref:`enabling-gimbal-mode`.
+
 
 Mechanical Setup
 ################
@@ -73,9 +93,9 @@ Sensor Magnet and Mounting Tips
 
 * Ensure the sensor magnet is firmly attached to the motor shaft, otherwise it may slip out of sync. Use strong adhesive to secure.
 
-* It is possible to have the magnet on the rear side of the PCB, i.e. opposite of the magnet sensor IC, but the gap needs to be reduced to account for the PCB thickness. This has been verified by MPS in `this forum post <https://forum.monolithicpower.com/t/mounting-ma702-and-magnet-on-opposite-sides-of-pcb/1609>`_, quoted below: 
+* It is possible to have the magnet on the rear side of the PCB, i.e. opposite of the magnet sensor IC, but the gap needs to be reduced to account for the PCB thickness. This has been verified by MPS in a forum post [1]_, quoted below: 
   *[...] this type of arrangement is possible, what really matters in the end is that there is enough magnetic field reaching the sensor.
-  Of course the minimum distance is imposed by the thickness of the PCB, so it puts some constraints on the design, that you have to take into account when chosing the magnet (you can use our online simulation tool for that). But as long as the PCB is not acting as a magnetic shield (due to copper plane), then it is fine.*
+  Of course the minimum distance is imposed by the thickness of the PCB, so it puts some constraints on the design, that you have to take into account when choosing the magnet (you can use our online simulation tool for that). But as long as the PCB is not acting as a magnetic shield (due to copper plane), then it is fine.*
 
 * Calibration needs to be performed without any loads on the motor. If the motor is coupled to a load, the sensor offset angle may not be determined correctly, leading to a sub-optimal setup.
 
@@ -118,7 +138,7 @@ Connect the CAN bus header to one of the two CAN sockets on the board. It is not
 
 * Tinymovr M5.x: you will need to provide an external 120Ω termination resistor.
 
-In small setups with few nodes and short wires, it is better to enable just a single termination resistor, either on one Tinymovr board or on the CAN adapter. In setups with many nodes and long cables, you may need to enable termination resistors in both terminal nodes.
+In small setups with few nodes and short wires, it is better to enable just a single termination resistor, either on one Tinymovr board or on the CAN adapter. In setups with many nodes and long cables, you may need to enable termination resistors in both terminal nodes. A typical CAN driver has an “open-drain” output structure, meaning that the dominant edge is actively driven and the recessive edge is not. Therefore, properly terminating the bus is very important because it ensures that the recessive edge decays properly, and in time for the next bit's sample point [2]_.
 
 .. warning::
    The UART pins in Tinymovr R5.1 have the silkscreen reversed. If you are planning to use UART with R5.1, consult :ref:`r51-erratum-1`. 
@@ -148,4 +168,8 @@ Multiple nodes can be connected in a single CAN Bus network by means of daisy-ch
 
 .. image:: daisy_chain.png
   :width: 800
-  :alt: Connecting multiple nodes by daisy-chaining
\ No newline at end of file
+  :alt: Connecting multiple nodes by daisy-chaining
+
+
+.. [1] MPS forum post on mounting MA702 and magnet on opposite sides of PCB: https://forum.monolithicpower.com/t/mounting-ma702-and-magnet-on-opposite-sides-of-pcb/1609
+.. [2] Split Termination, Texas Instruments. Available at `https://www.ti.com/document-viewer/lit/html/SSZTB40#:~:text=for%20each%20termination.-,Split%20Termination,typically%20between%201%2D100nF`.
\ No newline at end of file
diff --git a/docs/protocol/integrating.rst b/docs/protocol/integrating.rst
index 2b2ba020..a12d31f9 100644
--- a/docs/protocol/integrating.rst
+++ b/docs/protocol/integrating.rst
@@ -22,12 +22,11 @@ Here below is an example using the API from Python scripts and controlling hardw
 .. code-block:: python
 
     import can
-    from tinymovr.tee import init_tee
+    from tinymovr.bus_router import init_router
     from tinymovr.config import get_bus_config, create_device
 
-    params = get_bus_config(["canine", "slcan_disco"])
-    params["bitrate"] = bitrate
-    init_tee(can.Bus(**params))
+    params = get_bus_config(["canine", "slcan_disco"], bitrate=1000000)
+    init_router(can.Bus, params)
     tm = create_device(node_id=1)
 
     tm.controller.calibrate()
@@ -42,6 +41,97 @@ The above code block will instantiate a Tinymovr with CAN bus id of 1 and calibr
     tm.controller.velocity_mode()
     tm.controller.velocity.setpoint = 80000
 
+BusRouter API
+#############
+
+The `BusRouter` class in the Tinymovr library is designed to facilitate message distribution and communication on the CAN bus by distributing incoming messages based on the result of a filter callback.
+
+API Overview
+------------
+
+.. class:: BusRouter
+
+   Initializes and manages the routing of CAN bus messages between various clients. It integrates a simple forwarding mechanism for sending messages to simplify interfacing with CAN bus objects.
+
+   .. method:: __init__(bus_manager, timeout, logger)
+
+      Constructor for the BusRouter. Initializes the bus manager, timeout, and logger.
+
+      .. note::
+        This constructor should not be used directly. Instead, use the :func:`init_router` function.
+
+      :param bus_manager: Instance of BusManager to handle low-level bus interactions.
+      :param timeout: Timeout in seconds for receiving messages.
+      :param logger: Logger instance for logging information.
+
+   .. method:: run()
+
+      Main thread function that continuously listens for incoming frames and dispatches them to the appropriate clients based on filter results.
+
+   .. method:: add_client(filter_cb, recv_cb)
+
+      Registers a new client to receive messages that meet specific conditions.
+
+      :param filter_cb: Callback function that determines if the incoming frame should be passed to the client.
+      :param recv_cb: Callback function that handles the received frame.
+
+   .. method:: stop()
+
+      Stops the router's main running thread, effectively stopping the routing of messages.
+
+   .. method:: shutdown()
+
+      Performs a complete shutdown by stopping the router and closing all bus manager activities.
+
+   .. method:: send(frame)
+
+      Sends a frame via the bus manager.
+
+      :param frame: The CAN frame to be sent.
+
+Bus Router Management Functions
+-------------------------------
+
+These helper functions manage the lifecycle of the `BusRouter` instance.
+
+.. function:: init_router(bus_class, bus_params, logger, timeout=0.1)
+
+   Initializes a bus router using a python-can bus instance. This function creates a singleton instance of `BusRouter`.
+
+   :param bus_class: The bus class from python-can to be used.
+   :param bus_params: Parameters for the bus initialization.
+   :param logger: Logger instance for logging activities.
+   :param timeout: Timeout in seconds for receiving messages.
+   :return: An instance of `BusRouter`.
+
+.. function:: destroy_router()
+
+   Destroys the existing bus router and stops its associated thread.
+
+.. function:: get_router()
+
+   Returns the current instance of the bus router if it exists.
+
+Deprecations
+------------
+
+The following functions are deprecated and will be removed in future versions:
+
+.. function:: init_tee()
+   
+   .. deprecated:: 2.1
+    Use :func:`init_router` instead.
+
+.. function:: destroy_tee()
+    
+   .. deprecated:: 2.1
+    Use :func:`destroy_router` instead.
+
+.. function:: get_tee()
+
+   .. deprecated:: 2.1
+    Use :func:`get_router` instead.
+
 Error Codes
 ###########
 
diff --git a/docs/protocol/reference.rst b/docs/protocol/reference.rst
index d24de64f..0e48ccb4 100644
--- a/docs/protocol/reference.rst
+++ b/docs/protocol/reference.rst
@@ -10,11 +10,12 @@ protocol_hash
 -------------------------------------------------------------------
 
 ID: 0
+
 Type: uint32
 
 
-The Avlos protocol hash.
 
+The Avlos protocol hash.
 
 
 
@@ -22,11 +23,12 @@ uid
 -------------------------------------------------------------------
 
 ID: 1
+
 Type: uint32
 
 
-The unique device ID, unique to each PAC55xx chip produced.
 
+The unique device ID, unique to each PAC55xx chip produced.
 
 
 
@@ -34,11 +36,12 @@ fw_version
 -------------------------------------------------------------------
 
 ID: 2
+
 Type: str
 
 
-The firmware version.
 
+The firmware version.
 
 
 
@@ -46,11 +49,12 @@ hw_revision
 -------------------------------------------------------------------
 
 ID: 3
+
 Type: uint32
 
 
-The hardware revision.
 
+The hardware revision.
 
 
 
@@ -58,59 +62,64 @@ Vbus
 -------------------------------------------------------------------
 
 ID: 4
+
 Type: float
+
 Units: volt
 
 The measured bus voltage.
 
 
 
-
 Ibus
 -------------------------------------------------------------------
 
 ID: 5
+
 Type: float
+
 Units: ampere
 
 The estimated bus current. Only estimates current drawn by motor.
 
 
 
-
 power
 -------------------------------------------------------------------
 
 ID: 6
+
 Type: float
+
 Units: watt
 
 The estimated power. Only estimates power drawn by motor.
 
 
 
-
 temp
 -------------------------------------------------------------------
 
 ID: 7
+
 Type: float
+
 Units: degree_Celsius
 
 The internal temperature of the PAC55xx MCU.
 
 
 
-
 calibrated
 -------------------------------------------------------------------
 
 ID: 8
+
 Type: bool
 
 
-Whether the system has been calibrated.
 
+Whether the system has been calibrated.
 
 
 
@@ -118,195 +127,280 @@ errors
 -------------------------------------------------------------------
 
 ID: 9
+
 Type: uint8
 
 
+
 Any system errors, as a bitmask
 
 Flags: 
+
 - UNDERVOLTAGE
+
+warnings
+-------------------------------------------------------------------
+
+ID: 10
+
+Type: uint8
+
+
+
+Any system warnings, as a bitmask
+
+Flags: 
+
 - DRIVER_FAULT
+
 - CHARGE_PUMP_FAULT_STAT
+
 - CHARGE_PUMP_FAULT
+
 - DRV10_DISABLE
+
 - DRV32_DISABLE
-- DRV54_DISABLE
 
+- DRV54_DISABLE
 
 save_config() -> void
--------------------------------------------------------------------
+--------------------------------------------------------------------------------------------
+
+ID: 11
 
-ID: 10
 Return Type: void
 
 
-Save configuration to non-volatile memory.
 
+Save configuration to non-volatile memory.
 
 erase_config() -> void
--------------------------------------------------------------------
+--------------------------------------------------------------------------------------------
+
+ID: 12
 
-ID: 11
 Return Type: void
 
 
-Erase the config stored in non-volatile memory and reset the device.
 
+Erase the config stored in non-volatile memory and reset the device.
 
 reset() -> void
--------------------------------------------------------------------
+--------------------------------------------------------------------------------------------
+
+ID: 13
 
-ID: 12
 Return Type: void
 
 
-Reset the device.
 
+Reset the device.
 
 enter_dfu() -> void
--------------------------------------------------------------------
+--------------------------------------------------------------------------------------------
+
+ID: 14
 
-ID: 13
 Return Type: void
 
 
+
 Enter DFU mode.
 
+config_size
+-------------------------------------------------------------------
+
+ID: 15
+
+Type: uint32
+
+
+
+Size (in bytes) of the configuration object.
+
 
-scheduler.errors
+
+scheduler.load
 -------------------------------------------------------------------
 
-ID: 14
+ID: 16
+
+Type: uint32
+
+
+
+Processor load in ticks per PWM cycle.
+
+
+
+scheduler.warnings
+-------------------------------------------------------------------
+
+ID: 17
+
 Type: uint8
 
 
-Any scheduler errors, as a bitmask
+
+Any scheduler warnings, as a bitmask
 
 Flags: 
-- CONTROL_BLOCK_REENTERED
 
+- CONTROL_BLOCK_REENTERED
 
 controller.state
 -------------------------------------------------------------------
 
-ID: 15
+ID: 18
+
 Type: uint8
 
 
+
 The state of the controller.
 
+Options: 
+
+- IDLE
 
+- CALIBRATE
 
+- CL_CONTROL
 
 controller.mode
 -------------------------------------------------------------------
 
-ID: 16
+ID: 19
+
 Type: uint8
 
 
+
 The control mode of the controller.
 
+Options: 
+
+- CURRENT
+
+- VELOCITY
+
+- POSITION
 
+- TRAJECTORY
 
+- HOMING
 
 controller.warnings
 -------------------------------------------------------------------
 
-ID: 17
+ID: 20
+
 Type: uint8
 
 
+
 Any controller warnings, as a bitmask
 
 Flags: 
+
 - VELOCITY_LIMITED
+
 - CURRENT_LIMITED
-- MODULATION_LIMITED
 
+- MODULATION_LIMITED
 
 controller.errors
 -------------------------------------------------------------------
 
-ID: 18
+ID: 21
+
 Type: uint8
 
 
+
 Any controller errors, as a bitmask
 
 Flags: 
+
 - CURRENT_LIMIT_EXCEEDED
 
+- PRE_CL_I_SD_EXCEEDED
 
 controller.position.setpoint
 -------------------------------------------------------------------
 
-ID: 19
+ID: 22
+
 Type: float
-Units: tick
 
-The position setpoint.
+Units: tick
 
+The position setpoint in the user reference frame.
 
 
 
 controller.position.p_gain
 -------------------------------------------------------------------
 
-ID: 20
+ID: 23
+
 Type: float
 
 
-The proportional gain of the position controller.
 
+The proportional gain of the position controller.
 
 
 
 controller.velocity.setpoint
 -------------------------------------------------------------------
 
-ID: 21
+ID: 24
+
 Type: float
-Units: tick / second
 
-The velocity setpoint.
+Units: tick / second
 
+The velocity setpoint in the user reference frame.
 
 
 
 controller.velocity.limit
 -------------------------------------------------------------------
 
-ID: 22
+ID: 25
+
 Type: float
+
 Units: tick / second
 
 The velocity limit.
 
 
 
-
 controller.velocity.p_gain
 -------------------------------------------------------------------
 
-ID: 23
+ID: 26
+
 Type: float
 
 
-The proportional gain of the velocity controller.
 
+The proportional gain of the velocity controller.
 
 
 
 controller.velocity.i_gain
 -------------------------------------------------------------------
 
-ID: 24
+ID: 27
+
 Type: float
 
 
-The integral gain of the velocity controller.
 
+The integral gain of the velocity controller.
 
 
 
@@ -315,644 +409,941 @@ The integral gain of the velocity controller.
 controller.velocity.deadband
 -------------------------------------------------------------------
 
-ID: 25
+ID: 28
+
 Type: float
+
 Units: tick
 
 The deadband of the velocity integrator. A region around the position setpoint where the velocity integrator is not updated.
 
 
 
-
 controller.velocity.increment
 -------------------------------------------------------------------
 
-ID: 26
+ID: 29
+
 Type: float
 
 
-Max velocity setpoint increment (ramping) rate. Set to 0 to disable.
 
+Max velocity setpoint increment (ramping) rate. Set to 0 to disable.
 
 
 
 controller.current.Iq_setpoint
 -------------------------------------------------------------------
 
-ID: 27
+ID: 30
+
 Type: float
-Units: ampere
 
-The Iq setpoint.
+Units: ampere
 
+The Iq setpoint in the user reference frame.
 
 
 
 controller.current.Id_setpoint
 -------------------------------------------------------------------
 
-ID: 28
+ID: 31
+
 Type: float
-Units: ampere
 
-The Id setpoint.
+Units: ampere
 
+The Id setpoint in the user reference frame.
 
 
 
 controller.current.Iq_limit
 -------------------------------------------------------------------
 
-ID: 29
+ID: 32
+
 Type: float
+
 Units: ampere
 
 The Iq limit.
 
 
 
-
 controller.current.Iq_estimate
 -------------------------------------------------------------------
 
-ID: 30
+ID: 33
+
 Type: float
-Units: ampere
 
-The Iq estimate.
+Units: ampere
 
+The Iq estimate in the user reference frame.
 
 
 
 controller.current.bandwidth
 -------------------------------------------------------------------
 
-ID: 31
+ID: 34
+
 Type: float
+
 Units: hertz
 
 The current controller bandwidth.
 
 
 
-
 controller.current.Iq_p_gain
 -------------------------------------------------------------------
 
-ID: 32
+ID: 35
+
 Type: float
 
 
-The current controller proportional gain.
 
+The current controller proportional gain.
 
 
 
 controller.current.max_Ibus_regen
 -------------------------------------------------------------------
 
-ID: 33
+ID: 36
+
 Type: float
+
 Units: ampere
 
 The max current allowed to be fed back to the power source before flux braking activates.
 
 
 
-
 controller.current.max_Ibrake
 -------------------------------------------------------------------
 
-ID: 34
+ID: 37
+
 Type: float
+
 Units: ampere
 
 The max current allowed to be dumped to the motor windings during flux braking. Set to zero to deactivate flux braking.
 
 
 
-
 controller.voltage.Vq_setpoint
 -------------------------------------------------------------------
 
-ID: 35
+ID: 38
+
 Type: float
+
 Units: volt
 
 The Vq setpoint.
 
 
 
-
 calibrate() -> void
--------------------------------------------------------------------
+--------------------------------------------------------------------------------------------
+
+ID: 39
 
-ID: 36
 Return Type: void
 
 
-Calibrate the device.
 
+Calibrate the device.
 
 idle() -> void
--------------------------------------------------------------------
+--------------------------------------------------------------------------------------------
+
+ID: 40
 
-ID: 37
 Return Type: void
 
 
-Set idle mode, disabling the driver.
 
+Set idle mode, disabling the driver.
 
 position_mode() -> void
--------------------------------------------------------------------
+--------------------------------------------------------------------------------------------
+
+ID: 41
 
-ID: 38
 Return Type: void
 
 
-Set position control mode.
 
+Set position control mode.
 
 velocity_mode() -> void
--------------------------------------------------------------------
+--------------------------------------------------------------------------------------------
+
+ID: 42
 
-ID: 39
 Return Type: void
 
 
-Set velocity control mode.
 
+Set velocity control mode.
 
 current_mode() -> void
--------------------------------------------------------------------
+--------------------------------------------------------------------------------------------
+
+ID: 43
 
-ID: 40
 Return Type: void
 
 
+
 Set current control mode.
 
+set_pos_vel_setpoints(float pos_setpoint, float vel_setpoint) -> float
+--------------------------------------------------------------------------------------------
 
-set_pos_vel_setpoints(pos_setpoint, vel_setpoint) -> float
--------------------------------------------------------------------
+ID: 44
 
-ID: 41
 Return Type: float
 
 
-Set the position and velocity setpoints in one go, and retrieve the position estimate
 
+Set the position and velocity setpoints in the user reference frame in one go, and retrieve the position estimate
 
 .. _api-can-rate:
 
 comms.can.rate
 -------------------------------------------------------------------
 
-ID: 42
+ID: 45
+
 Type: uint32
 
 
-The baud rate of the CAN interface.
 
+The baud rate of the CAN interface.
 
 
 
 comms.can.id
 -------------------------------------------------------------------
 
-ID: 43
+ID: 46
+
 Type: uint32
 
 
-The ID of the CAN interface.
 
+The ID of the CAN interface.
 
 
 
-motor.R
+comms.can.heartbeat
 -------------------------------------------------------------------
 
-ID: 44
-Type: float
-Units: ohm
+ID: 47
 
-The motor Resistance value.
+Type: bool
+
+
+
+Toggle sending of heartbeat messages.
 
 
 
+motor.R
+-------------------------------------------------------------------
+
+ID: 48
+
+Type: float
+
+Units: ohm
+
+The motor Resistance value.
+
+
 
 motor.L
 -------------------------------------------------------------------
 
-ID: 45
+ID: 49
+
 Type: float
+
 Units: henry
 
 The motor Inductance value.
 
 
 
-
 motor.pole_pairs
 -------------------------------------------------------------------
 
-ID: 46
+ID: 50
+
 Type: uint8
 
 
-The motor pole pair count.
 
+The motor pole pair count.
 
 
 
 motor.type
 -------------------------------------------------------------------
 
-ID: 47
+ID: 51
+
 Type: uint8
 
 
+
 The type of the motor. Either high current or gimbal.
 
 Options: 
+
 - HIGH_CURRENT
+
 - GIMBAL
 
+motor.calibrated
+-------------------------------------------------------------------
+
+ID: 52
+
+Type: bool
+
+
+
+Whether the motor has been calibrated.
+
+
 
-motor.offset
+motor.I_cal
 -------------------------------------------------------------------
 
-ID: 48
+ID: 53
+
 Type: float
 
+Units: ampere
+
+The calibration current.
+
+
+
+motor.errors
+-------------------------------------------------------------------
+
+ID: 54
+
+Type: uint8
+
+
+
+Any motor/calibration errors, as a bitmask
+
+Flags: 
+
+- PHASE_RESISTANCE_OUT_OF_RANGE
 
-User-defined offset of the motor.
+- PHASE_INDUCTANCE_OUT_OF_RANGE
 
+- POLE_PAIRS_CALCULATION_DID_NOT_CONVERGE
 
+- POLE_PAIRS_OUT_OF_RANGE
 
+- ABNORMAL_CALIBRATION_VOLTAGE
 
-motor.direction
+sensors.user_frame.position_estimate
 -------------------------------------------------------------------
 
-ID: 49
-Type: int8
+ID: 55
 
+Type: float
 
-User-defined direction of the motor.
+Units: tick
 
+The filtered position estimate in the user reference frame.
 
 
 
-motor.calibrated
+sensors.user_frame.velocity_estimate
 -------------------------------------------------------------------
 
-ID: 50
-Type: bool
+ID: 56
 
+Type: float
 
-Whether the motor has been calibrated.
+Units: tick / second
 
+The filtered velocity estimate in the user reference frame.
 
 
 
-motor.I_cal
+sensors.user_frame.offset
 -------------------------------------------------------------------
 
-ID: 51
+ID: 57
+
 Type: float
-Units: ampere
 
-The calibration current.
+Units: tick
 
+The user defined offset.
 
 
 
-motor.errors
+sensors.user_frame.multiplier
 -------------------------------------------------------------------
 
-ID: 52
+ID: 58
+
+Type: float
+
+
+
+The user defined multipler.
+
+
+
+sensors.setup.onboard.calibrated
+-------------------------------------------------------------------
+
+ID: 59
+
+Type: bool
+
+
+
+Whether the sensor has been calibrated.
+
+
+
+sensors.setup.onboard.errors
+-------------------------------------------------------------------
+
+ID: 60
+
 Type: uint8
 
 
-Any motor/calibration errors, as a bitmask
+
+Any sensor errors, as a bitmask
 
 Flags: 
-- PHASE_RESISTANCE_OUT_OF_RANGE
-- PHASE_INDUCTANCE_OUT_OF_RANGE
-- INVALID_POLE_PAIRS
 
+- CALIBRATION_FAILED
+
+- READING_UNSTABLE
 
-encoder.position_estimate
+sensors.setup.external_spi.type
 -------------------------------------------------------------------
 
-ID: 53
+ID: 61
+
+Type: uint8
+
+
+
+The type of the external sensor.
+
+Options: 
+
+- MA7XX
+
+- AS5047
+
+- AMT22
+
+sensors.setup.external_spi.rate
+-------------------------------------------------------------------
+
+ID: 62
+
+Type: uint8
+
+
+
+The rate of the external sensor.
+
+Options: 
+
+- 1_5Mbps
+
+- 3Mbps
+
+- 6Mbps
+
+- 8Mbps
+
+- 12Mbps
+
+sensors.setup.external_spi.calibrated
+-------------------------------------------------------------------
+
+ID: 63
+
+Type: bool
+
+
+
+Whether the sensor has been calibrated.
+
+
+
+sensors.setup.external_spi.errors
+-------------------------------------------------------------------
+
+ID: 64
+
+Type: uint8
+
+
+
+Any sensor errors, as a bitmask
+
+Flags: 
+
+- CALIBRATION_FAILED
+
+- READING_UNSTABLE
+
+sensors.setup.hall.calibrated
+-------------------------------------------------------------------
+
+ID: 65
+
+Type: bool
+
+
+
+Whether the sensor has been calibrated.
+
+
+
+sensors.setup.hall.errors
+-------------------------------------------------------------------
+
+ID: 66
+
+Type: uint8
+
+
+
+Any sensor errors, as a bitmask
+
+Flags: 
+
+- CALIBRATION_FAILED
+
+- READING_UNSTABLE
+
+sensors.select.position_sensor.connection
+-------------------------------------------------------------------
+
+ID: 67
+
+Type: uint8
+
+
+
+The position sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL.
+
+Options: 
+
+- ONBOARD
+
+- EXTERNAL_SPI
+
+- HALL
+
+sensors.select.position_sensor.bandwidth
+-------------------------------------------------------------------
+
+ID: 68
+
 Type: float
-Units: tick
 
-The filtered encoder position estimate.
+Units: hertz
 
+The position sensor observer bandwidth.
 
 
 
-encoder.velocity_estimate
+sensors.select.position_sensor.raw_angle
 -------------------------------------------------------------------
 
-ID: 54
+ID: 69
+
+Type: int32
+
+
+
+The raw position sensor angle.
+
+
+
+sensors.select.position_sensor.position_estimate
+-------------------------------------------------------------------
+
+ID: 70
+
 Type: float
-Units: tick / second
 
-The filtered encoder velocity estimate.
+Units: tick
 
+The filtered position estimate in the position sensor reference frame.
 
 
 
-encoder.type
+sensors.select.position_sensor.velocity_estimate
 -------------------------------------------------------------------
 
-ID: 55
+ID: 71
+
+Type: float
+
+Units: tick / second
+
+The filtered velocity estimate in the position sensor reference frame.
+
+
+
+sensors.select.commutation_sensor.connection
+-------------------------------------------------------------------
+
+ID: 72
+
 Type: uint8
 
 
-The encoder type. Either INTERNAL or HALL.
+
+The commutation sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL.
 
 Options: 
-- INTERNAL
-- HALL
 
+- ONBOARD
+
+- EXTERNAL_SPI
 
-encoder.bandwidth
+- HALL
+
+sensors.select.commutation_sensor.bandwidth
 -------------------------------------------------------------------
 
-ID: 56
+ID: 73
+
 Type: float
+
 Units: hertz
 
-The encoder observer bandwidth.
+The commutation sensor observer bandwidth.
 
 
 
+sensors.select.commutation_sensor.raw_angle
+-------------------------------------------------------------------
+
+ID: 74
 
-encoder.calibrated
+Type: int32
+
+
+
+The raw commutation sensor angle.
+
+
+
+sensors.select.commutation_sensor.position_estimate
 -------------------------------------------------------------------
 
-ID: 57
-Type: bool
+ID: 75
 
+Type: float
 
-Whether the encoder has been calibrated.
+Units: tick
 
+The filtered position estimate in the commutation sensor reference frame.
 
 
 
-encoder.errors
+sensors.select.commutation_sensor.velocity_estimate
 -------------------------------------------------------------------
 
-ID: 58
-Type: uint8
+ID: 76
 
+Type: float
 
-Any encoder errors, as a bitmask
+Units: tick / second
+
+The filtered velocity estimate in the commutation sensor reference frame.
 
-Flags: 
-- CALIBRATION_FAILED
-- READING_UNSTABLE
 
 
 traj_planner.max_accel
 -------------------------------------------------------------------
 
-ID: 59
+ID: 77
+
 Type: float
+
 Units: tick / second
 
 The max allowed acceleration of the generated trajectory.
 
 
 
-
 traj_planner.max_decel
 -------------------------------------------------------------------
 
-ID: 60
+ID: 78
+
 Type: float
+
 Units: tick / second ** 2
 
 The max allowed deceleration of the generated trajectory.
 
 
 
-
 traj_planner.max_vel
 -------------------------------------------------------------------
 
-ID: 61
+ID: 79
+
 Type: float
+
 Units: tick / second
 
 The max allowed cruise velocity of the generated trajectory.
 
 
 
-
 traj_planner.t_accel
 -------------------------------------------------------------------
 
-ID: 62
+ID: 80
+
 Type: float
+
 Units: second
 
 In time mode, the acceleration time of the generated trajectory.
 
 
 
-
 traj_planner.t_decel
 -------------------------------------------------------------------
 
-ID: 63
+ID: 81
+
 Type: float
+
 Units: second
 
 In time mode, the deceleration time of the generated trajectory.
 
 
 
-
 traj_planner.t_total
 -------------------------------------------------------------------
 
-ID: 64
+ID: 82
+
 Type: float
+
 Units: second
 
 In time mode, the total time of the generated trajectory.
 
 
 
+move_to(float pos_setpoint) -> void
+--------------------------------------------------------------------------------------------
 
-move_to(pos_setpoint) -> void
--------------------------------------------------------------------
+ID: 83
 
-ID: 65
 Return Type: void
 
 
-Move to target position respecting velocity and acceleration limits.
 
+Move to target position in the user reference frame respecting velocity and acceleration limits.
 
-move_to_tlimit(pos_setpoint) -> void
--------------------------------------------------------------------
+move_to_tlimit(float pos_setpoint) -> void
+--------------------------------------------------------------------------------------------
+
+ID: 84
 
-ID: 66
 Return Type: void
 
 
-Move to target position respecting time limits for each sector.
 
+Move to target position in the user reference frame respecting time limits for each sector.
 
 traj_planner.errors
 -------------------------------------------------------------------
 
-ID: 67
+ID: 85
+
 Type: uint8
 
 
+
 Any errors in the trajectory planner, as a bitmask
 
 Flags: 
+
 - INVALID_INPUT
-- VCRUISE_OVER_LIMIT
 
+- VCRUISE_OVER_LIMIT
 
 homing.velocity
 -------------------------------------------------------------------
 
-ID: 68
+ID: 86
+
 Type: float
+
 Units: tick / second
 
 The velocity at which the motor performs homing.
 
 
 
-
 homing.max_homing_t
 -------------------------------------------------------------------
 
-ID: 69
+ID: 87
+
 Type: float
+
 Units: second
 
 The maximum time the motor is allowed to travel before homing times out and aborts.
 
 
 
-
 homing.retract_dist
 -------------------------------------------------------------------
 
-ID: 70
+ID: 88
+
 Type: float
+
 Units: tick
 
 The retraction distance the motor travels after the endstop has been found.
 
 
 
-
 homing.warnings
 -------------------------------------------------------------------
 
-ID: 71
+ID: 89
+
 Type: uint8
 
 
+
 Any homing warnings, as a bitmask
 
 Flags: 
-- HOMING_TIMEOUT
 
+- HOMING_TIMEOUT
 
 homing.stall_detect.velocity
 -------------------------------------------------------------------
 
-ID: 72
+ID: 90
+
 Type: float
+
 Units: tick / second
 
 The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered.
 
 
 
-
 homing.stall_detect.delta_pos
 -------------------------------------------------------------------
 
-ID: 73
+ID: 91
+
 Type: float
+
 Units: tick
 
 The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered.
 
 
 
-
 homing.stall_detect.t
 -------------------------------------------------------------------
 
-ID: 74
+ID: 92
+
 Type: float
+
 Units: second
 
 The time to remain in stall detection mode before the motor is considered stalled.
 
 
 
-
 home() -> void
--------------------------------------------------------------------
+--------------------------------------------------------------------------------------------
+
+ID: 93
 
-ID: 75
 Return Type: void
 
 
-Perform the homing operation.
 
+Perform the homing operation.
 
 watchdog.enabled
 -------------------------------------------------------------------
 
-ID: 76
+ID: 94
+
 Type: bool
 
 
-Whether the watchdog is enabled or not.
 
+Whether the watchdog is enabled or not.
 
 
 
 watchdog.triggered
 -------------------------------------------------------------------
 
-ID: 77
+ID: 95
+
 Type: bool
 
 
-Whether the watchdog has been triggered or not.
 
+Whether the watchdog has been triggered or not.
 
 
 
 watchdog.timeout
 -------------------------------------------------------------------
 
-ID: 78
+ID: 96
+
 Type: float
+
 Units: second
 
 The watchdog timeout period.
 
 
 
-
diff --git a/docs/sensors/AMT22_M52.jpg b/docs/sensors/AMT22_M52.jpg
new file mode 100644
index 00000000..c1ef2012
Binary files /dev/null and b/docs/sensors/AMT22_M52.jpg differ
diff --git a/docs/sensors/AS5047_M52.jpg b/docs/sensors/AS5047_M52.jpg
new file mode 100644
index 00000000..2fac2db7
Binary files /dev/null and b/docs/sensors/AS5047_M52.jpg differ
diff --git a/docs/sensors/MA7XX_M52.jpg b/docs/sensors/MA7XX_M52.jpg
new file mode 100644
index 00000000..5c41fb47
Binary files /dev/null and b/docs/sensors/MA7XX_M52.jpg differ
diff --git a/docs/sensors/connectors_R51.jpg b/docs/sensors/connectors_R51.jpg
new file mode 100644
index 00000000..72f504b2
Binary files /dev/null and b/docs/sensors/connectors_R51.jpg differ
diff --git a/docs/sensors/connectors_R52.jpg b/docs/sensors/connectors_R52.jpg
new file mode 100644
index 00000000..1a17039b
Binary files /dev/null and b/docs/sensors/connectors_R52.jpg differ
diff --git a/docs/sensors/hall_pinout.jpg b/docs/sensors/hall_pinout.jpg
deleted file mode 100644
index a3785c81..00000000
Binary files a/docs/sensors/hall_pinout.jpg and /dev/null differ
diff --git a/docs/sensors/hubmotor_diagram.png b/docs/sensors/hubmotor_diagram_R51.png
similarity index 100%
rename from docs/sensors/hubmotor_diagram.png
rename to docs/sensors/hubmotor_diagram_R51.png
diff --git a/docs/sensors/hubmotor_diagram_R52.jpg b/docs/sensors/hubmotor_diagram_R52.jpg
new file mode 100644
index 00000000..79ad155c
Binary files /dev/null and b/docs/sensors/hubmotor_diagram_R52.jpg differ
diff --git a/docs/sensors/reference_frames.png b/docs/sensors/reference_frames.png
new file mode 100644
index 00000000..9116a89d
Binary files /dev/null and b/docs/sensors/reference_frames.png differ
diff --git a/docs/sensors/sensors.rst b/docs/sensors/sensors.rst
index b00a4d9c..1882239a 100644
--- a/docs/sensors/sensors.rst
+++ b/docs/sensors/sensors.rst
@@ -1,20 +1,162 @@
 Sensors and Encoders
 ####################
 
+
 Overview
 ********
 
 This document provides information and guides for using the various angle sensor and encoder types supported by Tinymovr.
 
-Observer bandwidth
+
+Connector Overview
 ******************
 
-Tinymovr uses an observer in order to filter readings from the sensors. The bandwidth value corresponds to the desired observer bandwidth. It is a configurable value and depends on the dynamics that you wish to achieve with your motor. Keep in mind that high bandwidth values used with motors with fewer pole pairs will make the motors oscillate around the setpoint and have a rough tracking performance (perceivable "knocks" when the rotor moves). On the other hand, too low of a bandwidth value may cause the motor to lose tracking in highly dynamic motions. If you are certain such motions will not be possible (e.g. in heavy moving platforms) you may reduce the bandwidth to ensure smoother motion.
+.. image:: connectors_R52.jpg
+  :width: 800
+  :alt: Tinymovr R5.2 connector diagram
+
+.. image:: connectors_R51.jpg
+  :width: 800
+  :alt: Tinymovr R5.1 connector diagram
+
+
+Hardware Setup
+**************
+
+External SPI Sensors
+====================
+
+Tinymovr supports a number of external sensors over the SPI bus. Currently, the MPS MA7xx, the AMS AS5047 and the CUI AMT22 sensors are supported.
+
+.. note::
+  Even though Tinymovr R5.2 has the FLEX2 port which can function as SPI, due to a hardware incompatibility this port does not implement SPI correctly. As such, external sensors are only supported on the M5.x series at the moment.
+
+  Tinymovr R5.3 and above does not have this issue and supports external SPI sensors normally.
+
+
+MPS MA7XX
+------------------
+
+The MPS MA7XX are compact absolute angle magnetic sensors. We offer a breakout that is compatible with Tinymovr M5.x, but you can also use the MPS supplied evaluation board.
+
+.. image:: MA7XX_M52.jpg
+  :width: 800
+  :alt: MA7XX Sensor Breakout connection diagram for the Tinymovr M5.2
+
+A total of six wires need to be connected: 5V, GND, MISO, MOSI, SCLK and CS.
+
+
+AMS AS5047
+------------------
+
+The AMS AS5047 is a compact absolute angle magnetic sensor. It is compatible with Tinymovr M5.x. In the connection diagram below, we consider the AS5047 breakout board issued by AMS.
+
+.. image:: AS5047_M52.jpg
+  :width: 800
+  :alt: AS5047 Sensor connection diagram for the Tinymovr M5.2
+
+A total of six wires need to be connected: 5V, GND, MISO, MOSI, SCLK and CS.
+
+
+CUI AMT22
+------------------
+
+The CUI AMT22 is an absolute angle, 12 or 14-bit capacitive sensor. It is compatible with Tinymovr M5.x.
+
+  .. image:: AMT22_M52.jpg
+    :width: 800
+    :alt: AS5047 Sensor connection diagram for the Tinymovr M5.2
+
+A total of six wires need to be connected: 5V, GND, MISO, MOSI, SCLK and CS If using the AMS AMT-06C-1-036 prototype cable, you can additionally connect the cable shield (black wire) to one GND pin on the CAN bus ports or the SWD port.
+
+Hall Effect Sensor
+==================
+
+To use Hall effect sensors, you need to connect the sensor's power supply, phases and ground to the correct pins on the FLEX1 header of the Tinymovr R5.2 or greater, or the AUX header of the Tinymovr R5.1. Note the U, V and W pins. These need to be connected to the respective pins of the sensor. The pin labeled AUX/T is an input for a thermistor, but is currently not in use. In addition, power supply and GND pins need to be connected to the sensor.
+
+.. note::
+  Tinymovr R5.2 and above supply 5V on the FLEX1 power supply pin. You can safely connect this to the Hall effect sensor + terminal.
+
+  Tinymovr R5.1 supplies 3.3V on the AUX power supply pin. If your sensor uses 5V, or if it needs more than 50mA, you'll need to provide power externally, e.g. through a dedicated buck converter. 
+
+Example
+-------
+
+The figures below shows an example of wiring a hubwheel motor to Tinymovr R5.2 and R5.1 respectively, using the embedded Hall effect sensors of the motor for commutation. 
+
+.. image:: hubmotor_diagram_R52.jpg
+  :width: 800
+  :alt: Wiring diagram for connection of hub motor to Tinymovr R5.2
+
+.. image:: hubmotor_diagram_R51.png
+  :width: 800
+  :alt: Wiring diagram for connection of hub motor to Tinymovr R5.1
+
+
+Units
+*****
+
+In Tinymovr, a 'tick' traditionally represents 1/8192 of a full mechanical rotation. The system utilizes floating-point values, thereby allowing resolution beyond the granularity of a single tick — down to the precision defined by the IEEE754 standard. This means that even when high-resolution sensors (with 16, 18, 20 or more bits) are employed, their precision is fully retained. Internally, sensor measurements are scaled to conform with the 8192-tick representation. In addition, using the Tinymovr client library, you can define commands in any angle unit you with, such as turns, rads, degrees etc. This gives you freedom in your application beyond the tick representation.
+
+
+Reference Frames
+****************
+
+In the context of Tinymovr motor control, reference frames are essential for understanding the transformation of sensor data and user setpoints into motor control signals. The following diagram depicts the reference frames and their interconnections:
+
+The diagram below illustrates the flow of data from the physical sensors through various observers and frames, finally reaching the motor.
+
+.. image:: reference_frames.png
+  :width: 800
+  :alt: Diagram of the reference frames used in the firmware
+
+Position Sensor Frame (PSF)
+===========================
+
+The Position Sensor Frame (PSF) corresponds to the filtered position sensor data. The main function of this frame is to provide feedback on the estimated position and velocity of the rotor, and therefore provide feedback to the position and velocity control loops. As the homing and trajectory planners also rely on position and velocity estimates, this frame also affects those functions.
+
+Commutation Sensor Frame (CSF)
+==============================
+
+The Commutation Sensor Frame (CSF) corresponds to the filtered commutation sensor data. In ths simplest scenario, the position and commutation sensors are the same, as such the PSF and CSF are identical. The main function of this frame is to provide the estimated rotor angle to the current control loop, so that the electrical angle is derived in the Motor Frame, for Field Oriented Control.
+
+.. note::
+  The transform between MF and CSF always has a scale factor of 1. In other words, the commutation sensor is always assumed to be mechanically attached directly to the motor shaft, without reduction.
+
+Motor Frame (MF)
+================
+
+The origin of the Motor Frame (MF) corresponds to the zero electrical angle of the electrical cycle energized during calibration. This is the frame used by current control, and related dq, inverse Park and SVPWM transforms.
+
+User Frame (UF)
+===============
+
+The User Frame is the interface exposed to the Tinymovr API, allowing the user to command the motor using position, veocity and current setpoints. This frame is related to the PSF, so that the user commands are predominantly based on the position data, with the commutation aspect being internally managed by the firmware's observer algorithms.
+
+Frame Transforms
+================
+
+Data from the Position and Commutation Sensors is forwarded to their respective observers. The observers are responsible for filtering the sensor readings and providing position and velocity estimates. This processed data is then translated into two separate frames:
+
+1. The Position Sensor Frame, which carries the filtered position data.
+2. The Commutation Sensor Frame, which ensures the motor's proper electrical commutation.
+
+These frames are then employed to inform the Motor Frame, which is the final reference before actuating the motor. 
+
+As a summary, the following ransforms are derived during calibration and are stored in the Tinymovr firmware:
+
+1. UF <-> PSF
+2. PSF <-> CSF
+3. CSF <-> MF
+4. UF <-> MF
+
+Tinymovr makes use of the XF1 library, which has been developed for this purpose and offers convenience functions to perform transforms, derive transforms from data, as well as inverse and constrained transforms.
+
 
 Onboard Magnetic
 ****************
 
-All Tinymovr controllers feature an onboard magnetic absolute angle sensor that allows high precision angle measurement for efficient commutation and high bandwidth motor control. This is enabled by default and does not require any specific setup, apart from initial angle sensor/motor calibration.
+All Tinymovr controllers feature an onboard magnetic absolute angle sensor that allows high precision angle measurement for efficient commutation and highly dynamic motor control. This is enabled by default and does not require any specific setup, apart from initial reference frame calibration.
 
 The onboard angle sensor is enabled by default, so no special configuration is necessary. Should you need to switch to the onboard sensor, use the following commands:
 
@@ -27,74 +169,168 @@ The onboard angle sensor is enabled by default, so no special configuration is n
 
 The value of 300Hz in bandwidth is the default value configured based on the charasteristics of the onboard angle sensor.
 
+
+Sensor Configuration
+********************
+
+The sensor configuration consists of two steps. The first step concerns the setup of the individual sensors being used, and the second step concerns sensor selection. The corresponding sections in the device spec are `tmx.sensors.setup`, and `tmx.sensors.select`.
+
+Sensor Setup
+============
+
+Onboard Magnetic Sensor
+-----------------------
+
+The Onboard Magnetic Sensor does not require any configuration. In this section the calibration state and any sensor errors can be seen.
+
+
+External SPI Sensor
+-------------------
+
+The External SPI Sensor requires the correct sensor type to be set before enabling it. Three sensors are currently supported, the MPS MA7xx series, the AMS AS504x series, and the CUI AMT22 series. In addition, here you can see the calibration state and sensor errors.
+
+
 Hall Effect Sensor
-******************
+------------------
 
-Tinymovr R5 supports external Hall effect sensors for commutation and positioning. Hall effect sensors generate a specific sequence in the 3 phase Hall effect sensor signal as the rotor moves. By reading this sequence, the rotor position is determined in one of six 60 degree sectors along the electrical cycle. 
+Hall effect sensors generate a specific sequence in the 3 phase Hall effect sensor signal as the rotor moves. By reading this sequence, the rotor position is determined in one of six 60 degree sectors along the electrical cycle. 
 
-Hardware Setup
---------------
+The Hall Effect Sensor does not require any configuration. In this section the calibration state and any sensor errors can be seen.
 
-To use Hall effect sensors, you need to connect the sensor's power supply, phases and ground to the correct pins on the Tinymovr board. The pinout for the Hall effect sensor connector is shown below.
 
-.. image:: hall_pinout.jpg
-  :width: 800
-  :alt: Hall effect sensor connection diagram
+Sensor Selection
+================
+
+Sensor selection can be performed for positioning and for commutation. In both cases, the selection should be performed after hardware setup and any sensor setup has been fully completed, namely if using external sensors, the selection of the sensor type. The selection is among ONBOARD, EXTERNAL_SPI and HALL sensors. Once selection is complete, the Tinymovr needs to undergo calibration.
+
+
+Examples
+********
+
+External AS5047 Sensor for Commutation and Positioning
+======================================================
 
 .. note::
-  The diagram shows the connector side of the board, i.e. the side where the CAN, UART and SPI connectors, and also the DC-link capacitors are located.
+  This is only supported on the Tinymovr M series, and upcoming Tinymovr R versions
+  
+Ensure the hardware is properly connected. 
+
+Then, configure the external sensor type as follows:
+
+.. code-block:: python
+
+    tmx.sensors.setup.external_spi.type = tmx.sensors.setup.external_spi.type.AS5047
+
+Then select the `EXTERNAL_SPI` sensor for each of the position and commutation sensors:
+
+.. code-block:: python
+
+    tmx.sensors.select.commutation_sensor.connection = tmx.sensors.select.commutation_sensor.connection.EXTERNAL_SPI
+    tmx.sensors.select.position_sensor.connection = tmx.sensors.select.position_sensor.connection.EXTERNAL_SPI
+
+At this point, you are ready to perform motor/sensor calibration. This will measure the R and L values of the motor, derive frame transforms and eccentricity compensation tables.
+
+.. code-block:: python
+
+    tmx.controller.calibrate()
+
+After calibration finishes, you should be able to control the motor:
+
+.. code-block:: python
+
+    tmx.controller.velocity_mode()
+    tmx.controller.velocity.setpoint = 8192 # 60 rpm
+
+The motor should now move at a constant velocity.
+
+Once you have determined that the motor behaves as expected, set to idle and perform another config save to persist the configuration:
+
+.. code-block:: python
+
+    tmx.controller.idle()
+    tmx.save_config()
 
-Note the U, V and W pins. These need to be connected to the respective pins of the sensor. The pin labeled T is currently not in use. In addition, the 3.3V power supply and the GND need to be connected to the sensor as well.
+
+External AMT22 Sensor for Positioning and Onboard MA702/704 for Commutation
+===========================================================================
 
 .. note::
-  Tinymovr supplies 3.3V on the AUX power supply pin. If your sensor uses 5V, or if it needs more than 50mA, you'll need to provide power externally, e.g. through a dedicated buck converter. 
+  This is only supported on the Tinymovr M series, and upcoming Tinymovr R versions
 
-Example
-=======
+Ensure the hardware is properly connected. 
 
-The figure below shows an example of wiring a hub motor to Tinymovr, using the embedded Hall effect sensors of the motor for commutation. Note that the power and ground are connected to an external 5V power supply. Alternatively, if your Hall effect sensors are compatible with 3.3V input, you can connect the leads to the onboard connectors.
+Then, configure the external sensor type as follows:
 
-.. image:: hubmotor_diagram.png
-  :width: 800
-  :alt: Wiring diagram for connection of hub motor to Tinymovr
+.. code-block:: python
 
-Configuration
--------------
+    tmx.sensors.setup.external_spi.type = tmx.sensors.setup.external_spi.type.AMT22
 
-As a first step you need to configure the sensor type and observer bandwidth.
+Then select the `EXTERNAL_SPI` sensor for each of the position and commutation sensors:
 
 .. code-block:: python
 
-    tmx.encoder.type = 1
-    tmx.encoder.bandwidth = 100
+    tmx.sensors.select.commutation_sensor.connection = tmx.sensors.select.commutation_sensor.connection.ONBOARD
+    tmx.sensors.select.position_sensor.connection = tmx.sensors.select.position_sensor.connection.EXTERNAL_SPI
 
-This sets the type to Hall effect sensor, and the observer bandwidth to 100Hz. Note that the change of the sensor type will not propagate until the next reset; in effect, the `encoder.type` variable value will still show as the previous one, here as 0, until you finish configuration and reset.
+At this point, you are ready to perform motor/sensor calibration. This will measure the R and L values of the motor, derive frame transforms and eccentricity compensation tables.
 
-Next, you need to set the motor pole pairs:
+.. code-block:: python
+
+    tmx.controller.calibrate()
+
+After calibration finishes, you should be able to control the motor:
 
 .. code-block:: python
 
-    tmx.motor.pole_pairs = 15
-    
-As a final step, save configuration and reset:
+    tmx.controller.velocity_mode()
+    tmx.controller.velocity.setpoint = 8192 # 60 rpm
+
+The motor should now move at a constant velocity.
+
+Once you have determined that the motor behaves as expected, set to idle and perform another config save to persist the configuration:
 
 .. code-block:: python
 
+    tmx.controller.idle()
     tmx.save_config()
-    tmx.reset() # sensor type change is applied after reset
 
-The board needs to be reset following saving of the config, to enable the sensor type change. For safety reasons, any change to the sensor type is only enabled at next boot. 
 
-Next comes tuning of gains. Gains are determined on the resolution of a full mechanical turn fo the motor. When using the onboard magnetic sensor, the resolution is fixed to 8192 ticks. However, when using the Hall effect sensor, the mechanical resolution is variable, and amounts to `6 * pole_pair_count`. As such, if you have a 15 pp motor, your mechanical resolution would be 90. 
+Hall Effect Sensor
+==================
 
-Because of this vast change in resolution (almost 2 orders of magnitude), the gains need to be updated:
+.. note::
+  This is only supported on the Tinymovr R series.
+
+Ensure the hardware is properly connected. 
+
+Then select the `HALL` sensor for each of the position and commutation sensors, and configure the observer bandwidth as follows:
+
+.. code-block:: python
+
+    tmx.sensors.select.commutation_sensor.connection = HALL
+    tmx.sensors.select.position_sensor.connection = HALL
+    tmx.sensors.select.commutation_sensor.bandwidth = 200
+    tmx.sensors.select.position_sensor.bandwidth = 20
+
+This sets the type to Hall effect sensor, and each of the commutation and position observer bandwidths. The commutation observer is set to a higher bandwidth value, in order to ensure that commutation is accurate and a runoff scenario is avoided.
+
+Next, you need to set the motor pole pairs:
+
+.. code-block:: python
+
+    tmx.motor.pole_pairs = 15
+    
+Next comes tuning of gains. Gains are determined on the tick count of a full mechanical turn of the motor. When using the an absolute sensor, the tick count is fixed to 8192 ticks (the resolution can be higher as the tick count is a floating point value). 
+
+When using the Hall effect sensor, the tick count is defined as 8192 ticks in an electrical cycle. Thus, your mechanical cycle tick count is variable, depending on the pole pair count of your motor.
+Because of this it is possible that the gains need to be updated. Below we present an example of values that work well with a 15 pp hoverboard motor:
 
 .. code-block:: python
 
     tmx.controller.position.p_gain = 5
-    tmx.controller.velocity.p_gain = 0.07
+    tmx.controller.velocity.p_gain = 0.00001
 
-The values above are just an example using a 15 pp hoverboard motor. For your own motor, you need to determine these experimentally. In position control mode, start by raising the default velocity gain until your motor experiences oscillations. The back up by a factor of two, and repeat the same for position control. This simple tuning heuristic does not result in an optimal configuration but the gains are workable.
+For your own motor, you need to determine these experimentally. Take a look at :ref:`Tuning` for more information.
 
 At this point, you are ready to perform motor/sensor calibration. This will measure the R and L values of the motor, as well as the hall effect sensor sequence.
 
@@ -102,7 +338,7 @@ At this point, you are ready to perform motor/sensor calibration. This will meas
 
     tmx.controller.calibrate()
 
-After calibration finishes, you should be able to control the motor:
+After calibration finishes, you should be able to control the motor. Note that the default reference frame for the hall sensors maps to 8192 ticks per motor electrical cycle. You can change this by modifying the 
 
 .. code-block:: python
 
@@ -118,3 +354,8 @@ Once you have determined that the motor behaves as expected, set to idle and per
     tmx.controller.idle()
     tmx.save_config()
 
+
+Observer Bandwidth
+******************
+
+Tinymovr uses a second order observer that filters readings from the sensors, and maintains a position and velocity state. The bandwidth value corresponds to the desired observer bandwidth. It is a configurable value and depends on the dynamics that you wish to achieve with your motor. Keep in mind that high bandwidth values used with motors with fewer pole pairs will make the motors oscillate around the setpoint and have a rough tracking performance (perceivable "knocks" when the rotor moves). On the other hand, too low of a bandwidth value may cause the motor to lose tracking in highly dynamic motions. If you are certain such motions will not be possible (e.g. in heavy moving platforms) you may reduce the bandwidth to ensure smoother motion.
\ No newline at end of file
diff --git a/docs/troubleshooting.rst b/docs/troubleshooting.rst
index c76cfd1b..b00673ac 100644
--- a/docs/troubleshooting.rst
+++ b/docs/troubleshooting.rst
@@ -4,4 +4,5 @@ Troubleshooting
 
 Please take a look at `Github Discussions <https://github.com/yconst/Tinymovr/discussions>`_ and `Github Issues <https://github.com/yconst/Tinymovr/issues>`_ , and ask any questions there.
 
-You are more than welcome to join the `Tinymovr Discord Server <https://discord.gg/nM7PnXD4>`_ and ask any questions there, or have a chat with us!
\ No newline at end of file
+You are more than welcome to join the `Tinymovr Discord Server <https://discord.gg/nM7PnXD4>`_ and ask any questions there, or have a chat with us!
+
diff --git a/firmware/Makefile b/firmware/Makefile
index ea9c2846..ac08e9f8 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -108,9 +108,9 @@ debug: OBJECTS += $(BUILDDIR)/bootloader-$(REV).o
 debug: binary
 
 # Upgrade target
-upgrade: CFLAGS += -DNDEBUG -DPAC5XXX_DRIVER_TILE_RAM -DPAC5XXX_DRIVER_SOCBRIDGE_RAM -g0 -O3
-upgrade: CPPFLAGS += -DNDEBUG -DPAC5XXX_DRIVER_TILE_RAM -DPAC5XXX_DRIVER_SOCBRIDGE_RAM -g0 -O3
-upgrade: LDFLAGS += -O3
+upgrade: CFLAGS += -DNDEBUG -DPAC5XXX_DRIVER_TILE_RAM -DPAC5XXX_DRIVER_SOCBRIDGE_RAM -g0 -O2
+upgrade: CPPFLAGS += -DNDEBUG -DPAC5XXX_DRIVER_TILE_RAM -DPAC5XXX_DRIVER_SOCBRIDGE_RAM -g0 -O2
+upgrade: LDFLAGS += -O2
 upgrade: binary
 
 # Release target
@@ -119,6 +119,11 @@ release: upgrade
 
 # Binary target
 binary: print_board_info $(HEX) $(BIN) $(ELF)
+	@echo "============================================="
+	@echo "Successfully built target $(MAKECMDGOALS)"
+	@echo "Board revision is $(REV)"
+	@echo "Fw version is $(GIT_VERSION)"
+	@echo "============================================="
 
 # Objcopy to HEX
 $(HEX): $(ELF)
@@ -153,7 +158,8 @@ clean :
 # Print board revision
 .PHONY: print_board_info
 print_board_info:
-	@echo "===Building Tinymovr Bootloader==="
+	@echo "============================================="
+	@echo "Building target $(MAKECMDGOALS)"
 	@echo "Board revision is $(REV)"
 	@echo "Fw version is $(GIT_VERSION)"
-	@echo "=================================="
+	@echo "============================================="
diff --git a/firmware/bootloader/bootloader-M51.bin b/firmware/bootloader/bootloader-M51.bin
index 0945a943..a7547da8 100755
Binary files a/firmware/bootloader/bootloader-M51.bin and b/firmware/bootloader/bootloader-M51.bin differ
diff --git a/firmware/bootloader/bootloader-R32.bin b/firmware/bootloader/bootloader-R32.bin
index 8409f60e..711b8f7a 100755
Binary files a/firmware/bootloader/bootloader-R32.bin and b/firmware/bootloader/bootloader-R32.bin differ
diff --git a/firmware/bootloader/bootloader-R33.bin b/firmware/bootloader/bootloader-R33.bin
index f407b4d0..e0fcea4f 100755
Binary files a/firmware/bootloader/bootloader-R33.bin and b/firmware/bootloader/bootloader-R33.bin differ
diff --git a/firmware/bootloader/bootloader-R50.bin b/firmware/bootloader/bootloader-R50.bin
index 5dc7410c..c0f21845 100755
Binary files a/firmware/bootloader/bootloader-R50.bin and b/firmware/bootloader/bootloader-R50.bin differ
diff --git a/firmware/bootloader/bootloader-R51.bin b/firmware/bootloader/bootloader-R51.bin
index 936228a3..4b0757b1 100755
Binary files a/firmware/bootloader/bootloader-R51.bin and b/firmware/bootloader/bootloader-R51.bin differ
diff --git a/firmware/bootloader/bootloader-R52.bin b/firmware/bootloader/bootloader-R52.bin
index 73ecce31..f9010e2c 100755
Binary files a/firmware/bootloader/bootloader-R52.bin and b/firmware/bootloader/bootloader-R52.bin differ
diff --git a/firmware/bootloader/bootloader-R53.bin b/firmware/bootloader/bootloader-R53.bin
new file mode 100755
index 00000000..3eb1be1d
Binary files /dev/null and b/firmware/bootloader/bootloader-R53.bin differ
diff --git a/firmware/build_all.sh b/firmware/build_all.sh
index 03fc2eab..62e5e58f 100755
--- a/firmware/build_all.sh
+++ b/firmware/build_all.sh
@@ -7,7 +7,7 @@ DEST_DIR="./release_binaries"
 mkdir -p "$DEST_DIR"
 
 # Board revisions for which the firmware needs to be built
-REVISIONS=("R32" "R33" "R50" "R51" "R52" "M51")
+REVISIONS=("R32" "R33" "R50" "R51" "R52" "R53" "M51")
 
 # Build types
 TYPES=("release" "upgrade")
diff --git a/firmware/pac55xx.ld b/firmware/pac55xx.ld
index 3a8cf26f..85b7edd0 100644
--- a/firmware/pac55xx.ld
+++ b/firmware/pac55xx.ld
@@ -4,7 +4,8 @@ MEMORY
 {
 	rom0 (rx) : ORIGIN = 0x00000000, LENGTH = 0x00001000
 	rom (rx)  : ORIGIN = 0x00001000, LENGTH = 0x0001F000
-	ram (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00008000
+	ram (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00007F00
+	SHARED (rwx) : ORIGIN = 0x20007F00, LENGTH = 0x00000100
 }
 
 _eram = 0x20000000 + 0x00008000;
@@ -139,6 +140,12 @@ SECTIONS
 	__StackTop = ORIGIN(ram ) + LENGTH(ram );
 	__StackLimit = __StackTop - SIZEOF(.co_stack);
 	PROVIDE(__stack = __StackTop);
+
+	.shared (NOLOAD):
+    {
+        /* place all symbols in input sections that start with .shared */
+        KEEP(*(*.shared*))
+    } > SHARED
 	
 	/* Check if data + heap + stack exceeds ram  limit */
 	ASSERT(__StackLimit >= __HeapLimit, "region ram  overflowed with stack")
diff --git a/firmware/src/adc/adc.c b/firmware/src/adc/adc.c
index 179fd7fb..c9ca1123 100644
--- a/firmware/src/adc/adc.c
+++ b/firmware/src/adc/adc.c
@@ -17,6 +17,7 @@
 
 #include <math.h>
 #include <string.h>
+#include <src/can/can_endpoints.h>
 #include <src/motor/motor.h>
 #include <src/controller/controller.h>
 #include <src/adc/adc.h>
@@ -233,37 +234,37 @@ void ADC_DTSE_Init(void)
     pac5xxx_dtse_seq_config(18, ADC0, 0, ADC_IRQ0_EN, SEQ_END); // Get result at DTSERES18, Interrupt
 }
 
-TM_RAMFUNC float adc_get_mcu_temp(void)
+bool ADC_calibrate_offset(void)
+{
+    // We only need to wait here, the ADC loop will
+    // perform the offset calibration automatically
+    wait_pwm_cycles(10000);
+    return true;
+}
+
+TM_RAMFUNC float ADC_get_mcu_temp(void)
 {
     return adc_state.temp;
 }
 
-TM_RAMFUNC void ADC_GetPhaseCurrents(FloatTriplet *phc)
+TM_RAMFUNC void ADC_get_phase_currents(FloatTriplet *phc)
 {
     phc->A = adc_state.I_phase_meas.A;
-    if (motor_phases_swapped())
-    {
-        phc->B = adc_state.I_phase_meas.C;
-        phc->C = adc_state.I_phase_meas.B;
-    }
-    else
-    {
-        phc->B = adc_state.I_phase_meas.B;
-        phc->C = adc_state.I_phase_meas.C;
-    }
+    phc->B = adc_state.I_phase_meas.B;
+    phc->C = adc_state.I_phase_meas.C;
 }
 
 TM_RAMFUNC void ADC_update(void)
 {
     switch (controller_get_state())
     {
-        case STATE_CALIBRATE:
+        case CONTROLLER_STATE_CALIBRATE:
         {
             adc_config.I_phase_offset.A += (((float)PAC55XX_ADC->DTSERES6.VAL * SHUNT_SCALING_FACTOR) - adc_config.I_phase_offset.A) * adc_state.I_phase_offset_D;
             adc_config.I_phase_offset.B += (((float)PAC55XX_ADC->DTSERES8.VAL * SHUNT_SCALING_FACTOR) - adc_config.I_phase_offset.B) * adc_state.I_phase_offset_D;
             adc_config.I_phase_offset.C += (((float)PAC55XX_ADC->DTSERES10.VAL * SHUNT_SCALING_FACTOR) - adc_config.I_phase_offset.C) * adc_state.I_phase_offset_D;
         }
-        case STATE_CL_CONTROL:
+        case CONTROLLER_STATE_CL_CONTROL:
         {
             const float i_a = (((float)PAC55XX_ADC->DTSERES14.VAL * SHUNT_SCALING_FACTOR) - adc_config.I_phase_offset.A);
             const float i_b = (((float)PAC55XX_ADC->DTSERES16.VAL * SHUNT_SCALING_FACTOR) - adc_config.I_phase_offset.B);
diff --git a/firmware/src/adc/adc.h b/firmware/src/adc/adc.h
index 5c19e218..38008d5d 100644
--- a/firmware/src/adc/adc.h
+++ b/firmware/src/adc/adc.h
@@ -106,8 +106,9 @@ typedef struct
 
 void ADC_init(void);
 void ADC_reset(void);
-float adc_get_mcu_temp(void);
-void ADC_GetPhaseCurrents(FloatTriplet *phc);
+bool ADC_calibrate_offset(void);
+float ADC_get_mcu_temp(void);
+void ADC_get_phase_currents(FloatTriplet *phc);
 void ADC_update(void);
 
 ADCConfig *ADC_get_config(void);
diff --git a/firmware/src/can/can.c b/firmware/src/can/can.c
index d48ce0f7..d3720ab5 100644
--- a/firmware/src/can/can.c
+++ b/firmware/src/can/can.c
@@ -33,9 +33,14 @@
 static CANConfig config = {
     .id = 1,
     .kbaud_rate = CAN_BAUD_1000KHz,
-    .heartbeat_period = 1000};
+    .heartbeat_period = 1000
+};
 
-static CANState state ={0};
+static CANState can_state ={
+    .faults = 0,
+    .last_msg_ms = 0,
+    .send_heartbeat = true
+};
 
 extern volatile uint32_t msTicks;
 
@@ -44,7 +49,16 @@ const size_t endpoint_count = sizeof(avlos_endpoints) / sizeof(avlos_endpoints[0
 
 void CAN_init(void)
 {
-#if defined(BOARD_REV_R52)
+#if defined(BOARD_REV_R53)
+    // Clearing bits for MUX F this way completely crashed the mcu
+    // no hardfault or anything, just debugger disconnected. 
+    // It was not possible connect via swd afterwards so the
+    // board got bricked. Thus avoid this command. Thankfully
+    // it is not necessary for enabling the gpio.
+    // PAC55XX_SCC->PFMUXSEL.w &= 0xFFFFFF0F;        // Clear bits to select GPIO function
+    PAC55XX_GPIOF->MODE.P5 = IO_PUSH_PULL_OUTPUT; // GPIO configured as an output
+    PAC55XX_GPIOF->OUT.P5 = 0;                    // Set low to force transceiver into normal mode
+#elif defined(BOARD_REV_R52)
     PAC55XX_SCC->PDMUXSEL.w &= 0xFFFFFF0F;        // Clear bits to select GPIO function
     PAC55XX_GPIOD->MODE.P7 = IO_PUSH_PULL_OUTPUT; // GPIO configured as an output
     PAC55XX_GPIOD->OUT.P7 = 0;                    // Set low to force transceiver into normal mode
@@ -79,6 +93,7 @@ void CAN_init(void)
     PAC55XX_GPIOD->MODE.P4 = IO_PUSH_PULL_OUTPUT; // GPIO configured as an output
     PAC55XX_GPIOD->OUT.P4 = 0;                    // Set low to force transceiver into normal mode
 
+
 #endif
 
     can_io_config(CAN_BUS_PINS);
@@ -134,6 +149,16 @@ void CAN_set_ID(uint8_t id)
     }
 }
 
+bool CAN_get_send_heartbeat(void)
+{
+    return can_state.send_heartbeat;
+}
+
+void CAN_set_send_heartbeat(bool value)
+{
+    can_state.send_heartbeat = value;
+}
+
 void CAN_process_interrupt(void)
 {
     can_process_extended();
@@ -148,7 +173,7 @@ void CAN_process_interrupt(void)
         uint8_t response_type = callback(can_msg_buffer, &data_length, (uint8_t)rtr);
         if ((AVLOS_RET_READ == response_type || AVLOS_RET_CALL == response_type) && (data_length > 0))
         {
-            state.last_msg_ms = msTicks;
+            can_state.last_msg_ms = msTicks;
             can_transmit_extended(data_length, rx_id, can_msg_buffer);
         }
     }
@@ -167,14 +192,17 @@ void CAN_restore_config(CANConfig *config_)
 
 void CAN_update(void) {
     // Transmit heartbeat
-    const uint32_t msg_diff = msTicks - state.last_msg_ms;
-    if (msg_diff >= config.heartbeat_period && PAC55XX_CAN->SR.TBS != 0)
+    if (can_state.send_heartbeat == true)
     {
-        state.last_msg_ms = msTicks;
-        uint32_t proto_hash = _avlos_get_proto_hash();
-        uint8_t buf[8];
-        memcpy(buf, &proto_hash, sizeof(proto_hash));
-        memcpy((buf+sizeof(proto_hash)), GIT_VERSION, 4);
-        can_transmit_extended(sizeof(proto_hash)+4, 0x700 | config.id, buf);
+        const uint32_t msg_diff = msTicks - can_state.last_msg_ms;
+        if (msg_diff >= config.heartbeat_period && PAC55XX_CAN->SR.TBS != 0)
+        {
+            can_state.last_msg_ms = msTicks;
+            uint32_t proto_hash = _avlos_get_proto_hash();
+            uint8_t buf[8];
+            memcpy(buf, &proto_hash, sizeof(proto_hash));
+            memcpy((buf+sizeof(proto_hash)), GIT_VERSION, 4);
+            can_transmit_extended(sizeof(proto_hash)+4, 0x700 | config.id, buf);
+        }
     }
 }
diff --git a/firmware/src/can/can.h b/firmware/src/can/can.h
index 4b56340a..337917f7 100644
--- a/firmware/src/can/can.h
+++ b/firmware/src/can/can.h
@@ -28,6 +28,7 @@ typedef struct
 {
     uint8_t faults;
     uint32_t last_msg_ms;
+    bool send_heartbeat;
 } CANState;
 
 void CAN_init(void);
@@ -37,6 +38,9 @@ uint8_t CAN_get_ID(void);
 void CAN_set_ID(uint8_t id);
 void CAN_process_interrupt(void);
 
+bool CAN_get_send_heartbeat(void);
+void CAN_set_send_heartbeat(bool value);
+
 CANConfig *CAN_get_config(void);
 void CAN_restore_config(CANConfig *config_);
 
diff --git a/firmware/src/can/can_endpoints.c b/firmware/src/can/can_endpoints.c
index f9b6ec0f..9e843035 100644
--- a/firmware/src/can/can_endpoints.c
+++ b/firmware/src/can/can_endpoints.c
@@ -8,7 +8,7 @@
 #include <string.h>
 #include <src/adc/adc.h>
 #include <src/system/system.h>
-#include <src/encoder/encoder.h>
+#include <src/sensor/sensors.h>
 #include <src/observer/observer.h>
 #include <src/motor/motor.h>
 #include <src/scheduler/scheduler.h>
@@ -18,7 +18,7 @@
 #include <src/can/can_endpoints.h>
 
 
-uint8_t (*avlos_endpoints[79])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) = {&avlos_protocol_hash, &avlos_uid, &avlos_fw_version, &avlos_hw_revision, &avlos_Vbus, &avlos_Ibus, &avlos_power, &avlos_temp, &avlos_calibrated, &avlos_errors, &avlos_save_config, &avlos_erase_config, &avlos_reset, &avlos_enter_dfu, &avlos_scheduler_errors, &avlos_controller_state, &avlos_controller_mode, &avlos_controller_warnings, &avlos_controller_errors, &avlos_controller_position_setpoint, &avlos_controller_position_p_gain, &avlos_controller_velocity_setpoint, &avlos_controller_velocity_limit, &avlos_controller_velocity_p_gain, &avlos_controller_velocity_i_gain, &avlos_controller_velocity_deadband, &avlos_controller_velocity_increment, &avlos_controller_current_Iq_setpoint, &avlos_controller_current_Id_setpoint, &avlos_controller_current_Iq_limit, &avlos_controller_current_Iq_estimate, &avlos_controller_current_bandwidth, &avlos_controller_current_Iq_p_gain, &avlos_controller_current_max_Ibus_regen, &avlos_controller_current_max_Ibrake, &avlos_controller_voltage_Vq_setpoint, &avlos_controller_calibrate, &avlos_controller_idle, &avlos_controller_position_mode, &avlos_controller_velocity_mode, &avlos_controller_current_mode, &avlos_controller_set_pos_vel_setpoints, &avlos_comms_can_rate, &avlos_comms_can_id, &avlos_motor_R, &avlos_motor_L, &avlos_motor_pole_pairs, &avlos_motor_type, &avlos_motor_offset, &avlos_motor_direction, &avlos_motor_calibrated, &avlos_motor_I_cal, &avlos_motor_errors, &avlos_encoder_position_estimate, &avlos_encoder_velocity_estimate, &avlos_encoder_type, &avlos_encoder_bandwidth, &avlos_encoder_calibrated, &avlos_encoder_errors, &avlos_traj_planner_max_accel, &avlos_traj_planner_max_decel, &avlos_traj_planner_max_vel, &avlos_traj_planner_t_accel, &avlos_traj_planner_t_decel, &avlos_traj_planner_t_total, &avlos_traj_planner_move_to, &avlos_traj_planner_move_to_tlimit, &avlos_traj_planner_errors, &avlos_homing_velocity, &avlos_homing_max_homing_t, &avlos_homing_retract_dist, &avlos_homing_warnings, &avlos_homing_stall_detect_velocity, &avlos_homing_stall_detect_delta_pos, &avlos_homing_stall_detect_t, &avlos_homing_home, &avlos_watchdog_enabled, &avlos_watchdog_triggered, &avlos_watchdog_timeout };
+uint8_t (*avlos_endpoints[97])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd) = {&avlos_protocol_hash, &avlos_uid, &avlos_fw_version, &avlos_hw_revision, &avlos_Vbus, &avlos_Ibus, &avlos_power, &avlos_temp, &avlos_calibrated, &avlos_errors, &avlos_warnings, &avlos_save_config, &avlos_erase_config, &avlos_reset, &avlos_enter_dfu, &avlos_config_size, &avlos_scheduler_load, &avlos_scheduler_warnings, &avlos_controller_state, &avlos_controller_mode, &avlos_controller_warnings, &avlos_controller_errors, &avlos_controller_position_setpoint, &avlos_controller_position_p_gain, &avlos_controller_velocity_setpoint, &avlos_controller_velocity_limit, &avlos_controller_velocity_p_gain, &avlos_controller_velocity_i_gain, &avlos_controller_velocity_deadband, &avlos_controller_velocity_increment, &avlos_controller_current_Iq_setpoint, &avlos_controller_current_Id_setpoint, &avlos_controller_current_Iq_limit, &avlos_controller_current_Iq_estimate, &avlos_controller_current_bandwidth, &avlos_controller_current_Iq_p_gain, &avlos_controller_current_max_Ibus_regen, &avlos_controller_current_max_Ibrake, &avlos_controller_voltage_Vq_setpoint, &avlos_controller_calibrate, &avlos_controller_idle, &avlos_controller_position_mode, &avlos_controller_velocity_mode, &avlos_controller_current_mode, &avlos_controller_set_pos_vel_setpoints, &avlos_comms_can_rate, &avlos_comms_can_id, &avlos_comms_can_heartbeat, &avlos_motor_R, &avlos_motor_L, &avlos_motor_pole_pairs, &avlos_motor_type, &avlos_motor_calibrated, &avlos_motor_I_cal, &avlos_motor_errors, &avlos_sensors_user_frame_position_estimate, &avlos_sensors_user_frame_velocity_estimate, &avlos_sensors_user_frame_offset, &avlos_sensors_user_frame_multiplier, &avlos_sensors_setup_onboard_calibrated, &avlos_sensors_setup_onboard_errors, &avlos_sensors_setup_external_spi_type, &avlos_sensors_setup_external_spi_rate, &avlos_sensors_setup_external_spi_calibrated, &avlos_sensors_setup_external_spi_errors, &avlos_sensors_setup_hall_calibrated, &avlos_sensors_setup_hall_errors, &avlos_sensors_select_position_sensor_connection, &avlos_sensors_select_position_sensor_bandwidth, &avlos_sensors_select_position_sensor_raw_angle, &avlos_sensors_select_position_sensor_position_estimate, &avlos_sensors_select_position_sensor_velocity_estimate, &avlos_sensors_select_commutation_sensor_connection, &avlos_sensors_select_commutation_sensor_bandwidth, &avlos_sensors_select_commutation_sensor_raw_angle, &avlos_sensors_select_commutation_sensor_position_estimate, &avlos_sensors_select_commutation_sensor_velocity_estimate, &avlos_traj_planner_max_accel, &avlos_traj_planner_max_decel, &avlos_traj_planner_max_vel, &avlos_traj_planner_t_accel, &avlos_traj_planner_t_decel, &avlos_traj_planner_t_total, &avlos_traj_planner_move_to, &avlos_traj_planner_move_to_tlimit, &avlos_traj_planner_errors, &avlos_homing_velocity, &avlos_homing_max_homing_t, &avlos_homing_retract_dist, &avlos_homing_warnings, &avlos_homing_stall_detect_velocity, &avlos_homing_stall_detect_delta_pos, &avlos_homing_stall_detect_t, &avlos_homing_home, &avlos_watchdog_enabled, &avlos_watchdog_triggered, &avlos_watchdog_timeout };
 
 uint32_t _avlos_get_proto_hash(void)
 {
@@ -110,7 +110,7 @@ uint8_t avlos_temp(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
         float v;
-        v = adc_get_mcu_temp();
+        v = ADC_get_mcu_temp();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -142,6 +142,18 @@ if (AVLOS_CMD_READ == cmd) {
     return AVLOS_RET_NOACTION;
 }
 
+uint8_t avlos_warnings(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        uint8_t v;
+        v = system_get_warnings();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
 uint8_t avlos_save_config(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
     nvm_save_config();
@@ -151,7 +163,7 @@ uint8_t avlos_save_config(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command
 
 uint8_t avlos_erase_config(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
-    nvm_erase();
+    nvm_erase_and_reset();
 
     return AVLOS_RET_CALL;
 }
@@ -170,11 +182,35 @@ uint8_t avlos_enter_dfu(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cm
     return AVLOS_RET_CALL;
 }
 
-uint8_t avlos_scheduler_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+uint8_t avlos_config_size(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        uint32_t v;
+        v = system_get_config_size();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_scheduler_load(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        uint32_t v;
+        v = scheduler_get_load();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_scheduler_warnings(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
         uint8_t v;
-        v = scheduler_get_errors();
+        v = scheduler_get_warnings();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -336,7 +372,7 @@ uint8_t avlos_controller_velocity_i_gain(uint8_t * buffer, uint8_t * buffer_len,
 {
 if (AVLOS_CMD_READ == cmd) {
         float v;
-        v = controller_get_vel_integrator_gain();
+        v = controller_get_vel_integral_gain();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -344,7 +380,7 @@ if (AVLOS_CMD_READ == cmd) {
 else if (AVLOS_CMD_WRITE == cmd) {
         float v;
         memcpy(&v, buffer, sizeof(v));
-        controller_set_vel_integrator_gain(v);
+        controller_set_vel_integral_gain(v);
         return AVLOS_RET_WRITE;
     }
     return AVLOS_RET_NOACTION;
@@ -354,7 +390,7 @@ uint8_t avlos_controller_velocity_deadband(uint8_t * buffer, uint8_t * buffer_le
 {
 if (AVLOS_CMD_READ == cmd) {
         float v;
-        v = controller_get_vel_integrator_deadband();
+        v = controller_get_vel_integral_deadband();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -362,7 +398,7 @@ if (AVLOS_CMD_READ == cmd) {
 else if (AVLOS_CMD_WRITE == cmd) {
         float v;
         memcpy(&v, buffer, sizeof(v));
-        controller_set_vel_integrator_deadband(v);
+        controller_set_vel_integral_deadband(v);
         return AVLOS_RET_WRITE;
     }
     return AVLOS_RET_NOACTION;
@@ -568,7 +604,7 @@ uint8_t avlos_controller_set_pos_vel_setpoints(uint8_t * buffer, uint8_t * buffe
     float vel_setpoint;
     memcpy(&vel_setpoint, buffer+_offset, sizeof(vel_setpoint));
     _offset += sizeof(vel_setpoint);
-    float ret_val = controller_set_pos_vel_setpoints(pos_setpoint, vel_setpoint);
+    float ret_val = controller_set_pos_vel_setpoints_user_frame(pos_setpoint, vel_setpoint);
     memcpy(buffer, &ret_val, sizeof(ret_val));
     *buffer_len = sizeof(ret_val);
 
@@ -611,6 +647,24 @@ else if (AVLOS_CMD_WRITE == cmd) {
     return AVLOS_RET_NOACTION;
 }
 
+uint8_t avlos_comms_can_heartbeat(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        bool v;
+        v = CAN_get_send_heartbeat();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+else if (AVLOS_CMD_WRITE == cmd) {
+        bool v;
+        memcpy(&v, buffer, sizeof(v));
+        CAN_set_send_heartbeat(v);
+        return AVLOS_RET_WRITE;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
 uint8_t avlos_motor_R(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
@@ -683,11 +737,77 @@ else if (AVLOS_CMD_WRITE == cmd) {
     return AVLOS_RET_NOACTION;
 }
 
-uint8_t avlos_motor_offset(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+uint8_t avlos_motor_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        bool v;
+        v = motor_get_calibrated();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_motor_I_cal(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        float v;
+        v = motor_get_I_cal();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+else if (AVLOS_CMD_WRITE == cmd) {
+        float v;
+        memcpy(&v, buffer, sizeof(v));
+        motor_set_I_cal(v);
+        return AVLOS_RET_WRITE;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_motor_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        uint8_t v;
+        v = motor_get_errors();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_sensors_user_frame_position_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        float v;
+        v = user_frame_get_pos_estimate();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_sensors_user_frame_velocity_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        float v;
+        v = user_frame_get_vel_estimate();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_sensors_user_frame_offset(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
         float v;
-        v = motor_get_user_offset();
+        v = frame_user_to_position_sensor_get_offset();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -695,35 +815,35 @@ if (AVLOS_CMD_READ == cmd) {
 else if (AVLOS_CMD_WRITE == cmd) {
         float v;
         memcpy(&v, buffer, sizeof(v));
-        motor_set_user_offset(v);
+        frame_user_to_position_sensor_set_offset(v);
         return AVLOS_RET_WRITE;
     }
     return AVLOS_RET_NOACTION;
 }
 
-uint8_t avlos_motor_direction(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+uint8_t avlos_sensors_user_frame_multiplier(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
-        int8_t v;
-        v = motor_get_user_direction();
+        float v;
+        v = frame_user_to_position_sensor_get_multiplier();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
     }
 else if (AVLOS_CMD_WRITE == cmd) {
-        int8_t v;
+        float v;
         memcpy(&v, buffer, sizeof(v));
-        motor_set_user_direction(v);
+        frame_user_to_position_sensor_set_multiplier(v);
         return AVLOS_RET_WRITE;
     }
     return AVLOS_RET_NOACTION;
 }
 
-uint8_t avlos_motor_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+uint8_t avlos_sensors_setup_onboard_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
         bool v;
-        v = motor_get_calibrated();
+        v = sensor_onboard_get_is_calibrated();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -731,11 +851,125 @@ if (AVLOS_CMD_READ == cmd) {
     return AVLOS_RET_NOACTION;
 }
 
-uint8_t avlos_motor_I_cal(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+uint8_t avlos_sensors_setup_onboard_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        uint8_t v;
+        v = sensor_onboard_get_errors();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_sensors_setup_external_spi_type(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        uint8_t v;
+        v = sensor_external_spi_get_type_avlos();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+else if (AVLOS_CMD_WRITE == cmd) {
+        uint8_t v;
+        memcpy(&v, buffer, sizeof(v));
+        sensor_external_spi_set_type_avlos(v);
+        return AVLOS_RET_WRITE;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_sensors_setup_external_spi_rate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        uint8_t v;
+        v = sensor_external_spi_get_rate_avlos();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+else if (AVLOS_CMD_WRITE == cmd) {
+        uint8_t v;
+        memcpy(&v, buffer, sizeof(v));
+        sensor_external_spi_set_rate_avlos(v);
+        return AVLOS_RET_WRITE;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_sensors_setup_external_spi_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        bool v;
+        v = sensor_external_spi_get_is_calibrated();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_sensors_setup_external_spi_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        uint8_t v;
+        v = sensor_external_spi_get_errors();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_sensors_setup_hall_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        bool v;
+        v = sensor_hall_get_is_calibrated();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_sensors_setup_hall_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        uint8_t v;
+        v = sensor_hall_get_errors();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_sensors_select_position_sensor_connection(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        uint8_t v;
+        v = position_sensor_get_connection();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+else if (AVLOS_CMD_WRITE == cmd) {
+        uint8_t v;
+        memcpy(&v, buffer, sizeof(v));
+        position_sensor_set_connection(v);
+        return AVLOS_RET_WRITE;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_sensors_select_position_sensor_bandwidth(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
         float v;
-        v = motor_get_I_cal();
+        v = position_observer_get_bandwidth();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -743,17 +977,17 @@ if (AVLOS_CMD_READ == cmd) {
 else if (AVLOS_CMD_WRITE == cmd) {
         float v;
         memcpy(&v, buffer, sizeof(v));
-        motor_set_I_cal(v);
+        position_observer_set_bandwidth(v);
         return AVLOS_RET_WRITE;
     }
     return AVLOS_RET_NOACTION;
 }
 
-uint8_t avlos_motor_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+uint8_t avlos_sensors_select_position_sensor_raw_angle(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
-        uint8_t v;
-        v = motor_get_errors();
+        int32_t v;
+        v = sensor_position_get_raw_angle();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -761,11 +995,11 @@ if (AVLOS_CMD_READ == cmd) {
     return AVLOS_RET_NOACTION;
 }
 
-uint8_t avlos_encoder_position_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+uint8_t avlos_sensors_select_position_sensor_position_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
         float v;
-        v = observer_get_pos_estimate_user_frame();
+        v = position_observer_get_pos_estimate();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -773,11 +1007,11 @@ if (AVLOS_CMD_READ == cmd) {
     return AVLOS_RET_NOACTION;
 }
 
-uint8_t avlos_encoder_velocity_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+uint8_t avlos_sensors_select_position_sensor_velocity_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
         float v;
-        v = observer_get_vel_estimate_user_frame();
+        v = position_observer_get_vel_estimate();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -785,11 +1019,11 @@ if (AVLOS_CMD_READ == cmd) {
     return AVLOS_RET_NOACTION;
 }
 
-uint8_t avlos_encoder_type(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+uint8_t avlos_sensors_select_commutation_sensor_connection(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
         uint8_t v;
-        v = encoder_get_type();
+        v = commutation_sensor_get_connection();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -797,17 +1031,17 @@ if (AVLOS_CMD_READ == cmd) {
 else if (AVLOS_CMD_WRITE == cmd) {
         uint8_t v;
         memcpy(&v, buffer, sizeof(v));
-        encoder_set_type(v);
+        commutation_sensor_set_connection(v);
         return AVLOS_RET_WRITE;
     }
     return AVLOS_RET_NOACTION;
 }
 
-uint8_t avlos_encoder_bandwidth(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+uint8_t avlos_sensors_select_commutation_sensor_bandwidth(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
         float v;
-        v = observer_get_bw();
+        v = commutation_observer_get_bandwidth();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -815,17 +1049,17 @@ if (AVLOS_CMD_READ == cmd) {
 else if (AVLOS_CMD_WRITE == cmd) {
         float v;
         memcpy(&v, buffer, sizeof(v));
-        observer_set_bw(v);
+        commutation_observer_set_bandwidth(v);
         return AVLOS_RET_WRITE;
     }
     return AVLOS_RET_NOACTION;
 }
 
-uint8_t avlos_encoder_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+uint8_t avlos_sensors_select_commutation_sensor_raw_angle(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
-        bool v;
-        v = encoder_get_calibrated();
+        int32_t v;
+        v = sensor_commutation_get_raw_angle();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -833,11 +1067,23 @@ if (AVLOS_CMD_READ == cmd) {
     return AVLOS_RET_NOACTION;
 }
 
-uint8_t avlos_encoder_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+uint8_t avlos_sensors_select_commutation_sensor_position_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
-        uint8_t v;
-        v = encoder_get_errors();
+        float v;
+        v = commutation_observer_get_pos_estimate();
+        *buffer_len = sizeof(v);
+        memcpy(buffer, &v, sizeof(v));
+        return AVLOS_RET_READ;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+uint8_t avlos_sensors_select_commutation_sensor_velocity_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        float v;
+        v = commutation_observer_get_vel_estimate();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
diff --git a/firmware/src/can/can_endpoints.h b/firmware/src/can/can_endpoints.h
index 11fbdf12..952f3baa 100644
--- a/firmware/src/can/can_endpoints.h
+++ b/firmware/src/can/can_endpoints.h
@@ -8,95 +8,10 @@
 
 #pragma once
 #include <src/common.h>
+#include <src/tm_enums.h>
 
-typedef enum
-{
-    AVLOS_RET_NOACTION,
-    AVLOS_RET_READ = 1,
-    AVLOS_RET_WRITE = 2,
-    AVLOS_RET_CALL = 3
-} Avlos_Return;
-
-typedef enum
-{
-    AVLOS_CMD_WRITE,
-    AVLOS_CMD_READ = 1
-} Avlos_Command;
-
-typedef enum
-{
-    ERRORS_NONE = 0,
-    ERRORS_UNDERVOLTAGE = (1 << 0), 
-    ERRORS_DRIVER_FAULT = (1 << 1), 
-    ERRORS_CHARGE_PUMP_FAULT_STAT = (1 << 2), 
-    ERRORS_CHARGE_PUMP_FAULT = (1 << 3), 
-    ERRORS_DRV10_DISABLE = (1 << 4), 
-    ERRORS_DRV32_DISABLE = (1 << 5), 
-    ERRORS_DRV54_DISABLE = (1 << 6)
-} errors_flags;
-
-typedef enum
-{
-    SCHEDULER_ERRORS_NONE = 0,
-    SCHEDULER_ERRORS_CONTROL_BLOCK_REENTERED = (1 << 0)
-} scheduler_errors_flags;
-
-typedef enum
-{
-    CONTROLLER_WARNINGS_NONE = 0,
-    CONTROLLER_WARNINGS_VELOCITY_LIMITED = (1 << 0), 
-    CONTROLLER_WARNINGS_CURRENT_LIMITED = (1 << 1), 
-    CONTROLLER_WARNINGS_MODULATION_LIMITED = (1 << 2)
-} controller_warnings_flags;
-
-typedef enum
-{
-    CONTROLLER_ERRORS_NONE = 0,
-    CONTROLLER_ERRORS_CURRENT_LIMIT_EXCEEDED = (1 << 0)
-} controller_errors_flags;
-
-typedef enum
-{
-    MOTOR_ERRORS_NONE = 0,
-    MOTOR_ERRORS_PHASE_RESISTANCE_OUT_OF_RANGE = (1 << 0), 
-    MOTOR_ERRORS_PHASE_INDUCTANCE_OUT_OF_RANGE = (1 << 1), 
-    MOTOR_ERRORS_INVALID_POLE_PAIRS = (1 << 2)
-} motor_errors_flags;
-
-typedef enum
-{
-    ENCODER_ERRORS_NONE = 0,
-    ENCODER_ERRORS_CALIBRATION_FAILED = (1 << 0), 
-    ENCODER_ERRORS_READING_UNSTABLE = (1 << 1)
-} encoder_errors_flags;
-
-typedef enum
-{
-    TRAJ_PLANNER_ERRORS_NONE = 0,
-    TRAJ_PLANNER_ERRORS_INVALID_INPUT = (1 << 0), 
-    TRAJ_PLANNER_ERRORS_VCRUISE_OVER_LIMIT = (1 << 1)
-} traj_planner_errors_flags;
-
-typedef enum
-{
-    HOMING_WARNINGS_NONE = 0,
-    HOMING_WARNINGS_HOMING_TIMEOUT = (1 << 0)
-} homing_warnings_flags;
-
-typedef enum
-{
-    MOTOR_TYPE_HIGH_CURRENT = 0, 
-    MOTOR_TYPE_GIMBAL = 1
-} motor_type_options;
-
-typedef enum
-{
-    ENCODER_TYPE_INTERNAL = 0, 
-    ENCODER_TYPE_HALL = 1
-} encoder_type_options;
-
-static const uint32_t avlos_proto_hash = 3526126264;
-extern uint8_t (*avlos_endpoints[79])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+static const uint32_t avlos_proto_hash = 641680925;
+extern uint8_t (*avlos_endpoints[97])(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 extern uint32_t _avlos_get_proto_hash(void);
 
 /*
@@ -104,6 +19,8 @@ extern uint32_t _avlos_get_proto_hash(void);
 *
 * The Avlos protocol hash.
 *
+* Endpoint ID: 0
+*
 * @param buffer
 * @param buffer_len
 */
@@ -114,6 +31,8 @@ uint8_t avlos_protocol_hash(uint8_t * buffer, uint8_t * buffer_len, Avlos_Comman
 *
 * The unique device ID, unique to each PAC55xx chip produced.
 *
+* Endpoint ID: 1
+*
 * @param buffer
 * @param buffer_len
 */
@@ -124,6 +43,8 @@ uint8_t avlos_uid(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 *
 * The firmware version.
 *
+* Endpoint ID: 2
+*
 * @param buffer
 * @param buffer_len
 */
@@ -134,6 +55,8 @@ uint8_t avlos_fw_version(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command c
 *
 * The hardware revision.
 *
+* Endpoint ID: 3
+*
 * @param buffer
 * @param buffer_len
 */
@@ -144,6 +67,8 @@ uint8_t avlos_hw_revision(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command
 *
 * The measured bus voltage.
 *
+* Endpoint ID: 4
+*
 * @param buffer
 * @param buffer_len
 */
@@ -154,6 +79,8 @@ uint8_t avlos_Vbus(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 *
 * The estimated bus current. Only estimates current drawn by motor.
 *
+* Endpoint ID: 5
+*
 * @param buffer
 * @param buffer_len
 */
@@ -164,6 +91,8 @@ uint8_t avlos_Ibus(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 *
 * The estimated power. Only estimates power drawn by motor.
 *
+* Endpoint ID: 6
+*
 * @param buffer
 * @param buffer_len
 */
@@ -174,6 +103,8 @@ uint8_t avlos_power(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 *
 * The internal temperature of the PAC55xx MCU.
 *
+* Endpoint ID: 7
+*
 * @param buffer
 * @param buffer_len
 */
@@ -184,6 +115,8 @@ uint8_t avlos_temp(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 *
 * Whether the system has been calibrated.
 *
+* Endpoint ID: 8
+*
 * @param buffer
 * @param buffer_len
 */
@@ -194,16 +127,32 @@ uint8_t avlos_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command c
 *
 * Any system errors, as a bitmask
 *
+* Endpoint ID: 9
+*
 * @param buffer
 * @param buffer_len
 */
 uint8_t avlos_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
+/*
+* avlos_warnings
+*
+* Any system warnings, as a bitmask
+*
+* Endpoint ID: 10
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_warnings(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
 /*
 * avlos_save_config
 *
 * Save configuration to non-volatile memory.
 *
+* Endpoint ID: 11
+*
 * @param buffer
 * @param buffer_len
 */
@@ -214,6 +163,8 @@ uint8_t avlos_save_config(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command
 *
 * Erase the config stored in non-volatile memory and reset the device.
 *
+* Endpoint ID: 12
+*
 * @param buffer
 * @param buffer_len
 */
@@ -224,6 +175,8 @@ uint8_t avlos_erase_config(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command
 *
 * Reset the device.
 *
+* Endpoint ID: 13
+*
 * @param buffer
 * @param buffer_len
 */
@@ -234,26 +187,56 @@ uint8_t avlos_reset(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 *
 * Enter DFU mode.
 *
+* Endpoint ID: 14
+*
 * @param buffer
 * @param buffer_len
 */
 uint8_t avlos_enter_dfu(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_scheduler_errors
+* avlos_config_size
+*
+* Size (in bytes) of the configuration object.
+*
+* Endpoint ID: 15
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_config_size(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_scheduler_load
+*
+* Processor load in ticks per PWM cycle.
 *
-* Any scheduler errors, as a bitmask
+* Endpoint ID: 16
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_scheduler_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+uint8_t avlos_scheduler_load(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_scheduler_warnings
+*
+* Any scheduler warnings, as a bitmask
+*
+* Endpoint ID: 17
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_scheduler_warnings(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
 * avlos_controller_state
 *
 * The state of the controller.
 *
+* Endpoint ID: 18
+*
 * @param buffer
 * @param buffer_len
 */
@@ -264,6 +247,8 @@ uint8_t avlos_controller_state(uint8_t * buffer, uint8_t * buffer_len, Avlos_Com
 *
 * The control mode of the controller.
 *
+* Endpoint ID: 19
+*
 * @param buffer
 * @param buffer_len
 */
@@ -274,6 +259,8 @@ uint8_t avlos_controller_mode(uint8_t * buffer, uint8_t * buffer_len, Avlos_Comm
 *
 * Any controller warnings, as a bitmask
 *
+* Endpoint ID: 20
+*
 * @param buffer
 * @param buffer_len
 */
@@ -284,6 +271,8 @@ uint8_t avlos_controller_warnings(uint8_t * buffer, uint8_t * buffer_len, Avlos_
 *
 * Any controller errors, as a bitmask
 *
+* Endpoint ID: 21
+*
 * @param buffer
 * @param buffer_len
 */
@@ -292,7 +281,9 @@ uint8_t avlos_controller_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Co
 /*
 * avlos_controller_position_setpoint
 *
-* The position setpoint.
+* The position setpoint in the user reference frame.
+*
+* Endpoint ID: 22
 *
 * @param buffer
 * @param buffer_len
@@ -304,6 +295,8 @@ uint8_t avlos_controller_position_setpoint(uint8_t * buffer, uint8_t * buffer_le
 *
 * The proportional gain of the position controller.
 *
+* Endpoint ID: 23
+*
 * @param buffer
 * @param buffer_len
 */
@@ -312,7 +305,9 @@ uint8_t avlos_controller_position_p_gain(uint8_t * buffer, uint8_t * buffer_len,
 /*
 * avlos_controller_velocity_setpoint
 *
-* The velocity setpoint.
+* The velocity setpoint in the user reference frame.
+*
+* Endpoint ID: 24
 *
 * @param buffer
 * @param buffer_len
@@ -324,6 +319,8 @@ uint8_t avlos_controller_velocity_setpoint(uint8_t * buffer, uint8_t * buffer_le
 *
 * The velocity limit.
 *
+* Endpoint ID: 25
+*
 * @param buffer
 * @param buffer_len
 */
@@ -334,6 +331,8 @@ uint8_t avlos_controller_velocity_limit(uint8_t * buffer, uint8_t * buffer_len,
 *
 * The proportional gain of the velocity controller.
 *
+* Endpoint ID: 26
+*
 * @param buffer
 * @param buffer_len
 */
@@ -344,6 +343,8 @@ uint8_t avlos_controller_velocity_p_gain(uint8_t * buffer, uint8_t * buffer_len,
 *
 * The integral gain of the velocity controller.
 *
+* Endpoint ID: 27
+*
 * @param buffer
 * @param buffer_len
 */
@@ -354,6 +355,8 @@ uint8_t avlos_controller_velocity_i_gain(uint8_t * buffer, uint8_t * buffer_len,
 *
 * The deadband of the velocity integrator. A region around the position setpoint where the velocity integrator is not updated.
 *
+* Endpoint ID: 28
+*
 * @param buffer
 * @param buffer_len
 */
@@ -364,6 +367,8 @@ uint8_t avlos_controller_velocity_deadband(uint8_t * buffer, uint8_t * buffer_le
 *
 * Max velocity setpoint increment (ramping) rate. Set to 0 to disable.
 *
+* Endpoint ID: 29
+*
 * @param buffer
 * @param buffer_len
 */
@@ -372,7 +377,9 @@ uint8_t avlos_controller_velocity_increment(uint8_t * buffer, uint8_t * buffer_l
 /*
 * avlos_controller_current_Iq_setpoint
 *
-* The Iq setpoint.
+* The Iq setpoint in the user reference frame.
+*
+* Endpoint ID: 30
 *
 * @param buffer
 * @param buffer_len
@@ -382,7 +389,9 @@ uint8_t avlos_controller_current_Iq_setpoint(uint8_t * buffer, uint8_t * buffer_
 /*
 * avlos_controller_current_Id_setpoint
 *
-* The Id setpoint.
+* The Id setpoint in the user reference frame.
+*
+* Endpoint ID: 31
 *
 * @param buffer
 * @param buffer_len
@@ -394,6 +403,8 @@ uint8_t avlos_controller_current_Id_setpoint(uint8_t * buffer, uint8_t * buffer_
 *
 * The Iq limit.
 *
+* Endpoint ID: 32
+*
 * @param buffer
 * @param buffer_len
 */
@@ -402,7 +413,9 @@ uint8_t avlos_controller_current_Iq_limit(uint8_t * buffer, uint8_t * buffer_len
 /*
 * avlos_controller_current_Iq_estimate
 *
-* The Iq estimate.
+* The Iq estimate in the user reference frame.
+*
+* Endpoint ID: 33
 *
 * @param buffer
 * @param buffer_len
@@ -414,6 +427,8 @@ uint8_t avlos_controller_current_Iq_estimate(uint8_t * buffer, uint8_t * buffer_
 *
 * The current controller bandwidth.
 *
+* Endpoint ID: 34
+*
 * @param buffer
 * @param buffer_len
 */
@@ -424,6 +439,8 @@ uint8_t avlos_controller_current_bandwidth(uint8_t * buffer, uint8_t * buffer_le
 *
 * The current controller proportional gain.
 *
+* Endpoint ID: 35
+*
 * @param buffer
 * @param buffer_len
 */
@@ -434,6 +451,8 @@ uint8_t avlos_controller_current_Iq_p_gain(uint8_t * buffer, uint8_t * buffer_le
 *
 * The max current allowed to be fed back to the power source before flux braking activates.
 *
+* Endpoint ID: 36
+*
 * @param buffer
 * @param buffer_len
 */
@@ -444,6 +463,8 @@ uint8_t avlos_controller_current_max_Ibus_regen(uint8_t * buffer, uint8_t * buff
 *
 * The max current allowed to be dumped to the motor windings during flux braking. Set to zero to deactivate flux braking.
 *
+* Endpoint ID: 37
+*
 * @param buffer
 * @param buffer_len
 */
@@ -454,6 +475,8 @@ uint8_t avlos_controller_current_max_Ibrake(uint8_t * buffer, uint8_t * buffer_l
 *
 * The Vq setpoint.
 *
+* Endpoint ID: 38
+*
 * @param buffer
 * @param buffer_len
 */
@@ -464,6 +487,8 @@ uint8_t avlos_controller_voltage_Vq_setpoint(uint8_t * buffer, uint8_t * buffer_
 *
 * Calibrate the device.
 *
+* Endpoint ID: 39
+*
 * @param buffer
 * @param buffer_len
 */
@@ -474,6 +499,8 @@ uint8_t avlos_controller_calibrate(uint8_t * buffer, uint8_t * buffer_len, Avlos
 *
 * Set idle mode, disabling the driver.
 *
+* Endpoint ID: 40
+*
 * @param buffer
 * @param buffer_len
 */
@@ -484,6 +511,8 @@ uint8_t avlos_controller_idle(uint8_t * buffer, uint8_t * buffer_len, Avlos_Comm
 *
 * Set position control mode.
 *
+* Endpoint ID: 41
+*
 * @param buffer
 * @param buffer_len
 */
@@ -494,6 +523,8 @@ uint8_t avlos_controller_position_mode(uint8_t * buffer, uint8_t * buffer_len, A
 *
 * Set velocity control mode.
 *
+* Endpoint ID: 42
+*
 * @param buffer
 * @param buffer_len
 */
@@ -504,6 +535,8 @@ uint8_t avlos_controller_velocity_mode(uint8_t * buffer, uint8_t * buffer_len, A
 *
 * Set current control mode.
 *
+* Endpoint ID: 43
+*
 * @param buffer
 * @param buffer_len
 */
@@ -512,7 +545,9 @@ uint8_t avlos_controller_current_mode(uint8_t * buffer, uint8_t * buffer_len, Av
 /*
 * avlos_controller_set_pos_vel_setpoints
 *
-* Set the position and velocity setpoints in one go, and retrieve the position estimate
+* Set the position and velocity setpoints in the user reference frame in one go, and retrieve the position estimate
+*
+* Endpoint ID: 44
 *
 * @param buffer
 * @param buffer_len
@@ -524,6 +559,8 @@ uint8_t avlos_controller_set_pos_vel_setpoints(uint8_t * buffer, uint8_t * buffe
 *
 * The baud rate of the CAN interface.
 *
+* Endpoint ID: 45
+*
 * @param buffer
 * @param buffer_len
 */
@@ -534,16 +571,32 @@ uint8_t avlos_comms_can_rate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Comma
 *
 * The ID of the CAN interface.
 *
+* Endpoint ID: 46
+*
 * @param buffer
 * @param buffer_len
 */
 uint8_t avlos_comms_can_id(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
+/*
+* avlos_comms_can_heartbeat
+*
+* Toggle sending of heartbeat messages.
+*
+* Endpoint ID: 47
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_comms_can_heartbeat(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
 /*
 * avlos_motor_R
 *
 * The motor Resistance value.
 *
+* Endpoint ID: 48
+*
 * @param buffer
 * @param buffer_len
 */
@@ -554,6 +607,8 @@ uint8_t avlos_motor_R(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 *
 * The motor Inductance value.
 *
+* Endpoint ID: 49
+*
 * @param buffer
 * @param buffer_len
 */
@@ -564,6 +619,8 @@ uint8_t avlos_motor_L(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 *
 * The motor pole pair count.
 *
+* Endpoint ID: 50
+*
 * @param buffer
 * @param buffer_len
 */
@@ -574,126 +631,320 @@ uint8_t avlos_motor_pole_pairs(uint8_t * buffer, uint8_t * buffer_len, Avlos_Com
 *
 * The type of the motor. Either high current or gimbal.
 *
+* Endpoint ID: 51
+*
 * @param buffer
 * @param buffer_len
 */
 uint8_t avlos_motor_type(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_motor_offset
+* avlos_motor_calibrated
+*
+* Whether the motor has been calibrated.
 *
-* User-defined offset of the motor.
+* Endpoint ID: 52
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_motor_offset(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+uint8_t avlos_motor_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_motor_direction
+* avlos_motor_I_cal
+*
+* The calibration current.
 *
-* User-defined direction of the motor.
+* Endpoint ID: 53
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_motor_direction(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+uint8_t avlos_motor_I_cal(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_motor_calibrated
+* avlos_motor_errors
 *
-* Whether the motor has been calibrated.
+* Any motor/calibration errors, as a bitmask
+*
+* Endpoint ID: 54
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_motor_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+uint8_t avlos_motor_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_motor_I_cal
+* avlos_sensors_user_frame_position_estimate
 *
-* The calibration current.
+* The filtered position estimate in the user reference frame.
+*
+* Endpoint ID: 55
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_motor_I_cal(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+uint8_t avlos_sensors_user_frame_position_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_motor_errors
+* avlos_sensors_user_frame_velocity_estimate
 *
-* Any motor/calibration errors, as a bitmask
+* The filtered velocity estimate in the user reference frame.
+*
+* Endpoint ID: 56
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_motor_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+uint8_t avlos_sensors_user_frame_velocity_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_user_frame_offset
+*
+* The user defined offset.
+*
+* Endpoint ID: 57
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_user_frame_offset(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_encoder_position_estimate
+* avlos_sensors_user_frame_multiplier
+*
+* The user defined multipler.
 *
-* The filtered encoder position estimate.
+* Endpoint ID: 58
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_encoder_position_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+uint8_t avlos_sensors_user_frame_multiplier(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_encoder_velocity_estimate
+* avlos_sensors_setup_onboard_calibrated
 *
-* The filtered encoder velocity estimate.
+* Whether the sensor has been calibrated.
+*
+* Endpoint ID: 59
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_encoder_velocity_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+uint8_t avlos_sensors_setup_onboard_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_encoder_type
+* avlos_sensors_setup_onboard_errors
+*
+* Any sensor errors, as a bitmask
 *
-* The encoder type. Either INTERNAL or HALL.
+* Endpoint ID: 60
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_encoder_type(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+uint8_t avlos_sensors_setup_onboard_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_encoder_bandwidth
+* avlos_sensors_setup_external_spi_type
 *
-* The encoder observer bandwidth.
+* The type of the external sensor.
+*
+* Endpoint ID: 61
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_encoder_bandwidth(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+uint8_t avlos_sensors_setup_external_spi_type(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_encoder_calibrated
+* avlos_sensors_setup_external_spi_rate
+*
+* The rate of the external sensor.
 *
-* Whether the encoder has been calibrated.
+* Endpoint ID: 62
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_encoder_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+uint8_t avlos_sensors_setup_external_spi_rate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_encoder_errors
+* avlos_sensors_setup_external_spi_calibrated
 *
-* Any encoder errors, as a bitmask
+* Whether the sensor has been calibrated.
+*
+* Endpoint ID: 63
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_encoder_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+uint8_t avlos_sensors_setup_external_spi_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_setup_external_spi_errors
+*
+* Any sensor errors, as a bitmask
+*
+* Endpoint ID: 64
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_setup_external_spi_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_setup_hall_calibrated
+*
+* Whether the sensor has been calibrated.
+*
+* Endpoint ID: 65
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_setup_hall_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_setup_hall_errors
+*
+* Any sensor errors, as a bitmask
+*
+* Endpoint ID: 66
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_setup_hall_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_select_position_sensor_connection
+*
+* The position sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL.
+*
+* Endpoint ID: 67
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_select_position_sensor_connection(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_select_position_sensor_bandwidth
+*
+* The position sensor observer bandwidth.
+*
+* Endpoint ID: 68
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_select_position_sensor_bandwidth(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_select_position_sensor_raw_angle
+*
+* The raw position sensor angle.
+*
+* Endpoint ID: 69
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_select_position_sensor_raw_angle(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_select_position_sensor_position_estimate
+*
+* The filtered position estimate in the position sensor reference frame.
+*
+* Endpoint ID: 70
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_select_position_sensor_position_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_select_position_sensor_velocity_estimate
+*
+* The filtered velocity estimate in the position sensor reference frame.
+*
+* Endpoint ID: 71
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_select_position_sensor_velocity_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_select_commutation_sensor_connection
+*
+* The commutation sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL.
+*
+* Endpoint ID: 72
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_select_commutation_sensor_connection(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_select_commutation_sensor_bandwidth
+*
+* The commutation sensor observer bandwidth.
+*
+* Endpoint ID: 73
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_select_commutation_sensor_bandwidth(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_select_commutation_sensor_raw_angle
+*
+* The raw commutation sensor angle.
+*
+* Endpoint ID: 74
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_select_commutation_sensor_raw_angle(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_select_commutation_sensor_position_estimate
+*
+* The filtered position estimate in the commutation sensor reference frame.
+*
+* Endpoint ID: 75
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_select_commutation_sensor_position_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_select_commutation_sensor_velocity_estimate
+*
+* The filtered velocity estimate in the commutation sensor reference frame.
+*
+* Endpoint ID: 76
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_sensors_select_commutation_sensor_velocity_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
 * avlos_traj_planner_max_accel
 *
 * The max allowed acceleration of the generated trajectory.
 *
+* Endpoint ID: 77
+*
 * @param buffer
 * @param buffer_len
 */
@@ -704,6 +955,8 @@ uint8_t avlos_traj_planner_max_accel(uint8_t * buffer, uint8_t * buffer_len, Avl
 *
 * The max allowed deceleration of the generated trajectory.
 *
+* Endpoint ID: 78
+*
 * @param buffer
 * @param buffer_len
 */
@@ -714,6 +967,8 @@ uint8_t avlos_traj_planner_max_decel(uint8_t * buffer, uint8_t * buffer_len, Avl
 *
 * The max allowed cruise velocity of the generated trajectory.
 *
+* Endpoint ID: 79
+*
 * @param buffer
 * @param buffer_len
 */
@@ -724,6 +979,8 @@ uint8_t avlos_traj_planner_max_vel(uint8_t * buffer, uint8_t * buffer_len, Avlos
 *
 * In time mode, the acceleration time of the generated trajectory.
 *
+* Endpoint ID: 80
+*
 * @param buffer
 * @param buffer_len
 */
@@ -734,6 +991,8 @@ uint8_t avlos_traj_planner_t_accel(uint8_t * buffer, uint8_t * buffer_len, Avlos
 *
 * In time mode, the deceleration time of the generated trajectory.
 *
+* Endpoint ID: 81
+*
 * @param buffer
 * @param buffer_len
 */
@@ -744,6 +1003,8 @@ uint8_t avlos_traj_planner_t_decel(uint8_t * buffer, uint8_t * buffer_len, Avlos
 *
 * In time mode, the total time of the generated trajectory.
 *
+* Endpoint ID: 82
+*
 * @param buffer
 * @param buffer_len
 */
@@ -752,7 +1013,9 @@ uint8_t avlos_traj_planner_t_total(uint8_t * buffer, uint8_t * buffer_len, Avlos
 /*
 * avlos_traj_planner_move_to
 *
-* Move to target position respecting velocity and acceleration limits.
+* Move to target position in the user reference frame respecting velocity and acceleration limits.
+*
+* Endpoint ID: 83
 *
 * @param buffer
 * @param buffer_len
@@ -762,7 +1025,9 @@ uint8_t avlos_traj_planner_move_to(uint8_t * buffer, uint8_t * buffer_len, Avlos
 /*
 * avlos_traj_planner_move_to_tlimit
 *
-* Move to target position respecting time limits for each sector.
+* Move to target position in the user reference frame respecting time limits for each sector.
+*
+* Endpoint ID: 84
 *
 * @param buffer
 * @param buffer_len
@@ -774,6 +1039,8 @@ uint8_t avlos_traj_planner_move_to_tlimit(uint8_t * buffer, uint8_t * buffer_len
 *
 * Any errors in the trajectory planner, as a bitmask
 *
+* Endpoint ID: 85
+*
 * @param buffer
 * @param buffer_len
 */
@@ -784,6 +1051,8 @@ uint8_t avlos_traj_planner_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_
 *
 * The velocity at which the motor performs homing.
 *
+* Endpoint ID: 86
+*
 * @param buffer
 * @param buffer_len
 */
@@ -794,6 +1063,8 @@ uint8_t avlos_homing_velocity(uint8_t * buffer, uint8_t * buffer_len, Avlos_Comm
 *
 * The maximum time the motor is allowed to travel before homing times out and aborts.
 *
+* Endpoint ID: 87
+*
 * @param buffer
 * @param buffer_len
 */
@@ -804,6 +1075,8 @@ uint8_t avlos_homing_max_homing_t(uint8_t * buffer, uint8_t * buffer_len, Avlos_
 *
 * The retraction distance the motor travels after the endstop has been found.
 *
+* Endpoint ID: 88
+*
 * @param buffer
 * @param buffer_len
 */
@@ -814,6 +1087,8 @@ uint8_t avlos_homing_retract_dist(uint8_t * buffer, uint8_t * buffer_len, Avlos_
 *
 * Any homing warnings, as a bitmask
 *
+* Endpoint ID: 89
+*
 * @param buffer
 * @param buffer_len
 */
@@ -824,6 +1099,8 @@ uint8_t avlos_homing_warnings(uint8_t * buffer, uint8_t * buffer_len, Avlos_Comm
 *
 * The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered.
 *
+* Endpoint ID: 90
+*
 * @param buffer
 * @param buffer_len
 */
@@ -834,6 +1111,8 @@ uint8_t avlos_homing_stall_detect_velocity(uint8_t * buffer, uint8_t * buffer_le
 *
 * The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered.
 *
+* Endpoint ID: 91
+*
 * @param buffer
 * @param buffer_len
 */
@@ -844,6 +1123,8 @@ uint8_t avlos_homing_stall_detect_delta_pos(uint8_t * buffer, uint8_t * buffer_l
 *
 * The time to remain in stall detection mode before the motor is considered stalled.
 *
+* Endpoint ID: 92
+*
 * @param buffer
 * @param buffer_len
 */
@@ -854,6 +1135,8 @@ uint8_t avlos_homing_stall_detect_t(uint8_t * buffer, uint8_t * buffer_len, Avlo
 *
 * Perform the homing operation.
 *
+* Endpoint ID: 93
+*
 * @param buffer
 * @param buffer_len
 */
@@ -864,6 +1147,8 @@ uint8_t avlos_homing_home(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command
 *
 * Whether the watchdog is enabled or not.
 *
+* Endpoint ID: 94
+*
 * @param buffer
 * @param buffer_len
 */
@@ -874,6 +1159,8 @@ uint8_t avlos_watchdog_enabled(uint8_t * buffer, uint8_t * buffer_len, Avlos_Com
 *
 * Whether the watchdog has been triggered or not.
 *
+* Endpoint ID: 95
+*
 * @param buffer
 * @param buffer_len
 */
@@ -884,6 +1171,8 @@ uint8_t avlos_watchdog_triggered(uint8_t * buffer, uint8_t * buffer_len, Avlos_C
 *
 * The watchdog timeout period.
 *
+* Endpoint ID: 96
+*
 * @param buffer
 * @param buffer_len
 */
diff --git a/firmware/src/common.h b/firmware/src/common.h
index d64398d9..386f60aa 100644
--- a/firmware/src/common.h
+++ b/firmware/src/common.h
@@ -54,6 +54,8 @@
 #include "pac5xxx_tile_signal_manager.h"
 #include "config.h"
 
+#define ALWAYS_INLINE __attribute__((always_inline)) 
+
 #ifndef PAC5XXX_OK
 #define PAC5XXX_OK 0
 #endif
@@ -72,6 +74,7 @@
 #endif
 
 #define BTL_TRIGGER_PATTERN  0x39
+#define BTL_TRIGGER_PATTERN_RAM  0x5048434D
 
 #define PI (3.141592f)
 #define TWOPI (6.283185f)
@@ -87,17 +90,9 @@
 
 #define ERROR_FLAG_MAX_SIZE (5u)
 
-#define ENCODER_BITS (13)
-#define ENCODER_TICKS (1 << ENCODER_BITS)
-#define ENCODER_TICKS_FLOAT ((float)(ENCODER_TICKS))
-
-#define HALL_SECTORS (6)
-#define HALL_SECTOR_ANGLE (TWOPI / HALL_SECTORS)
-#define CAL_DIR_LEN_PER_SECTOR (CAL_DIR_LEN / HALL_SECTORS)
-
 #if defined BOARD_REV_R32 || defined BOARD_REV_R33
 #define BOARD_REV_R3
-#elif defined BOARD_REV_R50 || BOARD_REV_R51 || defined BOARD_REV_R52
+#elif defined BOARD_REV_R50 || BOARD_REV_R51 || defined BOARD_REV_R52 || defined BOARD_REV_R53
 #define BOARD_REV_R5
 #elif defined BOARD_REV_M51
 #define BOARD_REV_M5
@@ -123,24 +118,38 @@
 #define BOARD_REV_IDX 11
 #elif defined BOARD_REV_R52
 #define BOARD_REV_IDX 12
+#elif defined BOARD_REV_R53
+#define BOARD_REV_IDX 13
 #elif defined BOARD_REV_M50
 #define BOARD_REV_IDX 20
 #elif defined BOARD_REV_M51
 #define BOARD_REV_IDX 21
 #endif
 
+#if defined BOARD_REV_R3 || defined BOARD_REV_R5
+#define MIN_TRIP_CURRENT (5.0f) // A
+#elif defined BOARD_REV_M5
+#define MIN_TRIP_CURRENT (1.0f) // A
+#endif
+
 #define TIMER_FREQ_HZ (ACLK_FREQ_HZ >> TXCTL_PS_DIV)
 
+#define SENSOR_COMMON_RES_BITS (13)
+#define SENSOR_COMMON_RES_TICKS (1 << SENSOR_COMMON_RES_BITS)
+#define SENSOR_COMMON_RES_HALF_TICKS (SENSOR_COMMON_RES_TICKS/2)
+#define SENSOR_COMMON_RES_TICKS_FLOAT ((float)(SENSOR_COMMON_RES_TICKS))
+
+#define MOTOR_MAX_POLE_PAIRS 24
+
 static const float one_by_sqrt3 = 0.57735026919f;
 static const float two_by_sqrt3 = 1.15470053838f;
 static const float threehalfpi = 4.7123889f;
 static const float pi = PI;
 static const float halfpi = PI * 0.5f;
 static const float quarterpi = PI * 0.25f;
-static const float twopi_by_enc_ticks = TWOPI / ENCODER_TICKS;
-static const float twopi_by_hall_sectors = TWOPI / HALL_SECTORS;
+static const float twopi_by_common_ticks = TWOPI / SENSOR_COMMON_RES_TICKS;
 
-_Static_assert (TIMER_FREQ_HZ % (2*PWM_FREQ_HZ) == 0, "Timer frequency not an integer multiple of PWM frequency");
+_Static_assert(TIMER_FREQ_HZ % (2*PWM_FREQ_HZ) == 0, "Timer frequency not an integer multiple of PWM frequency");
 
 typedef struct 
 {
diff --git a/firmware/src/config.h b/firmware/src/config.h
index bfcbe617..91644f40 100644
--- a/firmware/src/config.h
+++ b/firmware/src/config.h
@@ -38,7 +38,8 @@
 #define VBUS_LOW_THRESHOLD          (10.4f)   // V
 #define VEL_HARD_LIMIT              (600000.0f)  // ticks/s
 #define I_HARD_LIMIT                (60.0f)    // A
-#define VEL_INTEGRATOR_THRESHOLD    (2000.0f) // ticks/s
+#define MAX_CL_INIT_STEPS           (200)
+#define PRE_CL_I_SD_MAX            (0.4f)
 
 // Encoder rectification lookup table size
 #define ECN_BITS (6)
diff --git a/firmware/src/controller/controller.c b/firmware/src/controller/controller.c
index 20853127..eb1e9ee7 100644
--- a/firmware/src/controller/controller.c
+++ b/firmware/src/controller/controller.c
@@ -15,19 +15,20 @@
 //  * You should have received a copy of the GNU General Public License
 //  * along with this program. If not, see <http://www.gnu.org/licenses/>.
 
-#include "src/system/system.h"
-#include <src/encoder/encoder.h>
+#include <src/system/system.h>
+#include <src/sensor/sensor.h>
 #include <src/observer/observer.h>
-#include "src/adc/adc.h"
-#include "src/motor/motor.h"
+#include <src/adc/adc.h>
+#include <src/motor/motor.h>
 #include <src/gatedriver/gatedriver.h>
 #include <src/utils/utils.h>
 #include <src/scheduler/scheduler.h>
-#include <src/motor/calibration.h>
 #include <src/can/can_endpoints.h>
 #include <src/controller/controller.h>
 #include "src/watchdog/watchdog.h"
 
+void CLPreStep(void);
+void CLPreCheck(void);
 void CLControlStep(void);
 static inline bool Controller_LimitVelocity(float min_limit, float max_limit, float vel_estimate,
                                                             float vel_gain, float *I);
@@ -35,16 +36,16 @@ static inline bool Controller_LimitVelocity(float min_limit, float max_limit, fl
 static MotionPlan motion_plan;
 static ControllerState state = {
 
-    .state = STATE_IDLE,
-    .mode = CTRL_CURRENT,
+    .state = CONTROLLER_STATE_IDLE,
+    .mode = CONTROLLER_MODE_CURRENT,
     .errors = CONTROLLER_ERRORS_NONE,
     .is_calibrating = false,
 
     .I_phase_meas = {0.0f, 0.0f, 0.0f},
     .modulation_values = {0.0f, 0.0f, 0.0f},
 
-    .Iq_est = 0.0f,
-    .Id_est = 0.0f,
+    .Iq_estimate = 0.0f,
+    .Id_estimate = 0.0f,
 
     .Ibus_est = 0.0f,
     .power_est = 0.0f,
@@ -57,12 +58,15 @@ static ControllerState state = {
 
     .Vq_setpoint = 0.0f,
 
-    .vel_integrator_Iq = 0.0f,
+    .vel_integrator = 0.0f,
 
-    .Iq_integrator_Vq = 0.0f,
-    .Id_integrator_Vd = 0.0f,
+    .Iq_integrator = 0.0f,
+    .Id_integrator = 0.0f,
 
-    .t_plan = 0.0f};
+    .t_plan = 0.0f
+};
+
+Statistics pre_cl_stats = {0};
 
 #if defined BOARD_REV_R32 || BOARD_REV_R33 || defined BOARD_REV_R5
 
@@ -71,12 +75,12 @@ static ControllerConfig config = {
     .I_limit = 10.0f,
     .pos_gain = 20.0f,
     .vel_gain = 8.0e-5f,
-    .vel_integrator_gain = 0.00020f,
-    .vel_integrator_deadband = 200.0f,
+    .vel_integral_gain = 0.00020f,
+    .vel_integral_deadband = 200.0f,
     .I_bw = 2000.0,
     .I_gain = 0.0f,
-    .Iq_integrator_gain = 0.0f,
-    .Id_integrator_gain = 0.0f,
+    .Iq_integral_gain = 0.0f,
+    .Id_integral_gain = 0.0f,
     .I_k = 0.3f,
     .vel_increment = 100.0f, // ticks/cycle
     .max_Ibus_regen = 0.0f,
@@ -89,12 +93,12 @@ static ControllerConfig config = {
     .I_limit = 4.0f,
     .pos_gain = 8.0f,
     .vel_gain = 5.0e-5f,
-    .vel_integrator_gain = 0.00020f,
-    .vel_integrator_deadband = 200.0f,
+    .vel_integral_gain = 0.00020f,
+    .vel_integral_deadband = 200.0f,
     .I_bw = 2000.0,
     .I_gain = 0.0f,
-    .Iq_integrator_gain = 0.0f,
-    .Id_integrator_gain = 0.0f,
+    .Iq_integral_gain = 0.0f,
+    .Id_integral_gain = 0.0f,
     .I_k = 0.3f,
     .vel_increment = 100.0f, // ticks/cycle
     .max_Ibus_regen = 0.0f,
@@ -108,49 +112,68 @@ void Controller_ControlLoop(void)
     {
         state.warnings = 0;
         const float Iq = controller_get_Iq_estimate();
-        if ((Iq > (config.I_limit * I_TRIP_MARGIN)) ||
-            (Iq < -(config.I_limit * I_TRIP_MARGIN)))
+        const float Iq_trip = our_fmaxf(config.I_limit * I_TRIP_MARGIN, MIN_TRIP_CURRENT);
+        if (our_fabsf(Iq) > Iq_trip)
         {
             state.errors |= CONTROLLER_ERRORS_CURRENT_LIMIT_EXCEEDED;
         }
-        if (errors_exist() && (state.state != STATE_IDLE))
+        if (errors_exist() && (state.state != CONTROLLER_STATE_IDLE))
         {
-            controller_set_state(STATE_IDLE);
+            controller_set_state(CONTROLLER_STATE_IDLE);
         }
 
-        if (state.state == STATE_CALIBRATE)
+        if (state.state == CONTROLLER_STATE_CALIBRATE)
         {
             state.is_calibrating = true;
-            reset_calibration();
-            if (ENCODER_MA7XX == encoder_get_type())
-            {
-                (void)((CalibrateADCOffset() && CalibrateResistance() && CalibrateInductance()) && CalibrateDirectionAndPolePairs() && calibrate_offset_and_rectification());
-            }
-            else if (ENCODER_HALL == encoder_get_type())
+            system_reset_calibration();
+            // TODO: sensors_calibrate should also return bool, and be integrated in the calibration sequence
+            if (ADC_calibrate_offset() && motor_calibrate_resistance() && motor_calibrate_inductance())
             {
-                (void)((CalibrateADCOffset() && CalibrateResistance() && CalibrateInductance()) && calibrate_hall_sequence());
+                (void)(sensors_calibrate());
             }
             state.is_calibrating = false;
-            controller_set_state(STATE_IDLE);
+            controller_set_state(CONTROLLER_STATE_IDLE); 
         }
-        else if (state.state == STATE_CL_CONTROL)
+        else if (state.state == CONTROLLER_STATE_CL_CONTROL)
         {
             // Check the watchdog and revert to idle if it has timed out
             if (Watchdog_triggered())
             {
-                controller_set_state(STATE_IDLE);
+                controller_set_state(CONTROLLER_STATE_IDLE);
                 Watchdog_reset();
             }
+            else if ((motor_get_is_gimbal() == false) && pre_cl_stats.size < MAX_CL_INIT_STEPS)
+            {
+                CLPreStep();
+            }
+            else if ((motor_get_is_gimbal() == false) && pre_cl_stats.size == MAX_CL_INIT_STEPS)
+            {
+                state.pos_setpoint = observer_get_pos_estimate(&position_observer);
+                CLPreStep();
+                CLPreCheck();
+            }
             else
             {
                 CLControlStep();
             }
         }
-        else
-        {
-            // pass
-        }
-        WaitForControlLoopInterrupt();
+        wait_for_control_loop_interrupt();
+    }
+}
+
+TM_RAMFUNC void CLPreStep(void)
+{
+    gate_driver_set_duty_cycle(&three_phase_zero);
+    // Should approximate zero as from Kirchoff
+    float Iphase_meas_sum = state.I_phase_meas.A + state.I_phase_meas.B + state.I_phase_meas.C; 
+    update_statistics(&pre_cl_stats, Iphase_meas_sum);
+}
+
+TM_RAMFUNC void CLPreCheck(void)
+{
+    if (calculate_standard_deviation(&pre_cl_stats) > PRE_CL_I_SD_MAX)
+    {
+        state.errors |= CONTROLLER_ERRORS_PRE_CL_I_SD_EXCEEDED;
     }
 }
 
@@ -158,22 +181,22 @@ TM_RAMFUNC void CLControlStep(void)
 {
     switch (state.mode)
     {
-        case CTRL_TRAJECTORY:
+        case CONTROLLER_MODE_TRAJECTORY:
         state.t_plan += PWM_PERIOD_S;
         // This will set state.pos_setpoint state.vel_setpoint (in user frame)
         if (!traj_planner_evaluate(state.t_plan, &motion_plan))
         {
             // Drop to position mode on error or completion
-            controller_set_mode(CTRL_POSITION);
+            controller_set_mode(CONTROLLER_MODE_POSITION);
             state.t_plan = 0;
         }
         break;
-        case CTRL_HOMING:
+        case CONTROLLER_MODE_HOMING:
         // This will set state.pos_setpoint state.vel_setpoint (in user frame)
         if (!homing_planner_evaluate())
         {
             // Drop to position mode on error or completion
-            controller_set_mode(CTRL_POSITION);
+            controller_set_mode(CONTROLLER_MODE_POSITION);
         }
         break;
         default: break;
@@ -195,49 +218,49 @@ TM_RAMFUNC void CLControlStep(void)
     // separate because the latter takes into account a user-configurable deadband
     // around the position setpoint, where the integrator "sees" no error
     float vel_setpoint = state.vel_ramp_setpoint ;
-    float vel_setpoint_integrator = state.vel_ramp_setpoint ;
+    float vel_setpoint_integral = state.vel_ramp_setpoint ;
 
-    if (state.mode >= CTRL_POSITION)
+    if (state.mode >= CONTROLLER_MODE_POSITION)
     {
-        const float delta_pos = observer_get_diff(state.pos_setpoint);
-        const float delta_pos_integrator = sgnf(delta_pos) * our_fmaxf(0, fabsf(delta_pos) - config.vel_integrator_deadband);
+        const float delta_pos = get_diff_position_sensor_frame(state.pos_setpoint);
+        const float delta_pos_integral = sgnf(delta_pos) * our_fmaxf(0, fabsf(delta_pos) - config.vel_integral_deadband);
         vel_setpoint += delta_pos * config.pos_gain;
-        vel_setpoint_integrator += delta_pos_integrator * config.pos_gain;
+        vel_setpoint_integral += delta_pos_integral * config.pos_gain;
     }
 
-    const float vel_estimate = observer_get_vel_estimate();
+    const float vel_estimate = observer_get_vel_estimate(&position_observer);
     float Iq_setpoint = state.Iq_setpoint;
 
-    if (state.mode >= CTRL_VELOCITY)
+    if (state.mode >= CONTROLLER_MODE_VELOCITY) 
     {
         const float delta_vel = vel_setpoint - vel_estimate;
-        const float delta_vel_integrator = vel_setpoint_integrator - vel_estimate;
         // Velocity limiting will be done later on based on the estimate
-        Iq_setpoint += delta_vel * config.vel_gain;
-        Iq_setpoint += state.vel_integrator_Iq;
-        state.vel_integrator_Iq += delta_vel_integrator * PWM_PERIOD_S * config.vel_integrator_gain;
+        Iq_setpoint += apply_velocity_transform(delta_vel * config.vel_gain + state.vel_integrator, frame_position_sensor_to_motor_p());
+        state.vel_integrator += (vel_setpoint_integral - vel_estimate) * PWM_PERIOD_S * config.vel_integral_gain;
     }
     else
     {
-        state.vel_integrator_Iq = 0.0f;
+        state.vel_integrator = 0.0f;
     }
 
     // Velocity-dependent current limiting
-    if (Controller_LimitVelocity(-config.vel_limit, config.vel_limit, vel_estimate, config.vel_gain, &Iq_setpoint) == true)
+    const float vel_estimate_motor_frame = apply_velocity_transform(vel_estimate, frame_position_sensor_to_motor_p());
+    if (Controller_LimitVelocity(-config.vel_limit, config.vel_limit, vel_estimate_motor_frame, config.vel_gain, &Iq_setpoint) == true)
     {
-        state.vel_integrator_Iq *= 0.995f;
+        state.vel_integrator *= 0.995f;
         state.warnings |= CONTROLLER_WARNINGS_VELOCITY_LIMITED;
     }
 
     // Absolute current & velocity integrator limiting
     if (our_clampc(&Iq_setpoint, -config.I_limit, config.I_limit) == true)
     {
-        state.vel_integrator_Iq *= 0.995f;
+        state.vel_integrator *= 0.995f;
         state.warnings |= CONTROLLER_WARNINGS_CURRENT_LIMITED;
     }
 
     // Flux braking
     const float Vbus_voltage = system_get_Vbus();
+    const float one_over_Vbus_voltage = 1.0f / Vbus_voltage;
     if (config.max_Ibrake > 0)
     {
         state.Id_setpoint = our_clamp(-state.Ibus_est*Vbus_voltage, 0, config.max_Ibrake);
@@ -247,7 +270,7 @@ TM_RAMFUNC void CLControlStep(void)
         state.Id_setpoint = 0.0f;
     }
 
-    const float e_phase = observer_get_epos();
+    const float e_phase = observer_get_epos_motor_frame();
     const float c_I = fast_cos(e_phase);
     const float s_I = fast_sin(e_phase);
 
@@ -255,13 +278,13 @@ TM_RAMFUNC void CLControlStep(void)
     float Vq;
     if (motor_get_is_gimbal() == true)
     {
-        const float e_phase_vel = observer_get_evel();
+        const float e_phase_vel = observer_get_evel_motor_frame();
         Vd = -e_phase_vel * motor_get_phase_inductance() * Iq_setpoint;
         Vq = motor_get_phase_resistance() * Iq_setpoint;
     }
     else
     {
-        ADC_GetPhaseCurrents(&(state.I_phase_meas));
+        ADC_get_phase_currents(&(state.I_phase_meas));
 
         // Clarke transform
         const float Ialpha = state.I_phase_meas.A;
@@ -271,23 +294,23 @@ TM_RAMFUNC void CLControlStep(void)
         const float Id = (c_I * Ialpha) + (s_I * Ibeta);
         const float Iq = (c_I * Ibeta) - (s_I * Ialpha);
 
-        state.Id_est += config.I_k * (Id - state.Id_est);
-        state.Iq_est += config.I_k * (Iq - state.Iq_est);
+        state.Id_estimate += config.I_k * (Id - state.Id_estimate);
+        state.Iq_estimate += config.I_k * (Iq - state.Iq_estimate);
 
-        const float delta_Id = state.Id_setpoint - state.Id_est;
-        const float delta_Iq = Iq_setpoint - state.Iq_est;
+        const float delta_Id = state.Id_setpoint - state.Id_estimate;
+        const float delta_Iq = Iq_setpoint - state.Iq_estimate;
 
-        state.Id_integrator_Vd += delta_Id * PWM_PERIOD_S * config.Id_integrator_gain;
-        state.Iq_integrator_Vq += delta_Iq * PWM_PERIOD_S * config.Iq_integrator_gain;
+        state.Id_integrator += delta_Id * PWM_PERIOD_S * config.Id_integral_gain;
+        state.Iq_integrator += delta_Iq * PWM_PERIOD_S * config.Iq_integral_gain;
 
-        Vd = (delta_Id * config.I_gain) + state.Id_integrator_Vd;
-        Vq = (delta_Iq * config.I_gain) + state.Iq_integrator_Vq;
+        Vd = (delta_Id * config.I_gain) + state.Id_integrator;
+        Vq = (delta_Iq * config.I_gain) + state.Iq_integrator;
     }
     state.Vq_setpoint = Vq;
     
-    float mod_q = Vq / Vbus_voltage;
-    float mod_d = Vd / Vbus_voltage;
-    state.Ibus_est = state.Iq_est * mod_q + state.Id_est * mod_d;
+    float mod_q = Vq * one_over_Vbus_voltage;
+    float mod_d = Vd * one_over_Vbus_voltage;
+    state.Ibus_est = state.Iq_estimate * mod_q + state.Id_estimate * mod_d;
     state.power_est = state.Ibus_est * Vbus_voltage;
 
     // dq modulation limiter
@@ -297,14 +320,14 @@ TM_RAMFUNC void CLControlStep(void)
     {
         mod_q *= dq_mod_scale_factor;
         mod_d *= dq_mod_scale_factor;
-        state.Id_integrator_Vd *= I_INTEGRATOR_DECAY_FACTOR;
-        state.Iq_integrator_Vq *= I_INTEGRATOR_DECAY_FACTOR;
+        state.Id_integrator *= I_INTEGRATOR_DECAY_FACTOR;
+        state.Iq_integrator *= I_INTEGRATOR_DECAY_FACTOR;
         state.warnings |= CONTROLLER_WARNINGS_MODULATION_LIMITED;
     }
 
     // Inverse Park transform
-    float mod_a = (c_I * mod_d) - (s_I * mod_q);
-    float mod_b = (c_I * mod_q) + (s_I * mod_d);
+    const float mod_a = (c_I * mod_d) - (s_I * mod_q);
+    const float mod_b = (c_I * mod_q) + (s_I * mod_d);
 
     SVM(mod_a, mod_b, &state.modulation_values.A,
         &state.modulation_values.B, &state.modulation_values.C);
@@ -312,64 +335,64 @@ TM_RAMFUNC void CLControlStep(void)
 }
 
 
-TM_RAMFUNC ControlState controller_get_state(void)
+TM_RAMFUNC controller_state_options controller_get_state(void)
 {
     return state.state;
 }
 
-TM_RAMFUNC void controller_set_state(ControlState new_state)
+TM_RAMFUNC void controller_set_state(controller_state_options new_state)
 {
     if ((new_state != state.state) && (false == state.is_calibrating))
     {
-        if ((new_state == STATE_CL_CONTROL) && (state.state == STATE_IDLE) && (!errors_exist()) && motor_get_calibrated())
+        if ((new_state == CONTROLLER_STATE_CL_CONTROL) && (state.state == CONTROLLER_STATE_IDLE) && (!errors_exist()) && motor_get_calibrated())
         {
-            state.pos_setpoint = observer_get_pos_estimate();
             gate_driver_enable();
-            state.state = STATE_CL_CONTROL;
+            state.state = CONTROLLER_STATE_CL_CONTROL;
         }
-        else if ((new_state == STATE_CALIBRATE) && (state.state == STATE_IDLE) && (!errors_exist()))
+        else if ((new_state == CONTROLLER_STATE_CALIBRATE) && (state.state == CONTROLLER_STATE_IDLE) && (!errors_exist()))
         {
             gate_driver_enable();
-            state.state = STATE_CALIBRATE;
+            state.state = CONTROLLER_STATE_CALIBRATE;
         }
-        else // state != STATE_IDLE --> Got to idle state anyway
+        else // state != CONTROLLER_STATE_IDLE --> Got to idle state anyway
         {
             gate_driver_set_duty_cycle(&three_phase_zero);
             gate_driver_disable();
-            state.state = STATE_IDLE;
+            memset(&pre_cl_stats, 0, sizeof(pre_cl_stats));
+            state.state = CONTROLLER_STATE_IDLE;
         }
     }
 }
 
-TM_RAMFUNC ControlMode controller_get_mode(void)
+TM_RAMFUNC controller_mode_options controller_get_mode(void)
 {
     return state.mode;
 }
 
-TM_RAMFUNC void controller_set_mode(ControlMode new_mode)
+TM_RAMFUNC void controller_set_mode(controller_mode_options new_mode)
 {
     if (new_mode != state.mode)
     {
         switch (new_mode)
         {
-            case CTRL_HOMING:
-            state.mode = CTRL_HOMING;
+            case CONTROLLER_MODE_HOMING:
+            state.mode = CONTROLLER_MODE_HOMING;
             break;
 
-            case CTRL_TRAJECTORY:
-            state.mode = CTRL_TRAJECTORY;
+            case CONTROLLER_MODE_TRAJECTORY:
+            state.mode = CONTROLLER_MODE_TRAJECTORY;
             break;
 
-            case CTRL_POSITION:
-            state.mode = CTRL_POSITION;
+            case CONTROLLER_MODE_POSITION:
+            state.mode = CONTROLLER_MODE_POSITION;
             break;
 
-            case CTRL_VELOCITY:
-            state.mode = CTRL_VELOCITY;
+            case CONTROLLER_MODE_VELOCITY:
+            state.mode = CONTROLLER_MODE_VELOCITY;
             break;
 
-            case CTRL_CURRENT:
-            state.mode = CTRL_CURRENT;
+            case CONTROLLER_MODE_CURRENT:
+            state.mode = CONTROLLER_MODE_CURRENT;
             break;
 
             default:
@@ -378,80 +401,60 @@ TM_RAMFUNC void controller_set_mode(ControlMode new_mode)
     }
 }
 
-TM_RAMFUNC float controller_get_pos_setpoint_user_frame(void)
+TM_RAMFUNC float controller_get_Iq_estimate_user_frame(void)
 {
-    return (state.pos_setpoint - motor_get_user_offset()) * motor_get_user_direction();
+    return apply_velocity_transform(state.Iq_estimate, frame_motor_to_user_p());
 }
 
-TM_RAMFUNC void controller_set_pos_setpoint_user_frame(float value)
+TM_RAMFUNC float controller_get_pos_setpoint_user_frame(void)
 {
-    // direction is either 1 or -1 so we can multiply instead of divide
-    state.pos_setpoint = value * motor_get_user_direction() + motor_get_user_offset();
+    return apply_transform(state.pos_setpoint, frame_position_sensor_to_user_p());
 }
 
 TM_RAMFUNC float controller_get_vel_setpoint_user_frame(void)
 {
-    return state.vel_setpoint * motor_get_user_direction();
-}
-
-TM_RAMFUNC void controller_set_vel_setpoint_user_frame(float value)
-{
-    // direction is either 1 or -1 so we can multiply instead of divide
-    state.vel_setpoint = value * motor_get_user_direction();
+    return apply_velocity_transform(state.vel_setpoint, frame_position_sensor_to_user_p());
 }
 
 TM_RAMFUNC float controller_get_Iq_estimate(void)
 {
-    return state.Iq_est;
+    return state.Iq_estimate;
 }
-
-TM_RAMFUNC float controller_get_Iq_setpoint(void)
+TM_RAMFUNC float controller_get_Iq_setpoint_user_frame(void)
 {
-    return state.Iq_setpoint;
+    return apply_velocity_transform(state.Iq_setpoint, frame_motor_to_user_p());
 }
 
-TM_RAMFUNC void controller_set_Iq_setpoint(float value)
+TM_RAMFUNC float controller_get_Id_setpoint_user_frame(void)
 {
-    state.Iq_setpoint = value;
+    return apply_velocity_transform(state.Id_setpoint, frame_motor_to_user_p());
 }
 
-TM_RAMFUNC float controller_get_Iq_estimate_user_frame(void)
+TM_RAMFUNC void controller_set_pos_setpoint_user_frame(float value)
 {
-    return state.Iq_est * motor_get_user_direction();
+    state.pos_setpoint = apply_transform(value, frame_user_to_position_sensor_p());
 }
 
-TM_RAMFUNC float controller_get_Iq_setpoint_user_frame(void)
+TM_RAMFUNC void controller_set_vel_setpoint_user_frame(float value)
 {
-    return state.Iq_setpoint * motor_get_user_direction();
+    state.vel_setpoint = apply_velocity_transform(value, frame_user_to_position_sensor_p());
 }
 
 TM_RAMFUNC void controller_set_Iq_setpoint_user_frame(float value)
 {
-    state.Iq_setpoint = value * motor_get_user_direction();
+    state.Iq_setpoint = apply_velocity_transform(value, frame_user_to_motor_p());
 }
 
-TM_RAMFUNC float controller_get_Id_setpoint_user_frame(void)
-{
-    return state.Id_setpoint;
-}
-
-TM_RAMFUNC float controller_get_Vq_setpoint_user_frame(void)
-{
-    return state.Vq_setpoint * motor_get_user_direction();
-}
-
-TM_RAMFUNC float controller_set_pos_vel_setpoints(float pos_setpoint, float vel_setpoint)
+float controller_set_pos_vel_setpoints_user_frame(float pos_setpoint, float vel_setpoint)
 {
     controller_set_pos_setpoint_user_frame(pos_setpoint);
     controller_set_vel_setpoint_user_frame(vel_setpoint);
-    return observer_get_pos_estimate_user_frame();
+    return user_frame_get_pos_estimate();
 }
 
-void controller_get_modulation_values(FloatTriplet *dc)
+TM_RAMFUNC float controller_get_Vq_setpoint_user_frame(void)
 {
-    dc->A = state.modulation_values.A;
-    dc->B = state.modulation_values.B;
-    dc->C = state.modulation_values.C;
+    return apply_velocity_transform(state.Vq_setpoint, frame_motor_to_user_p());
 }
 
 float controller_get_pos_gain(void)
@@ -480,29 +483,29 @@ void controller_set_vel_gain(float gain)
     }
 }
 
-float controller_get_vel_integrator_gain(void)
+float controller_get_vel_integral_gain(void)
 {
-    return config.vel_integrator_gain;
+    return config.vel_integral_gain;
 }
 
-void controller_set_vel_integrator_gain(float gain)
+void controller_set_vel_integral_gain(float gain)
 {
     if (gain >= 0.0f)
     {
-        config.vel_integrator_gain = gain;
+        config.vel_integral_gain = gain;
     }
 }
 
-float controller_get_vel_integrator_deadband(void)
+float controller_get_vel_integral_deadband(void)
 {
-    return config.vel_integrator_deadband;
+    return config.vel_integral_deadband;
 }
 
-void controller_set_vel_integrator_deadband(float value)
+void controller_set_vel_integral_deadband(float value)
 {
     if (value >= 0.0f)
     {
-        config.vel_integrator_deadband = value;
+        config.vel_integral_deadband = value;
     }
 }
 
@@ -616,8 +619,8 @@ void controller_restore_config(ControllerConfig *config_)
     config = *config_;
 }
 
-static inline bool Controller_LimitVelocity(float min_limit, float max_limit, float vel_estimate,
-                                            float vel_gain, float *I)
+static inline bool Controller_LimitVelocity(const float min_limit, const float max_limit, const float vel_estimate,
+                                            const float vel_gain, float *I)
 {
     float Imax = (max_limit - vel_estimate) * vel_gain;
     float Imin = (min_limit - vel_estimate) * vel_gain;
@@ -628,8 +631,8 @@ TM_RAMFUNC void controller_update_I_gains(void)
 {
     config.I_gain = config.I_bw * motor_get_phase_inductance();
     float plant_pole = motor_get_phase_resistance() / motor_get_phase_inductance();
-    config.Iq_integrator_gain = plant_pole * config.I_gain;
-    config.Id_integrator_gain = config.Iq_integrator_gain;
+    config.Iq_integral_gain = plant_pole * config.I_gain;
+    config.Id_integral_gain = config.Iq_integral_gain;
 }
 
 TM_RAMFUNC uint8_t controller_get_warnings(void)
diff --git a/firmware/src/controller/controller.h b/firmware/src/controller/controller.h
index 370349f7..209a8260 100644
--- a/firmware/src/controller/controller.h
+++ b/firmware/src/controller/controller.h
@@ -15,52 +15,35 @@
 //  * You should have received a copy of the GNU General Public License
 //  * along with this program. If not, see <http://www.gnu.org/licenses/>.
 
-#ifndef CONTROLLER_CONTROLLER_H_
-#define CONTROLLER_CONTROLLER_H_
+#pragma once
 
 #include <src/common.h>
+#include <src/tm_enums.h>
 #include <src/controller/trajectory_planner.h>
 #include <src/controller/homing_planner.h>
 
-typedef enum
-{
-    STATE_IDLE = 0,
-    STATE_CALIBRATE = 1,
-    STATE_CL_CONTROL = 2
-} ControlState;
-
-typedef enum
-{
-    CTRL_CURRENT = 0,
-    CTRL_VELOCITY = 1,
-    CTRL_POSITION = 2,
-    CTRL_TRAJECTORY = 3,
-    CTRL_HOMING = 4
-} ControlMode;
-
 typedef struct
 {
-    // TODO: State.state is very confusing, name appropriately
-    ControlState state;
-    ControlMode mode;
+    controller_state_options state;
+    controller_mode_options mode;
     uint8_t warnings;
     uint8_t errors;
     bool is_calibrating;
     FloatTriplet I_phase_meas;
     FloatTriplet modulation_values;
-    float Iq_est;
-    float Id_est;
+    float Iq_estimate;
+    float Id_estimate;
     float Ibus_est;
     float power_est;
-    float pos_setpoint;
-    float vel_setpoint;
+    float pos_setpoint; // expressed in position frame
+    float vel_setpoint; // expressed in position frame
     float vel_ramp_setpoint;
-    float Iq_setpoint;
-    float Id_setpoint;
-    float Vq_setpoint;
-    float vel_integrator_Iq;
-    float Iq_integrator_Vq;
-    float Id_integrator_Vd;
+    float Iq_setpoint; // expressed in commutation frame
+    float Id_setpoint; // expressed in commutation frame
+    float Vq_setpoint; // expressed in commutation frame
+    float vel_integrator;
+    float Iq_integrator;
+    float Id_integrator;
     float t_plan;
 } ControllerState;
 
@@ -71,12 +54,12 @@ typedef struct
     float I_limit;
     float pos_gain;
     float vel_gain;
-    float vel_integrator_gain;
-    float vel_integrator_deadband;
+    float vel_integral_gain;
+    float vel_integral_deadband;
     float I_bw;
     float I_gain;
-    float Iq_integrator_gain;
-    float Id_integrator_gain;
+    float Iq_integral_gain;
+    float Id_integral_gain;
     float I_k;
     float vel_increment;
     float max_Ibus_regen;
@@ -85,46 +68,45 @@ typedef struct
 
 void Controller_ControlLoop(void);
 
-ControlState controller_get_state(void);
-void controller_set_state(ControlState new_state);
+controller_state_options controller_get_state(void);
+void controller_set_state(controller_state_options new_state);
+
+controller_mode_options controller_get_mode(void);
+void controller_set_mode(controller_mode_options mode);
 
-ControlMode controller_get_mode(void);
-void controller_set_mode(ControlMode mode);
+inline void controller_calibrate(void) {controller_set_state(CONTROLLER_STATE_CALIBRATE);}
+inline void controller_idle(void) {controller_set_state(CONTROLLER_STATE_IDLE);}
+inline void controller_position_mode(void) {controller_set_mode(CONTROLLER_MODE_POSITION);controller_set_state(CONTROLLER_STATE_CL_CONTROL);}
+inline void controller_velocity_mode(void) {controller_set_mode(CONTROLLER_MODE_VELOCITY);controller_set_state(CONTROLLER_STATE_CL_CONTROL);}
+inline void controller_current_mode(void) {controller_set_mode(CONTROLLER_MODE_CURRENT);controller_set_state(CONTROLLER_STATE_CL_CONTROL);}
 
-inline void controller_calibrate(void) {controller_set_state(STATE_CALIBRATE);}
-inline void controller_idle(void) {controller_set_state(STATE_IDLE);}
-inline void controller_position_mode(void) {controller_set_mode(CTRL_POSITION);controller_set_state(STATE_CL_CONTROL);}
-inline void controller_velocity_mode(void) {controller_set_mode(CTRL_VELOCITY);controller_set_state(STATE_CL_CONTROL);}
-inline void controller_current_mode(void) {controller_set_mode(CTRL_CURRENT);controller_set_state(STATE_CL_CONTROL);}
+float controller_get_Iq_estimate_user_frame(void);
 
 float controller_get_pos_setpoint_user_frame(void);
-void controller_set_pos_setpoint_user_frame(float value);
 float controller_get_vel_setpoint_user_frame(void);
+float controller_get_Iq_setpoint_user_frame(void);
+float controller_get_Id_setpoint_user_frame(void);
+
+void controller_set_pos_setpoint_user_frame(float value);
 void controller_set_vel_setpoint_user_frame(float value);
+void controller_set_Iq_setpoint_user_frame(float value);
 
-float controller_get_Iq_estimate(void);
-float controller_get_Iq_setpoint(void);
-void controller_set_Iq_setpoint(float value);
+float controller_set_pos_vel_setpoints_user_frame(float pos_setpoint, float vel_setpoint);
 
-float controller_get_Iq_estimate_user_frame(void);
-float controller_get_Iq_setpoint_user_frame(void);
-void controller_set_Iq_setpoint_user_frame(float value);
-float controller_get_Id_setpoint_user_frame(void);
+float controller_get_Iq_estimate(void);
 
 float controller_get_Vq_setpoint_user_frame(void);
 
 float controller_set_pos_vel_setpoints(float pos_setpoint, float vel_setpoint);
 
-void controller_get_modulation_values(FloatTriplet *dc);
-
 float controller_get_pos_gain(void);
 void controller_set_pos_gain(float gain);
 float controller_get_vel_gain(void);
 void controller_set_vel_gain(float gain);
-float controller_get_vel_integrator_gain(void);
-void controller_set_vel_integrator_gain(float gain);
-float controller_get_vel_integrator_deadband(void);
-void controller_set_vel_integrator_deadband(float gain);
+float controller_get_vel_integral_gain(void);
+void controller_set_vel_integral_gain(float gain);
+float controller_get_vel_integral_deadband(void);
+void controller_set_vel_integral_deadband(float gain);
 float controller_get_Iq_gain(void);
 float controller_get_I_bw(void);
 void controller_set_I_bw(float bw);
@@ -154,4 +136,3 @@ uint8_t controller_get_errors(void);
 ControllerConfig *controller_get_config(void);
 void controller_restore_config(ControllerConfig *config_);
 
-#endif /* CONTROLLER_CONTROLLER_H_ */
diff --git a/firmware/src/controller/homing_planner.c b/firmware/src/controller/homing_planner.c
index fc015db5..5285c814 100644
--- a/firmware/src/controller/homing_planner.c
+++ b/firmware/src/controller/homing_planner.c
@@ -4,27 +4,28 @@
 #include <src/observer/observer.h>
 #include <src/controller/controller.h>
 #include <src/utils/utils.h>
+#include <src/xfs.h>
 #include <homing_planner.h>
 #include <src/can/can_endpoints.h>
 
 static HomingPlannerConfig config = {
-    .homing_velocity = -8192.0f, // ticks/s
+    .homing_velocity = SENSOR_COMMON_RES_TICKS_FLOAT, // ticks/s
     .max_homing_t = 20.0f, // s
-    .max_stay_vel = 4000.0f, // ticks/s
-    .max_stay_dpos = 1000.0f, // ticks
+    .max_stay_vel = (SENSOR_COMMON_RES_TICKS_FLOAT/2.0f), // ticks/s
+    .max_stay_dpos = (SENSOR_COMMON_RES_TICKS_FLOAT/8.0f), // ticks
     .max_stay_t = 1.0f, // s
-    .retract_distance = 1000.0f // ticks
+    .retract_distance = (SENSOR_COMMON_RES_TICKS_FLOAT/8.0f) // ticks
 };
 
 static HomingPlannerState state = {0};
 
 bool homing_planner_home(void)
 {
-    if (!errors_exist() && controller_get_mode() != CTRL_HOMING)
+    if (!errors_exist() && controller_get_mode() != CONTROLLER_MODE_HOMING)
     {
         state.stay_t_current = 0;
         state.home_t_current = 0;
-        controller_set_mode(CTRL_HOMING);
+        controller_set_mode(CONTROLLER_MODE_HOMING);
         state.warnings = HOMING_WARNINGS_NONE;
         return true;
     }
@@ -50,14 +51,14 @@ TM_RAMFUNC bool homing_planner_evaluate()
         controller_set_pos_setpoint_user_frame(next_pos_setpoint);
         controller_set_vel_setpoint_user_frame(config.homing_velocity);
 
-        if (fabsf(observer_get_vel_estimate_user_frame()) < config.max_stay_vel && fabsf(current_pos_setpoint - observer_get_pos_estimate_user_frame()) > config.max_stay_dpos)
+        const float observer_pos = user_frame_get_pos_estimate();
+        if (fabsf(user_frame_get_vel_estimate()) < config.max_stay_vel && fabsf(current_pos_setpoint - observer_pos) > config.max_stay_dpos)
         {
             state.stay_t_current += PWM_PERIOD_S;
             if (state.stay_t_current >= config.max_stay_t)
             {
                 // First time the endstop is considered found, reset origins and setpoints
-                motor_set_user_offset(0);
-                motor_set_user_offset(observer_get_pos_estimate_user_frame());
+                frame_user_to_position_sensor_set_offset(observer_pos);
                 controller_set_pos_setpoint_user_frame(0);
                 controller_set_vel_setpoint_user_frame(0);
             }
diff --git a/firmware/src/controller/trajectory_planner.c b/firmware/src/controller/trajectory_planner.c
index 0c7445dc..5ff670d0 100644
--- a/firmware/src/controller/trajectory_planner.c
+++ b/firmware/src/controller/trajectory_planner.c
@@ -23,8 +23,8 @@
 #include <src/controller/trajectory_planner.h>
 
 static TrajPlannerConfig config = {
-	.max_accel = ENCODER_TICKS_FLOAT,
-	.max_decel = ENCODER_TICKS_FLOAT,
+	.max_accel = SENSOR_COMMON_RES_TICKS_FLOAT,
+	.max_decel = SENSOR_COMMON_RES_TICKS_FLOAT,
 	.max_vel = 50000.0f,
 	.deltat_accel = 2.0f,
 	.deltat_decel = 2.0f,
@@ -40,7 +40,7 @@ bool planner_move_to_tlimit(float p_target)
 	if (!errors_exist() && planner_prepare_plan_tlimit(p_target, config.deltat_total, config.deltat_accel, config.deltat_decel, &motion_plan))
 	{
 		controller_set_motion_plan(motion_plan);
-		controller_set_mode(CTRL_TRAJECTORY);
+		controller_set_mode(CONTROLLER_MODE_TRAJECTORY);
 		response = true;
 	}
 	return response;
@@ -53,7 +53,7 @@ bool planner_move_to_vlimit(float p_target)
 	if (!errors_exist() && planner_prepare_plan_vlimit(p_target, config.max_vel, config.max_accel, config.max_decel, &motion_plan))
 	{
 		controller_set_motion_plan(motion_plan);
-		controller_set_mode(CTRL_TRAJECTORY);
+		controller_set_mode(CONTROLLER_MODE_TRAJECTORY);
 		response = true;
 	}
 	return response;
diff --git a/firmware/src/encoder/encoder.c b/firmware/src/encoder/encoder.c
deleted file mode 100644
index 60a8d3af..00000000
--- a/firmware/src/encoder/encoder.c
+++ /dev/null
@@ -1,134 +0,0 @@
-
-//  * This file is part of the Tinymovr-Firmware distribution
-//  * (https://github.com/yconst/tinymovr-firmware).
-//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
-//  *
-//  * This program is free software: you can redistribute it and/or modify
-//  * it under the terms of the GNU General Public License as published by
-//  * the Free Software Foundation, version 3.
-//  *
-//  * This program is distributed in the hope that it will be useful, but
-//  * WITHOUT ANY WARRANTY; without even the implied warranty of
-//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-//  * General Public License for more details.
-//  *
-//  * You should have received a copy of the GNU General Public License
-//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include <src/common.h>
-#include <src/motor/motor.h>
-#include <src/encoder/ma7xx.h>
-#include <src/encoder/hall.h>
-#include <src/encoder/encoder.h>
-
-static EncoderConfig config = {0};
-static EncoderState state = {0};
-
-void encoder_init(void)
-{
-#ifdef BOARD_REV_R5
-    if (ENCODER_MA7XX == config.encoder_type)
-    {
-#endif
-        ma7xx_init();
-        state.current_encoder_type = ENCODER_MA7XX;
-        state.get_error_ptr = &ma7xx_get_errors;
-        state.get_calibrated_ptr = &ma7xx_rec_is_calibrated;
-        state.get_angle_ptr = &ma7xx_get_angle_rectified;
-        state.update_angle_ptr = &ma7xx_update;
-        state.reset_encoder_ptr = &ma7xx_clear_rec_table;
-        state.ticks = ENCODER_TICKS;
-#ifdef BOARD_REV_R5
-    }
-    else if (ENCODER_HALL == config.encoder_type)
-    {
-        hall_init();
-        state.current_encoder_type = ENCODER_HALL;
-        state.get_error_ptr = &hall_get_errors;
-        state.get_calibrated_ptr = &hall_sector_map_is_calibrated;
-        state.get_angle_ptr = &hall_get_angle;
-        state.update_angle_ptr = &hall_update;
-        state.reset_encoder_ptr = &hall_clear_sector_map;
-        state.ticks = HALL_SECTORS;
-    }
-#endif
-}
-
-void encoder_reset(void)
-{
-    state.reset_encoder_ptr();
-}
-
-TM_RAMFUNC int16_t encoder_get_angle(void)
-{
-    return state.get_angle_ptr();
-}
-
-TM_RAMFUNC void encoder_update(bool check_error)
-{
-    if (state.update_angle_ptr)
-    {
-        state.update_angle_ptr(check_error);
-    }
-}
-
-TM_RAMFUNC uint16_t encoder_get_ticks(void)
-{
-    return state.ticks;
-}
-
-TM_RAMFUNC float encoder_ticks_to_eangle()
-{
-#ifdef BOARD_REV_R5
-    // We need to derive this during call, because the motor pole pairs
-    // may change after calibration, or after user input
-    if (ENCODER_MA7XX == state.current_encoder_type)
-    {
-#endif
-        return twopi_by_enc_ticks * motor_get_pole_pairs();
-#ifdef BOARD_REV_R5
-    }
-    return  twopi_by_hall_sectors;
-#endif
-}
-
-EncoderType encoder_get_type(void)
-{
-    return state.current_encoder_type;
-}
-
-TM_RAMFUNC bool encoder_get_calibrated(void)
-{
-    return state.get_calibrated_ptr();
-}
-
-TM_RAMFUNC void encoder_set_type(EncoderType enc_type)
-{
-#ifdef BOARD_REV_R5
-    if (ENCODER_MA7XX == enc_type)
-    {
-#endif
-        config.encoder_type = ENCODER_MA7XX;
-#ifdef BOARD_REV_R5
-    }
-    else if (ENCODER_HALL == enc_type)
-    {
-        config.encoder_type = ENCODER_HALL;
-    }
-#endif
-}
-
-TM_RAMFUNC uint8_t encoder_get_errors(void)
-{
-    return state.get_error_ptr();
-}
-
-EncoderConfig* encoder_get_config(void)
-{
-    return &config;
-}
-
-void encoder_restore_config(EncoderConfig* config_)
-{
-    config = *config_;
-}
diff --git a/firmware/src/encoder/encoder.h b/firmware/src/encoder/encoder.h
deleted file mode 100644
index c4bb746a..00000000
--- a/firmware/src/encoder/encoder.h
+++ /dev/null
@@ -1,67 +0,0 @@
-
-//  * This file is part of the Tinymovr-Firmware distribution
-//  * (https://github.com/yconst/tinymovr-firmware).
-//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
-//  * 
-//  * This program is free software: you can redistribute it and/or modify  
-//  * it under the terms of the GNU General Public License as published by  
-//  * the Free Software Foundation, version 3.
-//  *
-//  * This program is distributed in the hope that it will be useful, but 
-//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
-//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
-//  * General Public License for more details.
-//  *
-//  * You should have received a copy of the GNU General Public License 
-//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#pragma once
-
-#include <src/common.h>
-
-// TODO: Make external in a separate unit
-typedef bool (*bool_getter)(void);
-typedef void (*bool_setter)(bool);
-typedef uint8_t (*uint8_getter)(void);
-typedef int16_t (*int16_getter)(void);
-typedef void (*void_func)(void);
-
-typedef enum {
-	ENCODER_MA7XX = 0,
-	ENCODER_HALL = 1
-} EncoderType;
-
-typedef struct {
-    EncoderType current_encoder_type;
-    uint8_getter get_error_ptr;
-    bool_getter get_calibrated_ptr;
-    bool_setter update_angle_ptr;
-    int16_getter get_angle_ptr;
-    void_func reset_encoder_ptr;
-    uint16_t ticks;
-} EncoderState;
-
-typedef struct
-{
-    EncoderType encoder_type;
-} EncoderConfig;
-
-void encoder_init(void);
-void encoder_reset(void);
-
-int16_t encoder_get_angle(void);
-void encoder_update(bool check_error);
-
-uint16_t encoder_get_ticks(void);
-float encoder_ticks_to_eangle(void);
-
-EncoderType encoder_get_type(void);
-void encoder_set_type(EncoderType enc_type);
-
-bool encoder_get_calibrated(void);
-
-uint8_t encoder_get_errors(void);
-
-EncoderConfig* encoder_get_config(void);
-void encoder_restore_config(EncoderConfig* config_);
-
diff --git a/firmware/src/encoder/hall.c b/firmware/src/encoder/hall.c
deleted file mode 100644
index 5ce0c1c7..00000000
--- a/firmware/src/encoder/hall.c
+++ /dev/null
@@ -1,91 +0,0 @@
-
-//  * This file is part of the Tinymovr-Firmware distribution
-//  * (https://github.com/yconst/tinymovr-firmware).
-//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
-//  *
-//  * This program is free software: you can redistribute it and/or modify
-//  * it under the terms of the GNU General Public License as published by
-//  * the Free Software Foundation, version 3.
-//  *
-//  * This program is distributed in the hope that it will be useful, but
-//  * WITHOUT ANY WARRANTY; without even the implied warranty of
-//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-//  * General Public License for more details.
-//  *
-//  * You should have received a copy of the GNU General Public License
-//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include <string.h>
-#include <src/system/system.h>
-#include <src/encoder/hall.h>
-
-#define AIO6789_IO_MODE                 0x00
-#define AIO_INPUT                       0x00
-
-static HallConfig config = {0};
-static HallState state = {0};
-
-void hall_init(void)
-{
-    pac5xxx_tile_register_write(ADDR_CFGAIO7, AIO6789_IO_MODE | AIO_INPUT);
-    pac5xxx_tile_register_write(ADDR_CFGAIO8, AIO6789_IO_MODE | AIO_INPUT);
-    pac5xxx_tile_register_write(ADDR_CFGAIO9, AIO6789_IO_MODE | AIO_INPUT);
-}
-
-TM_RAMFUNC uint8_t hall_get_errors(void)
-{
-    return state.errors;
-}
-
-TM_RAMFUNC uint8_t *hall_get_error_ptr(void)
-{
-	return &(state.errors);
-}
-
-TM_RAMFUNC int16_t hall_get_angle(void)
-{
-    return state.angle;
-}
-
-TM_RAMFUNC void hall_update(bool check_error)
-{
-    const uint8_t sector = (pac5xxx_tile_register_read(ADDR_DINSIG1) >> 1) & 0x07;
-    state.sector = sector;
-    state.angle = config.sector_map[state.sector];
-}
-
-TM_RAMFUNC uint8_t hall_get_sector(void)
-{
-    return state.sector;
-}
-
-void hall_clear_sector_map(void)
-{
-    (void)memset(config.sector_map, 0, sizeof(config.sector_map));
-	config.sector_map_calibrated = false;
-}
-
-void hall_set_sector_map_calibrated(void)
-{
-    config.sector_map_calibrated = true;
-}
-
-bool hall_sector_map_is_calibrated(void)
-{
-    return config.sector_map_calibrated;
-}
-
-uint8_t *hall_get_sector_map_ptr(void)
-{
-    return config.sector_map;
-}
-
-HallConfig* hall_get_config(void)
-{
-    return &config;
-}
-
-void hall_restore_config(HallConfig* config_)
-{
-    config = *config_;
-}
\ No newline at end of file
diff --git a/firmware/src/encoder/hall.h b/firmware/src/encoder/hall.h
deleted file mode 100644
index bea71aac..00000000
--- a/firmware/src/encoder/hall.h
+++ /dev/null
@@ -1,50 +0,0 @@
-
-//  * This file is part of the Tinymovr-Firmware distribution
-//  * (https://github.com/yconst/tinymovr-firmware).
-//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
-//  * 
-//  * This program is free software: you can redistribute it and/or modify  
-//  * it under the terms of the GNU General Public License as published by  
-//  * the Free Software Foundation, version 3.
-//  *
-//  * This program is distributed in the hope that it will be useful, but 
-//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
-//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
-//  * General Public License for more details.
-//  *
-//  * You should have received a copy of the GNU General Public License 
-//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#pragma once
-
-#include <src/common.h>
-
-typedef struct
-{
-	uint8_t sector_map[8];
-    bool sector_map_calibrated;
-} HallConfig;
-
-typedef struct
-{
-    uint8_t errors;
-	uint16_t angle;
-    uint8_t sector;
-} HallState;
-
-void hall_init(void);
-
-uint8_t hall_get_errors(void);
-uint8_t *hall_get_error_ptr(void);
-
-int16_t hall_get_angle(void);
-void hall_update(bool check_error);
-uint8_t hall_get_sector(void);
-
-void hall_clear_sector_map(void);
-void hall_set_sector_map_calibrated(void);
-bool hall_sector_map_is_calibrated(void);
-uint8_t *hall_get_sector_map_ptr(void);
-
-HallConfig* hall_get_config(void);
-void hall_restore_config(HallConfig* config_);
\ No newline at end of file
diff --git a/firmware/src/encoder/ma7xx.c b/firmware/src/encoder/ma7xx.c
deleted file mode 100644
index e40a402a..00000000
--- a/firmware/src/encoder/ma7xx.c
+++ /dev/null
@@ -1,150 +0,0 @@
-
-//  * This file is part of the Tinymovr-Firmware distribution
-//  * (https://github.com/yconst/tinymovr-firmware).
-//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
-//  * 
-//  * This program is free software: you can redistribute it and/or modify  
-//  * it under the terms of the GNU General Public License as published by  
-//  * the Free Software Foundation, version 3.
-//  *
-//  * This program is distributed in the hope that it will be useful, but 
-//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
-//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
-//  * General Public License for more details.
-//  *
-//  * You should have received a copy of the GNU General Public License 
-//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include <string.h>
-#include <src/system/system.h>
-#include <src/ssp/ssp_func.h>
-#include <src/utils/utils.h>
-#include <src/can/can_endpoints.h>
-#include <src/encoder/ma7xx.h>
-
-static MA7xxConfig config = {0};
-static MA7xxState state = {0};
-
-
-void ma7xx_init(void)
-{
-    ssp_init(PRIMARY_ENCODER_SSP_PORT, SSP_MS_MASTER, 0, 0); // Mode 0
-    delay_us(16000); // ensure 16ms sensor startup time as per the datasheet
-    ma7xx_send_angle_cmd();
-    ma7xx_update(false);
-}
-
-TM_RAMFUNC uint8_t ma7xx_get_errors(void)
-{
-    return state.errors;
-}
-
-TM_RAMFUNC void ma7xx_send_angle_cmd(void)
-{
-	ssp_write_one(PRIMARY_ENCODER_SSP_STRUCT, MA_CMD_ANGLE);
-}
-
-TM_RAMFUNC int16_t ma7xx_get_angle_raw(void)
-{
-    return state.angle;
-}
-
-TM_RAMFUNC int16_t ma7xx_get_angle_rectified(void)
-{
-    const uint8_t offset_bits = (ENCODER_BITS - ECN_BITS);
-    const int16_t angle = state.angle;
-    const int16_t off_1 = config.rec_table[angle>>offset_bits];
-	const int16_t off_2 = config.rec_table[((angle>>offset_bits) + 1) % ECN_SIZE];
-	const int16_t off_interp = off_1 + ((off_2 - off_1)* (angle - ((angle>>offset_bits)<<offset_bits))>>offset_bits);
-	return angle + off_interp;
-}
-
-TM_RAMFUNC void ma7xx_update(bool check_error)
-{
-    const int16_t angle = ssp_read_one(PRIMARY_ENCODER_SSP_STRUCT) >> 3;
-
-    if (check_error)
-    {
-    	const int16_t delta = state.angle - angle;
-		if ( ((delta > MAX_ALLOWED_DELTA) || (delta < -MAX_ALLOWED_DELTA)) &&
-		     ((delta > MAX_ALLOWED_DELTA_ADD) || (delta < MIN_ALLOWED_DELTA_ADD)) &&
-		     ((delta > MAX_ALLOWED_DELTA_SUB) || (delta < MIN_ALLOWED_DELTA_SUB)) )
-		{
-            state.errors |= ENCODER_ERRORS_READING_UNSTABLE;
-		}
-    }
-    state.angle = angle;
-}
-
-void ma7xx_clear_rec_table(void)
-{
-    (void)memset(config.rec_table, 0, sizeof(config.rec_table));
-	config.rec_calibrated = false;
-}
-
-void ma7xx_set_rec_calibrated(void)
-{
-    config.rec_calibrated = true;
-}
-
-bool ma7xx_rec_is_calibrated(void)
-{
-    return config.rec_calibrated;
-}
-
-int16_t *ma7xx_get_rec_table_ptr(void)
-{
-    return config.rec_table;
-}
-
-MA7xxConfig* ma7xx_get_config(void)
-{
-    return &config;
-}
-
-void ma7xx_restore_config(MA7xxConfig* config_)
-{
-    config = *config_;
-}
-
-/**
- * @brief Write to a register of the encoder
- * 
- * @param reg The 5-bit register address
- * @param value The value to write
- * @return uint16_t 
- */
-uint16_t ma7xx_write_reg(uint8_t reg, uint8_t value)
-{
-    uint16_t cmd = MA_CMD_WRITE | reg << 8 | value;
-    uint32_t result = ssp_write_one(PRIMARY_ENCODER_SSP_STRUCT, cmd);
-
-    // Delay at least 20ms to let the encoder write to memory
-    delay_us(20100);
-    result |= ssp_write_one(PRIMARY_ENCODER_SSP_STRUCT, 0);
-
-    // The encoder returns the value written to memory, so check that it is the same as what we wrote
-    uint8_t retval =  ssp_read_one(PRIMARY_ENCODER_SSP_STRUCT) >> 8;
-    if ((retval != value) || (result != 0))
-    {
-        return false;
-    }
-    return true;
-}
-
-/**
- * @brief Read from a register of the encoder
- * 
- * @param register The 5-bit register address
- * @return uint8_t 
- */
-uint8_t ma7xx_read_reg(uint8_t reg)
-{
-    uint16_t cmd[2] = {MA_CMD_READ  | (reg << 8), 0};
-    uint16_t result = ssp_write_multi(PRIMARY_ENCODER_SSP_STRUCT, cmd, 2u);
-    if (result != 0)
-    {
-        return false;
-    }
-    return ssp_read_one(PRIMARY_ENCODER_SSP_STRUCT) >> 8;
-}
\ No newline at end of file
diff --git a/firmware/src/encoder/ma7xx.h b/firmware/src/encoder/ma7xx.h
deleted file mode 100644
index 39d0483d..00000000
--- a/firmware/src/encoder/ma7xx.h
+++ /dev/null
@@ -1,73 +0,0 @@
-
-//  * This file is part of the Tinymovr-Firmware distribution
-//  * (https://github.com/yconst/tinymovr-firmware).
-//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
-//  * 
-//  * This program is free software: you can redistribute it and/or modify  
-//  * it under the terms of the GNU General Public License as published by  
-//  * the Free Software Foundation, version 3.
-//  *
-//  * This program is distributed in the hope that it will be useful, but 
-//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
-//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
-//  * General Public License for more details.
-//  *
-//  * You should have received a copy of the GNU General Public License 
-//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#pragma once
-
-#include <src/common.h>
-
-#if defined BOARD_REV_R3
-#define PRIMARY_ENCODER_SSP_PORT SSPD
-#define PRIMARY_ENCODER_SSP_STRUCT PAC55XX_SSPD
-#elif defined BOARD_REV_R5 || defined BOARD_REV_M5
-#define PRIMARY_ENCODER_SSP_PORT SSPC
-#define PRIMARY_ENCODER_SSP_STRUCT PAC55XX_SSPC
-#endif
-
-#define MAX_ALLOWED_DELTA     (ENCODER_TICKS / 6)
-#define MAX_ALLOWED_DELTA_ADD (MAX_ALLOWED_DELTA + ENCODER_TICKS)
-#define MAX_ALLOWED_DELTA_SUB (MAX_ALLOWED_DELTA - ENCODER_TICKS)
-#define MIN_ALLOWED_DELTA_ADD (-MAX_ALLOWED_DELTA + ENCODER_TICKS)
-#define MIN_ALLOWED_DELTA_SUB (-MAX_ALLOWED_DELTA - ENCODER_TICKS)
-
-typedef struct
-{
-	bool rec_calibrated;
-	int16_t rec_table[ECN_SIZE];
-} MA7xxConfig;
-
-typedef struct
-{
-    uint8_t errors;
-	int16_t angle;
-    uint8_t error;
-} MA7xxState;
-
-// MA702 commands
-typedef enum {
-    MA_CMD_NOP              = 0x0000,
-    MA_CMD_ANGLE            = 0x0000,
-    MA_CMD_WRITE            = 0x8000,
-    MA_CMD_READ             = 0x4000
-} MA702Command;
-
-void ma7xx_init(void);
-
-uint8_t ma7xx_get_errors(void);
-void ma7xx_send_angle_cmd(void);
-int16_t ma7xx_get_angle_raw(void);
-int16_t ma7xx_get_angle_rectified(void);
-void ma7xx_update(bool check_error);
-
-void ma7xx_clear_rec_table(void);
-void ma7xx_set_rec_calibrated(void);
-bool ma7xx_rec_is_calibrated(void);
-int16_t *ma7xx_get_rec_table_ptr(void);
-MA7xxConfig* ma7xx_get_config(void);
-void ma7xx_restore_config(MA7xxConfig* config_);
-uint16_t ma7xx_write_reg(uint8_t, uint8_t);
-uint16_t ma7xx_write_data(uint16_t);
-uint8_t ma7xx_read_reg(uint8_t);
\ No newline at end of file
diff --git a/firmware/src/gatedriver/gatedriver.c b/firmware/src/gatedriver/gatedriver.c
index 0572390c..3ca8b329 100644
--- a/firmware/src/gatedriver/gatedriver.c
+++ b/firmware/src/gatedriver/gatedriver.c
@@ -15,10 +15,11 @@
 //  * You should have received a copy of the GNU General Public License 
 //  * along with this program. If not, see <http://www.gnu.org/licenses/>.
 
-#include <src/common.h>
 #include <src/motor/motor.h>
 #include <src/gatedriver/gatedriver.h>
 
+GateDriverState gate_driver_state = {0};
+
 TM_RAMFUNC void gate_driver_enable(void)
 {
     // Select PWMA peripheral for Port B
@@ -40,6 +41,8 @@ TM_RAMFUNC void gate_driver_enable(void)
 
     pac5xxx_tile_register_write(ADDR_CFGDRV4,
             pac5xxx_tile_register_read(ADDR_CFGDRV4) | 0x1); // BBM is bit 0
+
+    gate_driver_state.enabled = ((pac5xxx_tile_register_read(ADDR_ENDRV) & 0x1) == 1);
 }
 
 TM_RAMFUNC void gate_driver_disable(void)
@@ -58,24 +61,7 @@ TM_RAMFUNC void gate_driver_disable(void)
 
     // Turn on output enables
     PAC55XX_GPIOB->OUTMASK.w = 0x00;
-}
 
-TM_RAMFUNC bool gate_driver_is_enabled(void)
-{
-    return ((pac5xxx_tile_register_read(ADDR_ENDRV) & 0x1) == 1);
+    gate_driver_state.enabled = ((pac5xxx_tile_register_read(ADDR_ENDRV) & 0x1) == 1);
 }
 
-TM_RAMFUNC void gate_driver_set_duty_cycle(const FloatTriplet *dutycycles)
-{
-	m1_u_set_duty(dutycycles->A);
-	if (motor_phases_swapped())
-	{
-		m1_v_set_duty(dutycycles->C);
-		m1_w_set_duty(dutycycles->B);
-	}
-	else
-	{
-		m1_v_set_duty(dutycycles->B);
-		m1_w_set_duty(dutycycles->C);
-	}
-}
diff --git a/firmware/src/gatedriver/gatedriver.h b/firmware/src/gatedriver/gatedriver.h
index ae09bcfc..3bfd4d37 100644
--- a/firmware/src/gatedriver/gatedriver.h
+++ b/firmware/src/gatedriver/gatedriver.h
@@ -15,33 +15,45 @@
 //  * You should have received a copy of the GNU General Public License 
 //  * along with this program. If not, see <http://www.gnu.org/licenses/>.
 
-#ifndef GATEDRIVER_GATEDRIVER_H_
-#define GATEDRIVER_GATEDRIVER_H_
+#pragma once
 
+#include <src/common.h>
 
 void gate_driver_enable(void);
 void gate_driver_disable(void);
-bool gate_driver_is_enabled(void);
-void gate_driver_set_duty_cycle(const FloatTriplet *dc);
 
+typedef struct {
+    bool enabled;
+} GateDriverState;
+
+extern GateDriverState gate_driver_state;
 
-//=============================================
-// Motor Driver Duty Cycle Macro Functions
-//=============================================
 static inline void m1_u_set_duty(const float duty)
 {
     uint16_t val = ((uint16_t)(duty * (TIMER_FREQ_HZ/PWM_FREQ_HZ) )) >>1;
     PAC55XX_TIMERA->CCTR4.CTR = val;
 }
+
 static inline void m1_v_set_duty(const float duty)
 {
     uint16_t val = ((uint16_t)(duty * (TIMER_FREQ_HZ/PWM_FREQ_HZ) )) >>1;
     PAC55XX_TIMERA->CCTR5.CTR = val;
 }
+
 static inline void m1_w_set_duty(const float duty)
 {
     uint16_t val = ((uint16_t)(duty * (TIMER_FREQ_HZ/PWM_FREQ_HZ) )) >>1;
     PAC55XX_TIMERA->CCTR6.CTR = val;
 }
 
-#endif /* GATEDRIVER_GATEDRIVER_H_ */
+static inline void gate_driver_set_duty_cycle(const FloatTriplet *dutycycles)
+{
+	m1_u_set_duty(dutycycles->A);
+	m1_v_set_duty(dutycycles->B);
+	m1_w_set_duty(dutycycles->C);
+}
+
+static inline bool gate_driver_is_enabled(void)
+{
+    return gate_driver_state.enabled;
+}
diff --git a/firmware/src/main.c b/firmware/src/main.c
index 0b861e6e..46627e91 100644
--- a/firmware/src/main.c
+++ b/firmware/src/main.c
@@ -15,11 +15,11 @@
 //  * You should have received a copy of the GNU General Public License 
 //  * along with this program. If not, see <http://www.gnu.org/licenses/>.
 
-#include <src/encoder/encoder.h>
+
 #include "src/common.h"
 #include "src/system/system.h"
-
 #include "src/uart/uart_lowlevel.h"
+#include <src/sensor/sensor.h>
 #include "src/observer/observer.h"
 #include "src/adc/adc.h"
 #include "src/motor/motor.h"
@@ -34,13 +34,15 @@ int main(void)
 {
   	__disable_irq();
     system_init();
-    nvm_load_config(); // This will TRY to deserialize and import config
-    encoder_init();
-    UART_Init();
-	observer_init();
-	ADC_init();
+    UART_Init(); // Keep UART init before config load for now
+    if (!nvm_load_config())
+    {
+        sensors_init_with_defaults();
+        observers_init_with_defaults();
+    }
+    ADC_init();
     CAN_init();
-    Timer_Init();
+    timers_init();
     Watchdog_init();
     __enable_irq();
 
diff --git a/firmware/src/motor/calibration.c b/firmware/src/motor/calibration.c
deleted file mode 100644
index bade3463..00000000
--- a/firmware/src/motor/calibration.c
+++ /dev/null
@@ -1,335 +0,0 @@
-
-//  * This file is part of the Tinymovr-Firmware distribution
-//  * (https://github.com/yconst/tinymovr-firmware).
-//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
-//  *
-//  * This program is free software: you can redistribute it and/or modify
-//  * it under the terms of the GNU General Public License as published by
-//  * the Free Software Foundation, version 3.
-//  *
-//  * This program is distributed in the hope that it will be useful, but
-//  * WITHOUT ANY WARRANTY; without even the implied warranty of
-//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-//  * General Public License for more details.
-//  *
-//  * You should have received a copy of the GNU General Public License
-//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#include <src/adc/adc.h>
-#include <src/observer/observer.h>
-#include <src/gatedriver/gatedriver.h>
-#include <src/scheduler/scheduler.h>
-#include <src/utils/utils.h>
-#include <src/encoder/ma7xx.h>
-#include <src/encoder/hall.h>
-#include <src/controller/controller.h>
-#include <src/can/can_endpoints.h>
-#include <src/system/system.h>
-#include <src/motor/calibration.h>
-
-static inline void set_epos_and_wait(float angle, float I_setpoint)
-{
-    FloatTriplet modulation_values = {0.0f};
-    float pwm_setpoint = (I_setpoint * motor_get_phase_resistance()) / system_get_Vbus();
-    our_clampc(&pwm_setpoint, -PWM_LIMIT, PWM_LIMIT);
-    SVM(pwm_setpoint * fast_cos(angle), pwm_setpoint * fast_sin(angle),
-        &modulation_values.A, &modulation_values.B, &modulation_values.C);
-    gate_driver_set_duty_cycle(&modulation_values);
-    WaitForControlLoopInterrupt();
-}
-
-static inline void wait_pwm_cycles(uint32_t cycles)
-{
-    for (uint32_t i = 0; i < cycles; i++)
-    {
-        WaitForControlLoopInterrupt();
-    }
-}
-
-bool CalibrateADCOffset(void)
-{
-    // We only need to wait here, the ADC loop will
-    // perform the offset calibration automatically
-    wait_pwm_cycles(10000);
-    return true;
-}
-
-bool CalibrateResistance(void)
-{
-    if (!motor_get_is_gimbal())
-    {
-        float I_cal = motor_get_I_cal();
-        float V_setpoint = 0.0f;
-        FloatTriplet I_phase_meas = {0.0f};
-        FloatTriplet modulation_values = {0.0f};
-        for (uint32_t i = 0; i < CAL_R_LEN; i++)
-        {
-            ADC_GetPhaseCurrents(&I_phase_meas);
-            V_setpoint += CAL_V_GAIN * (I_cal - I_phase_meas.A);
-            const float pwm_setpoint = V_setpoint / system_get_Vbus();
-            SVM(pwm_setpoint, 0.0f, &modulation_values.A, &modulation_values.B, &modulation_values.C);
-            gate_driver_set_duty_cycle(&modulation_values);
-            WaitForControlLoopInterrupt();
-        }
-        const float R = our_fabsf(V_setpoint / I_cal);
-        gate_driver_set_duty_cycle(&three_phase_zero);
-        if ((R <= MIN_PHASE_RESISTANCE) || (R >= MAX_PHASE_RESISTANCE))
-        {
-            uint8_t *error_ptr = motor_get_error_ptr();
-            *error_ptr |= MOTOR_ERRORS_PHASE_RESISTANCE_OUT_OF_RANGE;
-            return false;
-        }
-        else
-        {
-            motor_set_phase_resistance(R);
-        }
-    }
-    return true;
-}
-
-bool CalibrateInductance(void)
-{
-    if (!motor_get_is_gimbal())
-    {
-        float V_setpoint = 0.0f;
-        float I_low = 0.0f;
-        float I_high = 0.0f;
-        FloatTriplet I_phase_meas = {0.0f};
-        FloatTriplet modulation_values = {0.0f};
-
-        for (uint32_t i = 0; i < CAL_L_LEN; i++)
-        {
-            ADC_GetPhaseCurrents(&I_phase_meas);
-            if ((i & 0x2u) == 0x2u)
-            {
-                I_high += I_phase_meas.A;
-                V_setpoint = -CAL_V_INDUCTANCE;
-            }
-            else
-            {
-                I_low += I_phase_meas.A;
-                V_setpoint = CAL_V_INDUCTANCE;
-            }
-            const float pwm_setpoint = V_setpoint / system_get_Vbus();
-            SVM(pwm_setpoint, 0.0f, &modulation_values.A, &modulation_values.B, &modulation_values.C);
-            gate_driver_set_duty_cycle(&modulation_values);
-            WaitForControlLoopInterrupt();
-        }
-        const float num_cycles = CAL_L_LEN / 2;
-        const float dI_by_dt = (I_high - I_low) / (PWM_PERIOD_S * num_cycles);
-        const float L = CAL_V_INDUCTANCE / dI_by_dt;
-        gate_driver_set_duty_cycle(&three_phase_zero);
-        if ((L <= MIN_PHASE_INDUCTANCE) || (L >= MAX_PHASE_INDUCTANCE))
-        {
-            uint8_t *error_ptr = motor_get_error_ptr();
-            *error_ptr |= MOTOR_ERRORS_PHASE_INDUCTANCE_OUT_OF_RANGE;
-            return false;
-        }
-        else
-        {
-            motor_set_phase_inductance(L);
-            controller_update_I_gains();
-        }
-    }
-    return true;
-}
-
-bool CalibrateDirectionAndPolePairs(void)
-{
-    // Note that, in order to generate the error table,
-    // we read the unwrapped positions given by the
-    // observer, but we generate and use the error table
-    // before the observer, at the encoder read.
-    const float epos_target = CAL_PHASE_TURNS * TWOPI;
-    const float I_setpoint = motor_get_I_cal();
-    bool success = true;
-    // Stay a bit at starting epos
-    for (uint32_t i = 0; i < CAL_STAY_LEN; i++)
-    {
-        set_epos_and_wait(0, I_setpoint);
-    }
-    const float epos_start = observer_get_pos_estimate();
-    float epos_end = 0;
-    // Move to target epos
-    for (uint32_t i = 0; i < CAL_DIR_LEN; i++)
-    {
-        set_epos_and_wait(epos_target * ((float)i / CAL_DIR_LEN), I_setpoint);
-    }
-    // Stay a bit at target epos
-    for (uint32_t i = 0; i < CAL_STAY_LEN; i++)
-    {
-        set_epos_and_wait(epos_target, I_setpoint);
-    }
-    // Find pole pairs
-    if (!motor_find_pole_pairs(ENCODER_TICKS, epos_start, observer_get_pos_estimate(), epos_target))
-    {
-        uint8_t *error_ptr = motor_get_error_ptr();
-        *error_ptr |= MOTOR_ERRORS_INVALID_POLE_PAIRS;
-        return false;
-    }
-    else
-    {
-        epos_end = observer_get_pos_estimate();
-    }
-    // Move back to start epos
-    for (uint32_t i = 0; i < CAL_DIR_LEN; i++)
-    {
-        set_epos_and_wait(epos_target * (1.0f - ((float)i / CAL_DIR_LEN)), I_setpoint);
-    }
-    gate_driver_set_duty_cycle(&three_phase_zero);
-    if (success && epos_start > epos_end)
-    {
-        motor_set_phases_swapped(true);
-    }
-    return success;
-}
-
-bool calibrate_hall_sequence(void)
-{
-    hall_clear_sector_map();
-    uint8_t *sector_map = hall_get_sector_map_ptr();
-    const float I_setpoint = motor_get_I_cal();
-    bool success = true;
-
-    // Stay a bit at starting epos
-	for (uint32_t i=0; i<CAL_STAY_LEN; i++)
-	{
-		set_epos_and_wait(0, I_setpoint);
-	}
-    const uint8_t init_sector = hall_get_sector();
-    uint8_t current_sector = init_sector;
-    uint8_t last_sector = init_sector;
-    uint8_t sector_pos = 0;
-    const float increment = TWOPI / CAL_DIR_LEN;
-    float angle = 0;
-
-    sector_map[current_sector] = sector_pos;
-
-    // Move to the 2nd sector
-    while ((current_sector == init_sector) && (angle < TWOPI / HALL_SECTORS))
-    {
-        set_epos_and_wait(angle, I_setpoint);
-        angle += increment;
-        current_sector = hall_get_sector();
-    }
-
-    // Save the rest of the sectors
-    while ((current_sector != init_sector) && (angle < TWOPI))
-    {
-        if (current_sector != last_sector)
-        {
-            last_sector = current_sector;
-            sector_pos++;
-            if (sector_pos >= HALL_SECTORS)
-            {
-                success = false;
-                break;
-            }
-            sector_map[current_sector] = sector_pos;
-        }
-        set_epos_and_wait(angle, I_setpoint);
-        angle += increment;
-        current_sector = hall_get_sector();
-    }
-
-    gate_driver_set_duty_cycle(&three_phase_zero);
-
-    // Check that the number of sectors discovered is the same as expected
-    if (sector_pos != HALL_SECTORS - 1)
-    {
-        success = false;
-    }
-
-    if (success)
-    {
-        hall_set_sector_map_calibrated();
-    }
-    else
-    {
-        uint8_t *error_ptr = hall_get_error_ptr();
-        *error_ptr |= ENCODER_ERRORS_CALIBRATION_FAILED;
-    }
-    return success;
-}
-
-bool calibrate_offset_and_rectification(void)
-{
-    // Size below is an arbitrary large number ie > ECN_SIZE * npp
-    float error_ticks[ECN_SIZE * 20];
-    const int16_t npp = motor_get_pole_pairs();
-    const int16_t n = ECN_SIZE * npp;
-    const int16_t nconv = 100;
-    const float delta = 2 * PI * npp / (n * nconv);
-    const float e_pos_to_ticks = ((float)ENCODER_TICKS) / (2 * PI * npp);
-    float e_pos_ref = 0.f;
-    const float I_setpoint = motor_get_I_cal();
-    int16_t *lut = ma7xx_get_rec_table_ptr();
-    set_epos_and_wait(e_pos_ref, I_setpoint);
-    wait_pwm_cycles(5000);
-    const uint16_t offset_idx = ma7xx_get_angle_raw() >> (ENCODER_BITS - ECN_BITS);
-
-    for (uint32_t i = 0; i < n; i++)
-    {
-        for (uint8_t j = 0; j < nconv; j++)
-        {
-            e_pos_ref += delta;
-            set_epos_and_wait(e_pos_ref, I_setpoint);
-        }
-        WaitForControlLoopInterrupt();
-        const float pos_meas = observer_get_pos_estimate();
-        error_ticks[i] = (int16_t)(e_pos_ref * e_pos_to_ticks - pos_meas);
-    }
-    for (uint32_t i = 0; i < n; i++)
-    {
-        for (uint8_t j = 0; j < nconv; j++)
-        {
-            e_pos_ref -= delta;
-            set_epos_and_wait(e_pos_ref, I_setpoint);
-        }
-        WaitForControlLoopInterrupt();
-        const float pos_meas = observer_get_pos_estimate();
-        error_ticks[n - i - 1] += (int16_t)(e_pos_ref * e_pos_to_ticks - pos_meas);
-    }
-    gate_driver_set_duty_cycle(&three_phase_zero);
-    gate_driver_disable();
-
-    // FIR filtering and map measurements to lut
-    for (int16_t i=0; i<ECN_SIZE; i++)
-    {
-        float acc = 0;
-        for (int16_t j = 0; j < ECN_SIZE; j++)
-        {
-            int16_t read_idx = -ECN_SIZE / 2 + j + i * npp;
-            if (read_idx < 0)
-            {
-                read_idx += n;
-            }
-            else if (read_idx > n - 1)
-            {
-                read_idx -= n;
-            }
-            acc += error_ticks[read_idx];
-        }
-        acc = acc / ((float)(ECN_SIZE * 2));
-        int16_t write_idx = i + offset_idx;
-        if (write_idx > (ECN_SIZE - 1))
-        {
-            write_idx -= ECN_SIZE;
-        }
-        lut[write_idx] = (int16_t)acc;
-    }
-    wait_pwm_cycles(5000);
-    ma7xx_set_rec_calibrated();
-    return true;
-}
-
-void reset_calibration(void)
-{
-    ADC_reset();
-    encoder_reset();
-    observer_reset();
-    motor_reset_calibration();
-    wait_pwm_cycles(5000);
-}
-
-
diff --git a/firmware/src/motor/calibration.h b/firmware/src/motor/calibration.h
deleted file mode 100644
index 7b442b4d..00000000
--- a/firmware/src/motor/calibration.h
+++ /dev/null
@@ -1,43 +0,0 @@
-
-//  * This file is part of the Tinymovr-Firmware distribution
-//  * (https://github.com/yconst/tinymovr-firmware).
-//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
-//  * 
-//  * This program is free software: you can redistribute it and/or modify  
-//  * it under the terms of the GNU General Public License as published by  
-//  * the Free Software Foundation, version 3.
-//  *
-//  * This program is distributed in the hope that it will be useful, but 
-//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
-//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
-//  * General Public License for more details.
-//  *
-//  * You should have received a copy of the GNU General Public License 
-//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
-
-#pragma once
-
-#include <src/common.h>
-#include <src/motor/motor.h>
-
-#define CAL_R_LEN             (2 * PWM_FREQ_HZ)
-#define CAL_L_LEN             (1 * PWM_FREQ_HZ)
-#define CAL_OFFSET_LEN        (1 * PWM_FREQ_HZ)
-#define CAL_STAY_LEN          (PWM_FREQ_HZ / 2)
-#define CAL_DIR_LEN           (3 * PWM_FREQ_HZ)
-#define CAL_PHASE_TURNS       (8)
-#if defined BOARD_REV_R32 || BOARD_REV_R33 || defined BOARD_REV_R5
-#define CAL_V_GAIN            (0.0005f)
-#define CAL_V_INDUCTANCE      (2.0f)
-#elif defined BOARD_REV_M5
-#define CAL_V_GAIN            (0.002f)
-#define CAL_V_INDUCTANCE      (5.0f)
-#endif
-
-bool CalibrateADCOffset(void);
-bool CalibrateResistance(void);
-bool CalibrateInductance(void);
-bool CalibrateDirectionAndPolePairs(void);
-bool calibrate_hall_sequence(void);
-bool calibrate_offset_and_rectification(void);
-void reset_calibration(void);
diff --git a/firmware/src/motor/motor.c b/firmware/src/motor/motor.c
index deb2be8e..d9d08030 100644
--- a/firmware/src/motor/motor.c
+++ b/firmware/src/motor/motor.c
@@ -15,8 +15,12 @@
 //  * You should have received a copy of the GNU General Public License
 //  * along with this program. If not, see <http://www.gnu.org/licenses/>.
 
+#include <src/adc/adc.h>
 #include <src/controller/controller.h>
+#include <src/observer/observer.h>
+#include <src/scheduler/scheduler.h>
 #include <src/utils/utils.h>
+#include <src/can/can_endpoints.h>
 #include <src/motor/motor.h>
 
 #if defined BOARD_REV_R32 || defined BOARD_REV_R33 || defined BOARD_REV_R5
@@ -26,16 +30,12 @@ static MotorConfig config = {
 	.phase_resistance = MIN_PHASE_RESISTANCE,
 	.phase_inductance = MIN_PHASE_INDUCTANCE,
 
-	.user_offset = 0.0f,
-	.user_direction = 1,
-
 	.I_cal = 6.0f,
 
 	.resistance_calibrated = false,
 	.inductance_calibrated = false,
 	.poles_calibrated = false,
 
-	.phases_swapped = false,
 	.is_gimbal = false};
 
 #elif defined BOARD_REV_M5
@@ -45,16 +45,12 @@ static MotorConfig config = {
 	.phase_resistance = MIN_PHASE_RESISTANCE,
 	.phase_inductance = MIN_PHASE_INDUCTANCE,
 
-	.user_offset = 0.0f,
-	.user_direction = 1,
-
 	.I_cal = 1.2f,
 
 	.resistance_calibrated = false,
 	.inductance_calibrated = false,
 	.poles_calibrated = false,
 
-	.phases_swapped = false,
 	.is_gimbal = false};
 
 #endif
@@ -76,13 +72,109 @@ void motor_reset_calibration()
 		config.resistance_calibrated = false;
 		config.inductance_calibrated = false;
 	}
-	config.user_offset = 0.0f;
-	config.user_direction = 1;
 	config.poles_calibrated = false;
-	config.phases_swapped = false;
 }
 
-TM_RAMFUNC uint8_t motor_find_pole_pairs(uint16_t ticks, float mpos_start, float mpos_end, float epos_rad)
+bool motor_calibrate_resistance(void)
+{
+    if (!motor_get_is_gimbal())
+    {
+        FloatTriplet I_phase_meas = {0.0f};
+        FloatTriplet modulation_values = {0.0f};
+
+		ADC_get_phase_currents(&I_phase_meas);
+
+		float I_meas = I_phase_meas.A;
+		float I_cal = motor_get_I_cal();
+        float V_setpoint = 0.0f;
+        
+        for (uint32_t i = 0; i < CAL_R_LEN; i++)
+        {
+            ADC_get_phase_currents(&I_phase_meas);
+            
+			// 
+            if (V_setpoint > MAX_CALIBRATION_VOLTAGE && I_meas < MIN_CALIBRATION_CURRENT)
+            {
+                uint8_t *error_ptr = motor_get_error_ptr();
+                *error_ptr |= MOTOR_ERRORS_ABNORMAL_CALIBRATION_VOLTAGE;
+                gate_driver_set_duty_cycle(&three_phase_zero);
+                return false;
+            }
+            
+            V_setpoint += CAL_V_GAIN * (I_cal - I_meas);
+			I_meas += CAL_I_GAIN * (I_phase_meas.A - I_meas);
+            const float pwm_setpoint = V_setpoint / system_get_Vbus();
+            SVM(pwm_setpoint, 0.0f, &modulation_values.A, &modulation_values.B, &modulation_values.C);
+            gate_driver_set_duty_cycle(&modulation_values);
+            wait_for_control_loop_interrupt();
+        }
+        
+        const float R = our_fabsf(V_setpoint / I_cal);
+        gate_driver_set_duty_cycle(&three_phase_zero);
+        
+        if ((R <= MIN_PHASE_RESISTANCE) || (R >= MAX_PHASE_RESISTANCE))
+        {
+            uint8_t *error_ptr = motor_get_error_ptr();
+            *error_ptr |= MOTOR_ERRORS_PHASE_RESISTANCE_OUT_OF_RANGE;
+            return false;
+        }
+        else
+        {
+            motor_set_phase_resistance(R);
+        }
+    }
+    return true;
+}
+
+
+bool motor_calibrate_inductance(void)
+{
+    if (!motor_get_is_gimbal())
+    {
+        float V_setpoint = 0.0f;
+        float I_low = 0.0f;
+        float I_high = 0.0f;
+        FloatTriplet I_phase_meas = {0.0f};
+        FloatTriplet modulation_values = {0.0f};
+
+        for (uint32_t i = 0; i < CAL_L_LEN; i++)
+        {
+            ADC_get_phase_currents(&I_phase_meas);
+            if ((i & 0x2u) == 0x2u)
+            {
+                I_high += I_phase_meas.A;
+                V_setpoint = -CAL_V_INDUCTANCE;
+            }
+            else
+            {
+                I_low += I_phase_meas.A;
+                V_setpoint = CAL_V_INDUCTANCE;
+            }
+            const float pwm_setpoint = V_setpoint / system_get_Vbus();
+            SVM(pwm_setpoint, 0.0f, &modulation_values.A, &modulation_values.B, &modulation_values.C);
+            gate_driver_set_duty_cycle(&modulation_values);
+            wait_for_control_loop_interrupt();
+        }
+        const float num_cycles = CAL_L_LEN / 2;
+        const float dI_by_dt = (I_high - I_low) / (PWM_PERIOD_S * num_cycles);
+        const float L = CAL_V_INDUCTANCE / dI_by_dt;
+        gate_driver_set_duty_cycle(&three_phase_zero);
+        if ((L <= MIN_PHASE_INDUCTANCE) || (L >= MAX_PHASE_INDUCTANCE))
+        {
+            uint8_t *error_ptr = motor_get_error_ptr();
+            *error_ptr |= MOTOR_ERRORS_PHASE_INDUCTANCE_OUT_OF_RANGE;
+            return false;
+        }
+        else
+        {
+            motor_set_phase_inductance(L);
+            controller_update_I_gains();
+        }
+    }
+    return true;
+}
+
+TM_RAMFUNC uint8_t motor_find_pole_pairs(uint32_t ticks, float mpos_start, float mpos_end, float epos_rad)
 {
 	const float mpos_diff = our_fabsf(mpos_end - mpos_start);
 	float mpos_diff_rad = TWOPI * mpos_diff / ticks;
@@ -90,13 +182,20 @@ TM_RAMFUNC uint8_t motor_find_pole_pairs(uint16_t ticks, float mpos_start, float
 	const uint8_t pairs_i = (uint8_t)our_floorf(pairs_f + 0.5f);
 	const float residual = our_fabsf(pairs_f - (float)pairs_i);
 
-	bool found = false;
-	if (residual <= 0.30f)
+	if (pairs_i > MOTOR_MAX_POLE_PAIRS)
+	{
+		uint8_t *error_ptr = motor_get_error_ptr();
+		*error_ptr |= MOTOR_ERRORS_POLE_PAIRS_OUT_OF_RANGE;
+		return false;
+	}
+	else if (residual > 0.30f)
 	{
-		found = true;
-		motor_set_pole_pairs(pairs_i);
+		uint8_t *error_ptr = motor_get_error_ptr();
+		*error_ptr |= MOTOR_ERRORS_POLE_PAIRS_CALCULATION_DID_NOT_CONVERGE;
+		return false;
 	}
-	return found;
+	motor_set_pole_pairs(pairs_i);
+	return true;
 }
 
 TM_RAMFUNC uint8_t motor_get_pole_pairs(void)
@@ -106,7 +205,7 @@ TM_RAMFUNC uint8_t motor_get_pole_pairs(void)
 
 TM_RAMFUNC void motor_set_pole_pairs(uint8_t pairs)
 {
-	if (pairs >= 1u)
+	if (pairs >= 1u && pairs <= MOTOR_MAX_POLE_PAIRS)
 	{
 		config.pole_pairs = pairs;
 		config.poles_calibrated = true;
@@ -180,16 +279,6 @@ TM_RAMFUNC void motor_set_I_cal(float I)
 	}
 }
 
-TM_RAMFUNC bool motor_phases_swapped(void)
-{
-	return config.phases_swapped;
-}
-
-TM_RAMFUNC void motor_set_phases_swapped(bool swapped)
-{
-	config.phases_swapped = swapped;
-}
-
 TM_RAMFUNC bool motor_get_calibrated(void)
 {
 	return config.resistance_calibrated && config.inductance_calibrated && config.poles_calibrated;
@@ -205,29 +294,6 @@ TM_RAMFUNC void motor_set_is_gimbal(bool gimbal)
 	config.is_gimbal = gimbal;
 }
 
-TM_RAMFUNC float motor_get_user_offset(void)
-{
-	return config.user_offset;
-}
-
-TM_RAMFUNC void motor_set_user_offset(float offset)
-{
-	config.user_offset = offset;
-}
-
-TM_RAMFUNC int8_t motor_get_user_direction(void)
-{
-	return config.user_direction;
-}
-
-TM_RAMFUNC void motor_set_user_direction(int8_t dir)
-{
-	if ((dir == -1) || (dir == 1))
-	{
-		config.user_direction = dir;
-	}
-}
-
 TM_RAMFUNC uint8_t motor_get_errors(void)
 {
 	return state.errors;
diff --git a/firmware/src/motor/motor.h b/firmware/src/motor/motor.h
index 25cb3481..053267c8 100644
--- a/firmware/src/motor/motor.h
+++ b/firmware/src/motor/motor.h
@@ -17,6 +17,9 @@
 
 #pragma once
 
+#include <src/utils/utils.h>
+#include <src/gatedriver/gatedriver.h>
+#include <src/system/system.h>
 #include <src/common.h>
 
 #if defined BOARD_REV_R32 || BOARD_REV_R33 || defined BOARD_REV_R5
@@ -24,29 +27,46 @@
 #define MAX_PHASE_RESISTANCE (1.0f)
 #define MIN_PHASE_INDUCTANCE (5e-6f)
 #define MAX_PHASE_INDUCTANCE (1e-3f)
+#define MAX_CALIBRATION_VOLTAGE (0.5f) // V
+#define MIN_CALIBRATION_CURRENT (0.2f) // A
 #elif defined BOARD_REV_M5
 #define MIN_PHASE_RESISTANCE (0.5f)
 #define MAX_PHASE_RESISTANCE (20.0f)
 #define MIN_PHASE_INDUCTANCE (1e-5f)
 #define MAX_PHASE_INDUCTANCE (1e-2f)
+#define MAX_CALIBRATION_VOLTAGE (5.0f) // V
+#define MIN_CALIBRATION_CURRENT (0.05f) // A
 #endif
 
+#define CAL_R_LEN             (2 * PWM_FREQ_HZ)
+#define CAL_L_LEN             (1 * PWM_FREQ_HZ)
+#define CAL_OFFSET_LEN        (1 * PWM_FREQ_HZ)
+#define CAL_STAY_LEN          (PWM_FREQ_HZ / 2)
+#define CAL_DIR_LEN           (3 * PWM_FREQ_HZ)
+#define CAL_PHASE_TURNS       (8)
+#define CAL_I_GAIN            (0.05f)
+#if defined BOARD_REV_R32 || BOARD_REV_R33 || defined BOARD_REV_R5
+#define CAL_V_GAIN            (0.0005f)
+#define CAL_V_INDUCTANCE      (2.0f)
+#elif defined BOARD_REV_M5
+#define CAL_V_GAIN            (0.002f)
+#define CAL_V_INDUCTANCE      (5.0f)
+#endif
+
+
+
 typedef struct
 {
 	uint8_t pole_pairs;
 	float phase_resistance;
 	float phase_inductance;
 
-	float user_offset;
-	float user_direction;
-
 	float I_cal;
 
 	bool resistance_calibrated;
 	bool inductance_calibrated;
 	bool poles_calibrated;
 
-	bool phases_swapped;
 	bool is_gimbal;
 } MotorConfig;
 
@@ -56,9 +76,11 @@ typedef struct
 } MotorState;
 
 void motor_reset_calibration(void);
+bool motor_calibrate_resistance(void);
+bool motor_calibrate_inductance(void);
 
 uint8_t motor_get_pole_pairs(void);
-uint8_t motor_find_pole_pairs(uint16_t ticks, float mpos_start, float mpos_end, float epos_rad);
+uint8_t motor_find_pole_pairs(uint32_t ticks, float mpos_start, float mpos_end, float epos_rad);
 void motor_set_pole_pairs(uint8_t pairs);
 
 float motor_get_phase_resistance(void);
@@ -72,23 +94,25 @@ void motor_set_phase_R_and_L(float R, float L);
 float motor_get_I_cal(void);
 void motor_set_I_cal(float I);
 
-bool motor_phases_swapped(void);
-void motor_set_phases_swapped(bool swapped);
-
 bool motor_get_calibrated(void);
 
 bool motor_get_is_gimbal(void);
 void motor_set_is_gimbal(bool gimbal);
 
-float motor_get_user_offset(void);
-void motor_set_user_offset(float offset);
-
-int8_t motor_get_user_direction(void);
-void motor_set_user_direction(int8_t dir);
-
 uint8_t motor_get_errors(void);
 uint8_t *motor_get_error_ptr(void);
 
 MotorConfig *motor_get_config(void);
 void motor_restore_config(MotorConfig *config_);
 
+static inline void set_epos_and_wait(float angle, float I_setpoint)
+{
+    FloatTriplet modulation_values = {0.0f};
+    float pwm_setpoint = (I_setpoint * motor_get_phase_resistance()) / system_get_Vbus();
+    our_clampc(&pwm_setpoint, -PWM_LIMIT, PWM_LIMIT);
+    SVM(pwm_setpoint * fast_cos(angle), pwm_setpoint * fast_sin(angle),
+        &modulation_values.A, &modulation_values.B, &modulation_values.C);
+    gate_driver_set_duty_cycle(&modulation_values);
+    wait_for_control_loop_interrupt();
+}
+
diff --git a/firmware/src/nvm/flash_func.c b/firmware/src/nvm/flash_func.c
index 13233a89..5485c994 100644
--- a/firmware/src/nvm/flash_func.c
+++ b/firmware/src/nvm/flash_func.c
@@ -22,7 +22,10 @@
 /// @retval None
 ///
 //==============================================================================
-TM_RAMFUNC void flash_erase_page(uint32_t page_num)
+#if defined(__GNUC__)
+    __attribute__((optimize("-O0")))
+#endif
+PAC5XXX_RAMFUNC void flash_erase_page(uint32_t page_num)
 {
     // Enable Write Access to FLash Controller
     PAC55XX_MEMCTL->FLASHLOCK = FLASH_LOCK_ALLOW_WRITE_ERASE_FLASH;
@@ -49,7 +52,10 @@ TM_RAMFUNC void flash_erase_page(uint32_t page_num)
 /// @retval None
 ///
 //==============================================================================
-TM_RAMFUNC void flash_erase_key(uint32_t key)
+#if defined(__GNUC__)
+    __attribute__((optimize("-O0")))
+#endif
+PAC5XXX_RAMFUNC void flash_erase_key(uint32_t key)
 {
     // Enable Write Access to FLash Controller
     PAC55XX_MEMCTL->FLASHLOCK = FLASH_LOCK_ALLOW_WRITE_ERASE_FLASH;
@@ -157,60 +163,4 @@ PAC5XXX_RAMFUNC void flash_write(uint8_t *p_dest, uint8_t *p_src, uint32_t size_
 
 }
 
-//================================================================================================
-/// @brief  Write to Flash location that is 16 byte aligned with a mulitple of 4 words
-///
-/// @note  Pay careful attention to alignment destination and source addresses along with 
-///        the number of words to write.  If these are not correct and Error will be returned
-///
-/// @param  p_dest: pointer to destination Flash to write, must be 16 byte aligned
-/// @param  p_src:  pointer to source buffer to write, must be 32-bit aligned
-/// @param  size_32bit_words: size in 32-bit words of the Flash to write, must be a multiple of 4
-///
-/// @retval PAC5XXX_OK: All is OK, PAC5XXX_ERROR: An error occurred
-///
-//=================================================================================================
-TM_RAMFUNC uint32_t flash_write_16byte_aligned(uint32_t *p_dest, uint32_t *p_src, uint32_t size_32bit_words)
-{
-
-    // If destination is not 16-byte aligned OR source isn't 32-bit aligned OR size is not a multiple of 16 bytes, then return an error
-	if( ((uint32_t)p_dest & 0xF) || ((uint32_t)p_src & 0x3) || (size_32bit_words % 4) )
-		return PAC5XXX_ERROR;
-
-	// Enable Write Access to FLash Controller
-	PAC55XX_MEMCTL->FLASHLOCK = FLASH_LOCK_ALLOW_WRITE_ERASE_FLASH;
-
-	// Ensure Flash write Word Count starts at 0
-	PAC55XX_MEMCTL->MEMCTL.WRITEWORDCNT = 0;
-
-	// Write data buffer to FLASH
-	for(uint32_t i = 0; i < size_32bit_words; i++)
-	{
-		p_dest[i] = p_src[i];
-	}
-
-    // Make sure were not still busy writing and add delay before allowing read/fetch access to flash
-    while(PAC55XX_MEMCTL->MEMSTATUS.WBUSY) { }  // wait for Flash Write WBUSY bit to be 0
-    pac_delay_asm_ramfunc(320);                 // delay 20uS after WBUSY=0
-
-    // Disable Write Access to FLash Controller
-	PAC55XX_MEMCTL->FLASHLOCK = 0;
-
-	return PAC5XXX_OK;
-}
-
-//==============================================================================
-///
-/// @brief  Write Flash with a single 32bit value
-///
-/// @param  dest_addr: Flash Destination address ( Should be A multiple of 4 )
-/// @param  data: 32-bit data to be programmed
-///
-/// @retval None
-///
-//==============================================================================
-TM_RAMFUNC void flash_write_word(uint32_t * p_dest, uint32_t value)
-{
-    flash_write((uint8_t *)p_dest, (uint8_t *)&value, 4);     // Destination, Source, size_bytes
-}
 
diff --git a/firmware/src/nvm/flash_func.h b/firmware/src/nvm/flash_func.h
index d9ed7b92..281018e0 100644
--- a/firmware/src/nvm/flash_func.h
+++ b/firmware/src/nvm/flash_func.h
@@ -22,6 +22,4 @@ extern void flash_erase_page(uint32_t page_num);
 extern void flash_erase_key(uint32_t key);
 extern void flash_write(uint8_t *p_dest, uint8_t *p_src, uint32_t size_bytes);
 extern void flash_write_word(uint32_t * p_dest, uint32_t value);
-extern uint32_t flash_write_16byte_aligned(uint32_t *p_dest, uint32_t *p_src, uint32_t size_32bit_words);
-extern uint32_t write_flash(uint32_t dest_addr, uint32_t num_words, uint32_t *src);
 #endif
diff --git a/firmware/src/nvm/nvm.c b/firmware/src/nvm/nvm.c
index b965ebbc..efb7dee5 100644
--- a/firmware/src/nvm/nvm.c
+++ b/firmware/src/nvm/nvm.c
@@ -1,4 +1,3 @@
-
 //  * This file is part of the Tinymovr-Firmware distribution
 //  * (https://github.com/yconst/tinymovr-firmware).
 //  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
@@ -23,63 +22,94 @@
 
 static struct NVMStruct s;
 
+const uint32_t config_size = sizeof(struct NVMStruct);
+
+uint32_t calculate_checksum(const uint8_t *data, size_t len)
+{
+    uint32_t checksum = 0;
+    for (size_t i = 0; i < len; ++i)
+    {
+        checksum += data[i];
+    }
+    return ~checksum + 1;
+}
+
 bool nvm_save_config(void)
 {
-	bool commited = false;
-	uint8_t data[sizeof(struct NVMStruct)];
-	s.node_id_1 = CAN_get_ID();
-	s.node_id_2 = CAN_get_ID();
-	s.adc_config = *ADC_get_config();
-	s.motor_config = *motor_get_config();
-	s.hall_config = *hall_get_config();
-	s.ma7xx_config = *ma7xx_get_config();
-	s.encoder_config = *encoder_get_config();
-	s.observer_config = *observer_get_config();
-	s.controller_config = *controller_get_config();
-	s.can_config = *CAN_get_config();
-	s.traj_planner_config = *traj_planner_get_config();
-	strlcpy(s.version, GIT_VERSION, sizeof(s.version));
-	memcpy(data, &s, sizeof(struct NVMStruct));
-	if (STATE_IDLE == controller_get_state())
-	{
-		uint8_t *dataBuffer = data;
-		__disable_irq();
-		flash_erase_page(SETTINGS_PAGE);
-		flash_write((uint8_t *)SETTINGS_PAGE_HEX, dataBuffer, sizeof(struct NVMStruct));
-		__enable_irq();
-		commited = true;
-	}
-	return commited;
+    uint8_t data[sizeof(struct NVMStruct)];
+    uint8_t readback_data[sizeof(struct NVMStruct)];
+
+    s.node_id_1 = CAN_get_ID();
+    s.node_id_2 = CAN_get_ID();
+    frames_get_config(&(s.frames_config));
+    s.adc_config = *ADC_get_config();
+    s.motor_config = *motor_get_config();
+    sensors_get_config(&(s.sensors_config));
+    observers_get_config(&(s.observers_config));
+    s.controller_config = *controller_get_config();
+    s.can_config = *CAN_get_config();
+    s.traj_planner_config = *traj_planner_get_config();
+    strncpy(s.version, GIT_VERSION, sizeof(s.version));
+    
+    memcpy(data, &s, sizeof(struct NVMStruct) - sizeof(s.checksum));
+    s.checksum = calculate_checksum(data, sizeof(struct NVMStruct) - sizeof(s.checksum));
+    memcpy(data + sizeof(struct NVMStruct) - sizeof(s.checksum), &s.checksum, sizeof(s.checksum));
+
+    if (CONTROLLER_STATE_IDLE == controller_get_state())
+    {
+        uint8_t *dataBuffer = data;
+        __disable_irq();
+        nvm_erase();
+        flash_write((uint8_t *)SETTINGS_PAGE_HEX, dataBuffer, sizeof(struct NVMStruct));
+        __enable_irq();
+
+        memcpy(readback_data, (uint8_t *)SETTINGS_PAGE_HEX, sizeof(struct NVMStruct));
+        if (memcmp(data, readback_data, sizeof(struct NVMStruct)) == 0)
+        {
+            return true;
+        }
+    }
+    return false;
 }
 
 bool nvm_load_config(void)
 {
-	memcpy(&s, (uint8_t *)SETTINGS_PAGE_HEX, sizeof(struct NVMStruct));
-	// TODO: Also validate checksum
-	bool loaded = false;
-	char static_version[16];
-	strlcpy(static_version, GIT_VERSION, sizeof(static_version));
-	if (strcmp(s.version, static_version) == 0)
-	{
-		ADC_restore_config(&s.adc_config);
-		motor_restore_config(&s.motor_config);
-		hall_restore_config(&s.hall_config);
-		ma7xx_restore_config(&s.ma7xx_config);
-		encoder_restore_config(&s.encoder_config);
-		observer_restore_config(&s.observer_config);
-		controller_restore_config(&s.controller_config);
-		CAN_restore_config(&s.can_config);
-		traj_planner_restore_config(&s.traj_planner_config);
-		loaded = true;
-	}
-	return loaded;
+    memcpy(&s, (uint8_t *)SETTINGS_PAGE_HEX, sizeof(struct NVMStruct));
+    uint32_t calculated_checksum = calculate_checksum((uint8_t *)&s, sizeof(struct NVMStruct) - sizeof(s.checksum));
+    if (calculated_checksum != s.checksum)
+    {
+        return false;
+    }
+
+    if (strncmp(s.version, GIT_VERSION, sizeof(s.version)) == 0)
+    {
+        frames_restore_config(&s.frames_config);
+        ADC_restore_config(&s.adc_config);
+        motor_restore_config(&s.motor_config);
+        sensors_restore_config(&s.sensors_config);
+        observers_restore_config(&s.observers_config);
+        controller_restore_config(&s.controller_config);
+        CAN_restore_config(&s.can_config);
+        traj_planner_restore_config(&s.traj_planner_config);
+        return true;
+    }
+    return false;
 }
 
 void nvm_erase(void)
 {
-	if (STATE_IDLE == controller_get_state())
+    for (uint8_t i = 0; i < SETTINGS_PAGE_COUNT; i++)
 	{
-		flash_erase_page(SETTINGS_PAGE);
+		flash_erase_page(SETTINGS_PAGE + i);
+	}
+}
+
+// This separate function is needed to interface with the protocol
+void nvm_erase_and_reset(void)
+{
+	if (CONTROLLER_STATE_IDLE == controller_get_state())
+    {
+		nvm_erase();
 		system_reset();
 	}
 }
diff --git a/firmware/src/nvm/nvm.h b/firmware/src/nvm/nvm.h
index eee680cb..d2d78966 100644
--- a/firmware/src/nvm/nvm.h
+++ b/firmware/src/nvm/nvm.h
@@ -1,4 +1,3 @@
-
 //  * This file is part of the Tinymovr-Firmware distribution
 //  * (https://github.com/yconst/tinymovr-firmware).
 //  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
@@ -17,35 +16,40 @@
 
 #pragma once
 
+#include <src/xfs.h>
 #include <src/adc/adc.h>
 #include <src/motor/motor.h>
-#include <src/encoder/hall.h>
-#include <src/encoder/ma7xx.h>
-#include <src/encoder/encoder.h>
+#include <src/sensor/sensors.h>
 #include <src/observer/observer.h>
 #include <src/controller/controller.h>
 #include <src/controller/trajectory_planner.h>
 #include <src/can/can.h>
 
-#define SETTINGS_PAGE (120)
-#define SETTINGS_PAGE_HEX (0x0001E000)
-
 struct NVMStruct {
     uint8_t node_id_1;
     uint8_t node_id_2;
+    FramesConfig frames_config;
     ADCConfig adc_config;
     MotorConfig motor_config;
-    HallConfig hall_config;
-    MA7xxConfig ma7xx_config;
-    EncoderConfig encoder_config;
-    ObserverConfig observer_config;
+    SensorsConfig sensors_config;
+    ObserversConfig observers_config;
     ControllerConfig controller_config;
     CANConfig can_config;
     TrajPlannerConfig traj_planner_config;
     char version[16];
+    uint32_t checksum;
 };
 
+#define SETTINGS_PAGE (120)
+#define SETTINGS_PAGE_HEX (0x0001E000)
+#define NVM_PAGE_SIZE (1024)
+#define SETTINGS_PAGE_COUNT (DIVIDE_AND_ROUND_UP(sizeof(struct NVMStruct), NVM_PAGE_SIZE))
+
+extern const uint32_t config_size;
+
 bool nvm_save_config(void);
 bool nvm_load_config(void);
 void nvm_erase(void);
+void nvm_erase_and_reset(void);
+uint32_t calculate_checksum(const uint8_t *data, size_t len);
 
diff --git a/firmware/src/observer/observer.c b/firmware/src/observer/observer.c
index ddadf125..a84a346f 100644
--- a/firmware/src/observer/observer.c
+++ b/firmware/src/observer/observer.c
@@ -21,129 +21,76 @@
 #include <src/system/system.h>
 #include <src/observer/observer.h>
 
-static ObserverState state = {0};
-
-static ObserverConfig config = {
-		.track_bw = 350.0f,
-		.kp = 0.0f,
-		.ki = 0.0f,
-};
+bool observer_init_with_defaults(Observer *o, Sensor **s)
+{
+	ObserverConfig c = {.track_bw=350};
+    return observer_init_with_config(o, s, &c);
+}
 
-void observer_init(void)
+bool observer_init_with_config(Observer *o, Sensor **s, ObserverConfig *c)
 {
-	observer_reset();
-    observer_set_bw(config.track_bw);
-	// We keep local copies of a few key variables from
-	// the encoder, because it is faster than calling
-	// the encoder function pointer
-	state.encoder_type = encoder_get_type();
-	state.encoder_ticks = encoder_get_ticks();
-	state.encoder_half_ticks = state.encoder_ticks/2;
+	o->sensor_ptr = s;
+	o->config = *c;
+    observer_set_bandwidth(o, c->track_bw);
+	return true;
 }
 
-void observer_reset(void)
+void observer_update_params(Observer *o)
 {
-	state.pos_sector = 0;
-	state.pos_estimate_wrapped = 0;
-	state.vel_estimate = 0;
+    o->config.kp = 2.0f * o->config.track_bw;
+	o->config.ki = 0.25f * (o->config.kp * o->config.kp);
+	o->config.kp_period = o->config.kp * PWM_PERIOD_S;
+	o->config.ki_period = o->config.ki * PWM_PERIOD_S;
 }
 
-TM_RAMFUNC void observer_update(void)
+void observer_reset_state(Observer *o)
 {
-	const int16_t angle_meas = encoder_get_angle();
-	const float delta_pos_est = PWM_PERIOD_S * state.vel_estimate;
-	float delta_pos_meas = angle_meas - state.pos_estimate_wrapped;
-	if (delta_pos_meas < -state.encoder_half_ticks)
-	{
-		delta_pos_meas += state.encoder_ticks;
-	}
-	else if (delta_pos_meas >= state.encoder_half_ticks)
-	{
-		delta_pos_meas -= state.encoder_ticks;
-	}
-	const float delta_pos_error = delta_pos_meas - delta_pos_est;
-	const float incr_pos = delta_pos_est + (PWM_PERIOD_S * config.kp * delta_pos_error);
-	state.pos_estimate_wrapped += incr_pos;
-	if (state.pos_estimate_wrapped < 0)
-	{
-		state.pos_estimate_wrapped += state.encoder_ticks;
-		state.pos_sector -= 1;
-	}
-	else if (state.pos_estimate_wrapped >= state.encoder_ticks)
-	{
-		state.pos_estimate_wrapped -= state.encoder_ticks;
-		state.pos_sector += 1;
-	}
-	state.vel_estimate += PWM_PERIOD_S * config.ki * delta_pos_error;
+	o->pos_sector = 0;
+	o->pos_estimate_wrapped = 0;
+	o->vel_estimate = 0;
+	o->current = false;
 }
 
-float observer_get_bw(void)
+float observer_get_bandwidth(Observer *o)
 {
-    return config.track_bw;
+    return o->config.track_bw;
 }
 
-void observer_set_bw(float bw)
+void observer_set_bandwidth(Observer *o, float bw)
 {
     if (bw > 0.0f)
     {
-        config.track_bw = bw;
-		config.kp = 2.0f * config.track_bw;
-    	config.ki = 0.25f * (config.kp * config.kp);
+        o->config.track_bw = bw;
+		observer_update_params(o);
     }
 }
 
-TM_RAMFUNC float observer_get_pos_estimate(void)
-{
-	const float primary = state.encoder_ticks * state.pos_sector;
-	return primary + state.pos_estimate_wrapped;
-}
-
-TM_RAMFUNC float observer_get_diff(float target)
-{
-	const float primary = state.encoder_ticks * state.pos_sector;
-	const float diff_sector = target - primary;
-	return diff_sector - state.pos_estimate_wrapped;
-}
-
-TM_RAMFUNC float observer_get_vel_estimate(void)
-{
-	return state.vel_estimate;
-}
-
-TM_RAMFUNC float observer_get_epos(void)
+void observers_init_with_defaults(void)
 {
-	if (ENCODER_MA7XX == state.encoder_type)
-	{
-		return state.pos_estimate_wrapped * twopi_by_enc_ticks * motor_get_pole_pairs();
-	}
-	return state.pos_estimate_wrapped * twopi_by_hall_sectors;
+    observer_init_with_defaults(&commutation_observer, &commutation_sensor_p);
+	observer_init_with_defaults(&position_observer, &position_sensor_p);
 }
 
-TM_RAMFUNC float observer_get_evel(void)
+void observers_get_config(ObserversConfig *_config)
 {
-	if (ENCODER_MA7XX == state.encoder_type)
-	{
-		return state.vel_estimate * twopi_by_enc_ticks * motor_get_pole_pairs();
-	}
-	return state.vel_estimate * twopi_by_hall_sectors;
+	_config->config_commutation = commutation_observer.config;
+	_config->config_position = position_observer.config;
 }
 
-TM_RAMFUNC float observer_get_pos_estimate_user_frame(void)
+void observers_restore_config(ObserversConfig *_config)
 {
-	return (observer_get_pos_estimate() - motor_get_user_offset()) * motor_get_user_direction();
+	observer_init_with_config(&commutation_observer, &commutation_sensor_p, &(_config->config_commutation));
+	observer_init_with_config(&position_observer, &position_sensor_p, &(_config->config_position));
+	observer_update_params(&commutation_observer);
+	observer_update_params(&position_observer);
 }
 
-TM_RAMFUNC float observer_get_vel_estimate_user_frame(void)
+void commutation_observer_set_bandwidth(float bw)
 {
-	return state.vel_estimate * motor_get_user_direction();
+	observer_set_bandwidth(&commutation_observer, bw);
 }
 
-ObserverConfig* observer_get_config(void)
+void position_observer_set_bandwidth(float bw)
 {
-	return &config;
-}
-
-void observer_restore_config(ObserverConfig* config_)
-{
-	config = *config_;
-}
+	observer_set_bandwidth(&position_observer, bw);
+}
\ No newline at end of file
diff --git a/firmware/src/observer/observer.h b/firmware/src/observer/observer.h
index 4e6b4b85..4d784203 100644
--- a/firmware/src/observer/observer.h
+++ b/firmware/src/observer/observer.h
@@ -16,47 +16,196 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef OBSERVER_OBSERVER_H_
-#define OBSERVER_OBSERVER_H_
+#pragma once
 
 #include <stdint.h>
 #include <src/common.h>
-#include <src/encoder/encoder.h>
+#include <src/sensor/sensors.h>
+#include <src/xfs.h>
 
-typedef struct 
-{
-	int32_t pos_sector;
-	float pos_estimate_wrapped;
-	float vel_estimate;
-	uint16_t encoder_ticks;
-	uint16_t encoder_half_ticks;
-	EncoderType encoder_type;
-} ObserverState;
+typedef struct Observer Observer;
 
 typedef struct 
 {
 	float track_bw;
 	float kp;
 	float ki;
+	float kp_period;
+	float ki_period;
 } ObserverConfig;
 
-void observer_init(void);
-void observer_reset(void);
+struct Observer {
+	ObserverConfig config;
+	Sensor **sensor_ptr;
+	int32_t pos_sector;
+	float pos_estimate_wrapped;
+	float vel_estimate;
+	bool initialized : 1;
+	bool current : 1;
+};
+
+typedef struct {
+    ObserverConfig config_commutation;
+	ObserverConfig config_position;
+} ObserversConfig;
+
+Observer commutation_observer;
+Observer position_observer;
+
+bool observer_init_with_defaults(Observer *o, Sensor **s);
+bool observer_init_with_config(Observer *o, Sensor **s, ObserverConfig *c);
+void observer_update_params(Observer *o);
+void observer_reset_state(Observer *o);
+
+float observer_get_bandwidth(Observer *o);
+void observer_set_bandwidth(Observer *o, float bw);
+
+void observers_init_with_defaults(void);
+void observers_get_config(ObserversConfig *config_);
+void observers_restore_config(ObserversConfig *config_);
+ 
+static inline void observer_update(Observer *o)
+{
+	if (o->current == false)
+	{
+		const float angle_meas = sensor_get_angle_rectified_normalized(*(o->sensor_ptr));
+		const float delta_pos_est = PWM_PERIOD_S * o->vel_estimate;
+		float delta_pos_meas = angle_meas - o->pos_estimate_wrapped;
+		if (delta_pos_meas < -SENSOR_COMMON_RES_HALF_TICKS)
+		{
+			delta_pos_meas += SENSOR_COMMON_RES_TICKS;
+		}
+		else if (delta_pos_meas >= SENSOR_COMMON_RES_HALF_TICKS)
+		{
+			delta_pos_meas -= SENSOR_COMMON_RES_TICKS;
+		}
+		const float delta_pos_error = delta_pos_meas - delta_pos_est;
+		const float incr_pos = delta_pos_est + (o->config.kp_period * delta_pos_error);
+		o->pos_estimate_wrapped += incr_pos;
+		if (o->pos_estimate_wrapped < 0)
+		{
+			o->pos_estimate_wrapped += SENSOR_COMMON_RES_TICKS;
+			o->pos_sector -= 1;
+		}
+		else if (o->pos_estimate_wrapped >= SENSOR_COMMON_RES_TICKS)
+		{
+			o->pos_estimate_wrapped -= SENSOR_COMMON_RES_TICKS;
+			o->pos_sector += 1;
+		}
+		o->vel_estimate += o->config.ki_period * delta_pos_error;
+		o->current = true;
+	}
+}
+
+static inline Observer *observer_get_for_sensor(Sensor *s)
+{
+	if (*(commutation_observer.sensor_ptr) == s)
+	{
+		return &commutation_observer;
+	}
+	else if (*(position_observer.sensor_ptr) == s)
+	{
+		return &position_observer;
+	}
+	else
+	{
+		return NULL;
+	}
+}
+
+static inline void observer_invalidate(Observer *o)
+{
+	o->current = false;
+}
+
+static inline float observer_get_pos_estimate(Observer *o)
+{
+	const float primary = SENSOR_COMMON_RES_TICKS * o->pos_sector;
+	return primary + o->pos_estimate_wrapped;
+}
+
+static inline float get_diff_position_sensor_frame(float target)
+{
+	const float primary = SENSOR_COMMON_RES_TICKS * position_observer.pos_sector;
+	const float diff_sector = target - primary;
+	return diff_sector - position_observer.pos_estimate_wrapped;
+}
+
+static inline float observer_get_vel_estimate(Observer *o)
+{
+	return o->vel_estimate;
+}
+
+// Interface functions
+
+static inline float commutation_observer_get_bandwidth(void)
+{
+	return observer_get_bandwidth(&commutation_observer);
+}
+
+void commutation_observer_set_bandwidth(float bw);
+
+static inline float position_observer_get_bandwidth(void)
+{
+	return observer_get_bandwidth(&position_observer);
+}
+
+void position_observer_set_bandwidth(float bw);
 
-void observer_update(void);
-float observer_get_pos_estimate(void);
-float observer_get_diff(float target);
-float observer_get_vel_estimate(void);
-float observer_get_epos(void);
-float observer_get_evel(void);
+static inline float commutation_observer_get_pos_estimate(void)
+{
+	return observer_get_pos_estimate(&commutation_observer);
+}
 
-float observer_get_pos_estimate_user_frame(void);
-float observer_get_vel_estimate_user_frame(void);
+static inline float commutation_observer_get_vel_estimate(void)
+{
+	return observer_get_vel_estimate(&commutation_observer);
+}
 
-float observer_get_bw(void);
-void observer_set_bw(float bw);
+static inline float position_observer_get_pos_estimate(void)
+{
+	return observer_get_pos_estimate(&position_observer);
+}
 
-ObserverConfig* observer_get_config(void);
-void observer_restore_config(ObserverConfig* config_);
+static inline float position_observer_get_vel_estimate(void)
+{
+	return observer_get_vel_estimate(&position_observer);
+}
 
-#endif /* OBSERVER_OBSERVER_H_ */
+static inline float user_frame_get_pos_estimate(void)
+{
+	return apply_transform(position_observer_get_pos_estimate(), frame_position_sensor_to_user_p());
+}
+
+static inline float user_frame_get_vel_estimate(void)
+{
+	return apply_velocity_transform(position_observer_get_vel_estimate(), frame_position_sensor_to_user_p());
+}
+
+static inline float motor_frame_get_pos_estimate(void)
+{
+	return apply_transform(commutation_observer_get_pos_estimate(), frame_commutation_sensor_to_motor_p());
+}
+
+static inline float motor_frame_get_vel_estimate(void)
+{
+	return apply_velocity_transform(commutation_observer_get_vel_estimate(), frame_commutation_sensor_to_motor_p());
+}
+
+static inline float observer_get_epos_motor_frame(void)
+{
+	if (SENSOR_TYPE_HALL == ((*(commutation_observer.sensor_ptr))->config.type))
+	{
+		return motor_frame_get_pos_estimate() * twopi_by_common_ticks;
+	}
+	return motor_frame_get_pos_estimate() * twopi_by_common_ticks * motor_get_pole_pairs();
+}
+
+static inline float observer_get_evel_motor_frame(void)
+{
+	if (SENSOR_TYPE_HALL == ((*(commutation_observer.sensor_ptr))->config.type))
+	{
+		return motor_frame_get_vel_estimate() * twopi_by_common_ticks;
+	}
+	return motor_frame_get_vel_estimate() * twopi_by_common_ticks * motor_get_pole_pairs();
+}
\ No newline at end of file
diff --git a/firmware/src/scheduler/scheduler.c b/firmware/src/scheduler/scheduler.c
index 6082b1a5..f45dfbbd 100644
--- a/firmware/src/scheduler/scheduler.c
+++ b/firmware/src/scheduler/scheduler.c
@@ -22,8 +22,8 @@
 #include <src/can/can.h>
 #include <src/uart/uart_interface.h>
 #include <src/uart/uart_lowlevel.h>
-#include <src/encoder/encoder.h>
-#include <src/encoder/ma7xx.h>
+#include <src/sensor/sensor.h>
+#include <src/sensor/ma7xx.h>
 #include <src/observer/observer.h>
 #include <src/can/can_endpoints.h>
 #include <src/scheduler/scheduler.h>
@@ -31,82 +31,81 @@
 
 volatile uint32_t msTicks = 0;
 
-SchedulerState state = {0};
+volatile SchedulerState scheduler_state = {0};
 
-void WaitForControlLoopInterrupt(void)
+void wait_for_control_loop_interrupt(void)
 {
-	
-	while (!state.adc_interrupt)
+	while (!scheduler_state.adc_interrupt)
 	{
 		
-		if (state.can_interrupt)
+		if (scheduler_state.can_interrupt)
 		{
 			// Handle CAN
 			CAN_process_interrupt();
 			// Only clear the flag if all messages in the CAN RX buffer have been processed
 			if (PAC55XX_CAN->SR.RBS == 0)
 			{
-				state.can_interrupt = false;
+				scheduler_state.can_interrupt = false;
 			}
 		}
-		else if (state.uart_message_interrupt)
+		else if (scheduler_state.uart_message_interrupt)
 		{
 			// Handle UART
-			state.uart_message_interrupt = false;
+			scheduler_state.uart_message_interrupt = false;
 			UART_process_message();
 		}
-		else if (state.wwdt_interrupt)
+		else if (scheduler_state.wwdt_interrupt)
 		{
-			state.wwdt_interrupt = false;
+			scheduler_state.wwdt_interrupt = false;
 			WWDT_process_interrupt();
 		}
 		else
 		{
-			state.busy = false;
+			scheduler_state.busy = false;
+			scheduler_state.load = DWT->CYCCNT;
 			// Go back to sleep
 			__DSB();
 			__ISB();
 			__WFI();
 		}
 	}
-	state.busy = true;
-	state.adc_interrupt = false;
+	scheduler_state.busy = true;
+	scheduler_state.adc_interrupt = false;
+	DWT->CYCCNT = 0;
 	// We have to service the control loop by updating
 	// current measurements and encoder estimates.
-	if (ENCODER_MA7XX == encoder_get_type())
-	{
-		ma7xx_send_angle_cmd();
-	}
+	sensor_invalidate(commutation_sensor_p);
+	sensor_invalidate(position_sensor_p);
+	observer_invalidate(&commutation_observer);
+	observer_invalidate(&position_observer);
+	// If both pointers point to the same sensor, it will only br prepared and updated once
+	sensor_prepare(commutation_sensor_p);
+	sensor_prepare(position_sensor_p);
 	ADC_update();
-	
-	encoder_update(true);
-	observer_update();
+	sensor_update(commutation_sensor_p, true);
+	sensor_update(position_sensor_p, true);
+	observer_update(&commutation_observer);
+	observer_update(&position_observer);
 	// At this point control is returned to main loop.
 }
 
 void ADC_IRQHandler(void)
 {
 	PAC55XX_ADC->ADCINT.ADCIRQ0IF = 1;
-	// Only in case the gate driver is enabled, ensure
-	// the control deadline is not missed,
-	// i.e. the previous control loop is complete prior
-	// to the ADC triggering the next
-	if ((gate_driver_is_enabled() == true) && (state.busy == true))
-	{
-		state.errors |= SCHEDULER_ERRORS_CONTROL_BLOCK_REENTERED;
-		// We do not change the control state here, to
-		// avoid any concurrency issues
-	}
-	else
+	scheduler_state.adc_interrupt = true;
+	// Only in case the gate driver is enabled, raise a
+	// warning if the control loop is about to be
+	// reentered
+	if (gate_driver_is_enabled() && scheduler_state.busy)
 	{
-		state.adc_interrupt = true;
+		scheduler_state.warnings |= SCHEDULER_WARNINGS_CONTROL_BLOCK_REENTERED;
 	}
 }
 
 void CAN_IRQHandler(void)
 {
 	pac5xxx_can_int_clear_RI();
-	state.can_interrupt = true;
+	scheduler_state.can_interrupt = true;
 }
 
 void SysTick_Handler(void)
@@ -118,18 +117,14 @@ void SysTick_Handler(void)
 
 void UART_ReceiveMessageHandler(void)
 {
-	state.uart_message_interrupt = true;
+	scheduler_state.uart_message_interrupt = true;
 }
 
 void Wdt_IRQHandler(void)
 {
-	state.wwdt_interrupt = true;
+	scheduler_state.wwdt_interrupt = true;
 	PAC55XX_WWDT->WWDTLOCK = WWDTLOCK_REGS_WRITE_AVALABLE;
     // Interrupt flag needs to be cleared here
     PAC55XX_WWDT->WWDTFLAG.IF = 1;
 }
 
-TM_RAMFUNC uint8_t scheduler_get_errors(void)
-{
-	return state.errors;
-}
diff --git a/firmware/src/scheduler/scheduler.h b/firmware/src/scheduler/scheduler.h
index e4af643d..32f8f887 100644
--- a/firmware/src/scheduler/scheduler.h
+++ b/firmware/src/scheduler/scheduler.h
@@ -24,10 +24,21 @@ typedef struct
 	bool uart_message_interrupt;
     bool wwdt_interrupt;
 	bool busy;
+	uint32_t load;
 
-    uint8_t errors;
+    uint8_t warnings;
 } SchedulerState;
 
-void WaitForControlLoopInterrupt(void);
+extern volatile SchedulerState scheduler_state;
 
-uint8_t scheduler_get_errors(void);
+void wait_for_control_loop_interrupt(void);
+
+static inline uint8_t scheduler_get_warnings(void)
+{
+	return scheduler_state.warnings;
+}
+
+static inline uint32_t scheduler_get_load(void)
+{
+	return scheduler_state.load;
+}
\ No newline at end of file
diff --git a/firmware/src/sensor/amt22.c b/firmware/src/sensor/amt22.c
new file mode 100644
index 00000000..d2dd575f
--- /dev/null
+++ b/firmware/src/sensor/amt22.c
@@ -0,0 +1,88 @@
+
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  * 
+//  * This program is free software: you can redistribute it and/or modify  
+//  * it under the terms of the GNU General Public License as published by  
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but 
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License 
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include <string.h>
+#include <src/system/system.h>
+#include <src/utils/utils.h>
+#include <src/can/can_endpoints.h>
+#include <src/sensor/sensor.h>
+#include <src/sensor/amt22.h>
+
+void amt22_make_blank_sensor(Sensor *s)
+{
+    AMT22Sensor *as = (AMT22Sensor *)s;
+    as->config.rate = SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps;
+    s->config.type = SENSOR_TYPE_AMT22;
+    s->bits = AMT22_BITS;
+    s->ticks = AMT22_TICKS;
+    s->normalization_factor = SENSOR_COMMON_RES_TICKS_FLOAT / s->ticks;
+    s->get_raw_angle_func = amt22_get_raw_angle;
+    s->update_func = amt22_update; 
+    s->prepare_func = amt22_send_angle_cmd; 
+    s->reset_func = amt22_reset; 
+    s->init_func = amt22_init; 
+    s->deinit_func = amt22_deinit;
+    s->get_errors_func = amt22_get_errors; 
+    s->is_calibrated_func = amt22_is_calibrated; 
+    s->get_ss_config_func = amt22_get_ss_config;
+}
+
+bool amt22_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct, sensors_setup_external_spi_rate_options rate)
+{
+    AMT22SensorConfig c = {0};
+    c.ssp_port = port;
+    c.ssp_struct = ssp_struct;
+    c.rate = rate;
+    return amt22_init_with_config(s, &c);
+}
+
+bool amt22_init_with_config(Sensor *s, const AMT22SensorConfig *c)
+{
+    AMT22Sensor *as = (AMT22Sensor *)s;
+    as->config = *c;
+    return amt22_init(s);
+}
+
+bool amt22_init(Sensor *s)
+{
+    AMT22Sensor *as = (AMT22Sensor *)s;
+    ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, SSP_DATA_SIZE_8, SWSEL_SW, 0, 0);
+    delay_us(50000); 
+    
+    amt22_send_angle_cmd(s); 
+    amt22_update(s, false); 
+
+    s->initialized = true;
+    return true;
+}
+
+void amt22_deinit(Sensor *s)
+{
+    ssp_deinit(((AMT22Sensor *)s)->config.ssp_port);
+    s->initialized = false;
+}
+
+void amt22_reset(Sensor *s)
+{
+    sensor_reset(s);
+}
+
+void amt22_get_ss_config(Sensor *s, void* buffer)
+{
+    const AMT22Sensor *ss = ((const AMT22Sensor *)s);
+    memcpy(buffer, &(ss->config), sizeof(ss->config));
+}
diff --git a/firmware/src/sensor/amt22.h b/firmware/src/sensor/amt22.h
new file mode 100644
index 00000000..86a11d6e
--- /dev/null
+++ b/firmware/src/sensor/amt22.h
@@ -0,0 +1,129 @@
+
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  * 
+//  * This program is free software: you can redistribute it and/or modify  
+//  * it under the terms of the GNU General Public License as published by  
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but 
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License 
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <src/ssp/ssp_func.h>
+#include <src/can/can_endpoints.h>
+#include <src/sensor/sensor.h>
+
+#define AMT22_BITS (14) // Assuming a 14-bit resolution for consistency
+#define AMT22_TICKS (1 << AMT22_BITS)
+#define AMT22_MAX_ALLOWED_DELTA     (AMT22_TICKS / 6)
+#define AMT22_MAX_ALLOWED_DELTA_ADD (AMT22_MAX_ALLOWED_DELTA + AMT22_TICKS)
+#define AMT22_MAX_ALLOWED_DELTA_SUB (AMT22_MAX_ALLOWED_DELTA - AMT22_TICKS)
+#define AMT22_MIN_ALLOWED_DELTA_ADD (-AMT22_MAX_ALLOWED_DELTA + AMT22_TICKS)
+#define AMT22_MIN_ALLOWED_DELTA_SUB (-AMT22_MAX_ALLOWED_DELTA - AMT22_TICKS)
+
+typedef enum {
+    AMT22_CMD_READ_ANGLE       = 0xFFFF
+} AMT22Command;
+
+typedef struct
+{
+    SSP_TYPE ssp_port;
+    PAC55XX_SSP_TYPEDEF *ssp_struct;
+    sensors_setup_external_spi_rate_options rate;
+} AMT22SensorConfig;
+
+typedef struct
+{
+    Sensor base;
+    AMT22SensorConfig config;
+    uint8_t errors;
+    int32_t angle;
+    uint8_t val_l;
+    uint8_t val_h;
+
+} AMT22Sensor;
+
+void amt22_make_blank_sensor(Sensor *s);
+bool amt22_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct, sensors_setup_external_spi_rate_options rate);
+bool amt22_init_with_config(Sensor *s, const AMT22SensorConfig *c);
+bool amt22_init(Sensor *s);
+void amt22_deinit(Sensor *s);
+void amt22_reset(Sensor *s);
+void amt22_get_ss_config(Sensor *s, void* buffer);
+
+static inline bool amt22_is_calibrated(const Sensor *s)
+{
+    return s->config.rec_calibrated;
+}
+
+static inline uint8_t amt22_get_errors(const Sensor *s)
+{
+    return ((const AMT22Sensor *)s)->errors;
+}
+
+static inline void amt22_send_angle_cmd(const Sensor *s)
+{
+    // AMT22 has specific timing requirements with respect to reading
+    // the first and second bytes of the angle.
+    // TODO: These delays make reading of the sensor really slow
+    // refactor using interrupts
+    AMT22Sensor *as = ((AMT22Sensor *)s);
+    as->config.ssp_struct->SSCR.SWSS = 0;
+    delay_us(3);
+    ssp_write_one(as->config.ssp_struct, AMT22_CMD_READ_ANGLE);
+    as->val_h = ssp_read_one(as->config.ssp_struct);
+    delay_us(3);
+    ssp_write_one(as->config.ssp_struct, AMT22_CMD_READ_ANGLE);
+    as->val_l = ssp_read_one(as->config.ssp_struct);
+    delay_us(3);
+    as->config.ssp_struct->SSCR.SWSS = 1;
+}
+
+static inline int32_t amt22_get_raw_angle(const Sensor *s)
+{
+    return ((const AMT22Sensor *)s)->angle;
+}
+
+static inline void amt22_update(Sensor *s, bool check_error)
+{
+    AMT22Sensor *as = (AMT22Sensor *)s;
+
+    const int32_t value = (((as->val_h & 0xff) << 8) | (as->val_l & 0xff)); 
+    
+    // TODO: This checksum calculation is sub-optimal
+    bool binaryArray[16];
+
+    for(int i = 0; i < 16; i++) binaryArray[i] = (0x01) & (value >> (i));
+
+    if ((binaryArray[15] == !(binaryArray[13] ^ binaryArray[11] ^ binaryArray[9] ^ binaryArray[7] ^ binaryArray[5] ^ binaryArray[3] ^ binaryArray[1]))
+          && (binaryArray[14] == !(binaryArray[12] ^ binaryArray[10] ^ binaryArray[8] ^ binaryArray[6] ^ binaryArray[4] ^ binaryArray[2] ^ binaryArray[0])))
+    {
+        const int32_t angle = value & 0x3FFF; 
+        if (check_error)
+        {
+            const int32_t delta = as->angle - angle;
+            if ( ((delta > AMT22_MAX_ALLOWED_DELTA) || (delta < -AMT22_MAX_ALLOWED_DELTA)) &&
+                ((delta > AMT22_MAX_ALLOWED_DELTA_ADD) || (delta < AMT22_MIN_ALLOWED_DELTA_ADD)) &&
+                ((delta > AMT22_MAX_ALLOWED_DELTA_SUB) || (delta < AMT22_MIN_ALLOWED_DELTA_SUB)) )
+            {
+                
+            }
+            else
+            {
+                as->angle = value & 0x3FFF;
+            }
+        }
+        
+    }
+    
+}
+
+
diff --git a/firmware/src/sensor/as5047.c b/firmware/src/sensor/as5047.c
new file mode 100644
index 00000000..0659bee7
--- /dev/null
+++ b/firmware/src/sensor/as5047.c
@@ -0,0 +1,87 @@
+
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  * 
+//  * This program is free software: you can redistribute it and/or modify  
+//  * it under the terms of the GNU General Public License as published by  
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but 
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License 
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include <string.h>
+#include <src/system/system.h>
+#include <src/utils/utils.h>
+#include <src/can/can_endpoints.h>
+#include <src/sensor/sensor.h>
+#include <src/sensor/as5047.h>
+
+void as5047p_make_blank_sensor(Sensor *s)
+{
+    AS5047PSensor *as = (AS5047PSensor *)s;
+    as->config.rate = SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps;
+    s->config.type = SENSOR_TYPE_AS5047;
+    s->bits = AS5047_BITS;
+    s->ticks = AS5047_TICKS;
+    s->normalization_factor = SENSOR_COMMON_RES_TICKS_FLOAT / s->ticks;
+    s->get_raw_angle_func = as5047p_get_raw_angle;
+    s->update_func = as5047p_update; 
+    s->prepare_func = as5047p_send_angle_cmd; 
+    s->reset_func = as5047p_reset;
+    s->init_func = as5047p_init; 
+    s->deinit_func = as5047p_deinit; 
+    s->get_errors_func = as5047p_get_errors; 
+    s->is_calibrated_func = as5047p_is_calibrated; 
+    s->get_ss_config_func = as5047p_get_ss_config;
+}
+
+bool as5047p_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct, sensors_setup_external_spi_rate_options rate)
+{
+    AS5047PSensorConfig c = {0};
+    c.ssp_port = port;
+    c.ssp_struct = ssp_struct;
+    c.rate = rate;
+    return as5047p_init_with_config(s, &c);
+}
+
+bool as5047p_init_with_config(Sensor *s, const AS5047PSensorConfig *c)
+{
+    AS5047PSensor *as = (AS5047PSensor *)s;
+    as->config = *c;
+    return as5047p_init(s);
+}
+
+bool as5047p_init(Sensor *s)
+{
+    AS5047PSensor *as = (AS5047PSensor *)s;
+    ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, SSP_DATA_SIZE_16, SWSEL_SPI, 1, 0);
+    delay_us(10000); // Example delay, adjust based on AS5047P datasheet
+
+    as5047p_send_angle_cmd(s); 
+    as5047p_update(s, false); 
+    s->initialized = true;
+    return true;
+}
+
+void as5047p_deinit(Sensor *s)
+{
+    ssp_deinit(((AS5047PSensor *)s)->config.ssp_port);
+    s->initialized = false;
+}
+
+void as5047p_reset(Sensor *s)
+{
+    sensor_reset(s);
+}
+
+void as5047p_get_ss_config(Sensor *s, void* buffer)
+{
+    const AS5047PSensor *ss = ((const AS5047PSensor *)s);
+    memcpy(buffer, &(ss->config), sizeof(ss->config));
+}
\ No newline at end of file
diff --git a/firmware/src/sensor/as5047.h b/firmware/src/sensor/as5047.h
new file mode 100644
index 00000000..81b17fe9
--- /dev/null
+++ b/firmware/src/sensor/as5047.h
@@ -0,0 +1,96 @@
+
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  * 
+//  * This program is free software: you can redistribute it and/or modify  
+//  * it under the terms of the GNU General Public License as published by  
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but 
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License 
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <src/ssp/ssp_func.h>
+#include <src/can/can_endpoints.h>
+#include <src/sensor/sensor.h>
+
+#define AS5047_BITS (14) // The bits we READ, not the advertised resolution
+#define AS5047_TICKS (1 << AS5047_BITS)
+#define AS5047_MAX_ALLOWED_DELTA     (AS5047_TICKS / 6)
+#define AS5047_MAX_ALLOWED_DELTA_ADD (AS5047_MAX_ALLOWED_DELTA + AS5047_TICKS)
+#define AS5047_MAX_ALLOWED_DELTA_SUB (AS5047_MAX_ALLOWED_DELTA - AS5047_TICKS)
+#define AS5047_MIN_ALLOWED_DELTA_ADD (-AS5047_MAX_ALLOWED_DELTA + AS5047_TICKS)
+#define AS5047_MIN_ALLOWED_DELTA_SUB (-AS5047_MAX_ALLOWED_DELTA - AS5047_TICKS)
+
+typedef enum {
+    AS5047P_CMD_NOP              = 0x0000,
+    AS5047P_CMD_READ_ANGLE       = 0x3FFF 
+} AS5047PCommand;
+
+typedef struct
+{
+    SSP_TYPE ssp_port;
+    PAC55XX_SSP_TYPEDEF *ssp_struct;
+    sensors_setup_external_spi_rate_options rate;
+} AS5047PSensorConfig;
+
+typedef struct
+{
+    Sensor base;
+    AS5047PSensorConfig config;
+    uint8_t errors;
+    int32_t angle;
+} AS5047PSensor;
+
+void as5047p_make_blank_sensor(Sensor *s);
+bool as5047p_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct, sensors_setup_external_spi_rate_options rate);
+bool as5047p_init_with_config(Sensor *s, const AS5047PSensorConfig *c);
+bool as5047p_init(Sensor *s);
+void as5047p_deinit(Sensor *s);
+void as5047p_reset(Sensor *s);
+void as5047p_get_ss_config(Sensor *s, void* buffer);
+
+static inline bool as5047p_is_calibrated(const Sensor *s)
+{
+    return s->config.rec_calibrated;
+}
+
+static inline uint8_t as5047p_get_errors(const Sensor *s)
+{
+    return ((const AS5047PSensor *)s)->errors;
+}
+
+static inline void as5047p_send_angle_cmd(const Sensor *s)
+{
+    ssp_write_one(((const AS5047PSensor *)s)->config.ssp_struct, AS5047P_CMD_READ_ANGLE);
+}
+
+static inline int32_t as5047p_get_raw_angle(const Sensor *s)
+{
+    return ((const AS5047PSensor *)s)->angle;
+}
+
+static inline void as5047p_update(Sensor *s, bool check_error)
+{
+    AS5047PSensor *as = (AS5047PSensor *)s;
+    volatile uint16_t read_value = ssp_read_one(as->config.ssp_struct);
+    const int32_t angle = read_value & 0x3FFF; // Mask to get the angle value
+    if (check_error)
+    {
+    	const int32_t delta = as->angle - angle;
+		if ( ((delta > AS5047_MAX_ALLOWED_DELTA) || (delta < -AS5047_MAX_ALLOWED_DELTA)) &&
+		     ((delta > AS5047_MAX_ALLOWED_DELTA_ADD) || (delta < AS5047_MIN_ALLOWED_DELTA_ADD)) &&
+		     ((delta > AS5047_MAX_ALLOWED_DELTA_SUB) || (delta < AS5047_MIN_ALLOWED_DELTA_SUB)) )
+		{
+            as->errors |= SENSORS_SETUP_ONBOARD_ERRORS_READING_UNSTABLE;
+		}
+    }
+    as->angle = angle;
+}
\ No newline at end of file
diff --git a/firmware/src/sensor/hall.c b/firmware/src/sensor/hall.c
new file mode 100644
index 00000000..38d7a368
--- /dev/null
+++ b/firmware/src/sensor/hall.c
@@ -0,0 +1,160 @@
+
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  *
+//  * This program is free software: you can redistribute it and/or modify
+//  * it under the terms of the GNU General Public License as published by
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include <string.h>
+#include <src/system/system.h>
+#include <src/can/can_endpoints.h>
+#include <src/sensor/sensor.h>
+#include <src/sensor/hall.h>
+
+#define AIO6789_IO_MODE                 0x00
+#define AIO_INPUT                       0x00
+
+void hall_make_blank_sensor(Sensor *s)
+{
+    s->config.type = SENSOR_TYPE_HALL;
+    s->bits = HALL_BITS;
+    s->ticks = HALL_SECTORS;
+    s->normalization_factor = SENSOR_COMMON_RES_TICKS_FLOAT / s->ticks;
+    s->get_raw_angle_func = hall_get_angle;
+    s->update_func = hall_update;
+    s->reset_func = hall_reset;
+    s->init_func = hall_init;
+    s->deinit_func = hall_deinit;
+    s->get_errors_func = hall_get_errors;
+    s->is_calibrated_func = hall_sector_map_is_calibrated;
+    s->calibrate_func = hall_calibrate_sequence;
+    s->get_ss_config_func = hall_get_ss_config;
+}
+
+bool hall_init_with_defaults(Sensor *s)
+{
+    HallSensorConfig c = {0};
+    return hall_init_with_config(s, &c);
+}
+
+bool hall_init_with_config(Sensor *s, const HallSensorConfig *c)
+{
+    HallSensor *ms = (HallSensor *)s;
+    ms->config = *c;
+    return hall_init(s);
+}
+
+bool hall_init(Sensor *s)
+{
+    HallSensor *ms = (HallSensor *)s;
+    ms->hw_defaults[0] = pac5xxx_tile_register_read(ADDR_CFGAIO7);
+    ms->hw_defaults[1] = pac5xxx_tile_register_read(ADDR_CFGAIO8);
+    ms->hw_defaults[2] = pac5xxx_tile_register_read(ADDR_CFGAIO9);
+    pac5xxx_tile_register_write(ADDR_CFGAIO7, AIO6789_IO_MODE | AIO_INPUT);
+    pac5xxx_tile_register_write(ADDR_CFGAIO8, AIO6789_IO_MODE | AIO_INPUT);
+    pac5xxx_tile_register_write(ADDR_CFGAIO9, AIO6789_IO_MODE | AIO_INPUT);
+    s->initialized = true;
+    return true;
+}
+
+void hall_deinit(Sensor *s)
+{
+    HallSensor *ms = (HallSensor *)s;
+    pac5xxx_tile_register_write(ADDR_CFGAIO7, ms->hw_defaults[0]);
+    pac5xxx_tile_register_write(ADDR_CFGAIO8, ms->hw_defaults[1]);
+    pac5xxx_tile_register_write(ADDR_CFGAIO9, ms->hw_defaults[2]);
+    s->initialized = false;
+}
+
+void hall_reset(Sensor *s)
+{
+    HallSensor *ms = (HallSensor *)s;
+    memset(ms->config.sector_map, 0, sizeof(ms->config.sector_map));
+    ms->config.sector_map_calibrated = false;
+}
+
+bool hall_calibrate_sequence(Sensor *s, Observer *o)
+{
+    HallSensor *ms = (HallSensor *)s;
+    HallSensorConfig *c = &(ms->config);
+    (void)memset(c->sector_map, 0, sizeof(c->sector_map));
+	c->sector_map_calibrated = false;
+    uint8_t *sector_map = c->sector_map;
+    const float I_setpoint = motor_get_I_cal();
+    bool success = true;
+
+    // Stay a bit at starting epos
+	for (uint32_t i=0; i<CAL_STAY_LEN; i++)
+	{
+		set_epos_and_wait(0, I_setpoint);
+	}
+    const uint8_t init_sector = hall_get_sector(s);
+    uint8_t current_sector = init_sector;
+    uint8_t last_sector = init_sector;
+    uint8_t sector_pos = 0;
+    const float increment = TWOPI / CAL_DIR_LEN;
+    float angle = 0;
+
+    sector_map[current_sector] = sector_pos;
+
+    // Move to the 2nd sector
+    while ((current_sector == init_sector) && (angle < TWOPI / HALL_SECTORS))
+    {
+        set_epos_and_wait(angle, I_setpoint);
+        angle += increment;
+        current_sector = hall_get_sector(s);
+    }
+
+    // Save the rest of the sectors
+    while ((current_sector != init_sector) && (angle < TWOPI))
+    {
+        if (current_sector != last_sector)
+        {
+            last_sector = current_sector;
+            sector_pos++;
+            if (sector_pos >= HALL_SECTORS)
+            {
+                success = false;
+                break;
+            }
+            sector_map[current_sector] = sector_pos;
+        }
+        set_epos_and_wait(angle, I_setpoint);
+        angle += increment;
+        current_sector = hall_get_sector(s);
+    }
+
+    gate_driver_set_duty_cycle(&three_phase_zero);
+
+    // Check that the number of sectors discovered is the same as expected
+    if (sector_pos != HALL_SECTORS - 1)
+    {
+        success = false;
+    }
+
+    if (success)
+    {
+        c->sector_map_calibrated = true;
+    }
+    else
+    {
+        ms->errors |= SENSORS_SETUP_HALL_ERRORS_CALIBRATION_FAILED;
+    }
+    return success;
+}
+
+void hall_get_ss_config(Sensor *s, void* buffer)
+{
+    const HallSensor *ss = ((const HallSensor *)s);
+    memcpy(buffer, &(ss->config), sizeof(ss->config));
+}
\ No newline at end of file
diff --git a/firmware/src/sensor/hall.h b/firmware/src/sensor/hall.h
new file mode 100644
index 00000000..c2fcf5ca
--- /dev/null
+++ b/firmware/src/sensor/hall.h
@@ -0,0 +1,81 @@
+
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  * 
+//  * This program is free software: you can redistribute it and/or modify  
+//  * it under the terms of the GNU General Public License as published by  
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but 
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License 
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <src/sensor/sensor.h>
+
+#define HALL_BITS (3)
+#define HALL_SECTORS ((1 << HALL_BITS) - 2)
+#define HALL_SECTOR_ANGLE (TWOPI / HALL_SECTORS)
+#define CAL_DIR_LEN_PER_SECTOR (CAL_DIR_LEN / HALL_SECTORS)
+
+static const float twopi_by_hall_sectors = TWOPI / HALL_SECTORS;
+
+typedef struct
+{
+	uint8_t sector_map[8];
+    bool sector_map_calibrated;
+} HallSensorConfig;
+
+typedef struct
+{
+    Sensor base;
+    HallSensorConfig config;
+    uint8_t errors;
+	int32_t angle;
+    uint8_t sector;
+    uint8_t hw_defaults[3];
+} HallSensor;
+
+void hall_make_blank_sensor(Sensor *s);
+bool hall_init_with_defaults(Sensor *s);
+bool hall_init_with_config(Sensor *s, const HallSensorConfig *c);
+bool hall_init(Sensor *s);
+void hall_deinit(Sensor *s);
+void hall_reset(Sensor *s);
+bool hall_calibrate_sequence(Sensor *s, Observer *o);
+void hall_get_ss_config(Sensor *s, void* buffer);
+
+static inline uint8_t hall_get_errors(const Sensor *s)
+{
+    return ((HallSensor *)s)->errors;
+}
+
+static inline int32_t hall_get_angle(const Sensor *s)
+{
+    return ((HallSensor *)s)->angle;
+}
+
+static inline void hall_update(Sensor *s, bool check_error)
+{
+    HallSensor *ms = (HallSensor *)s;
+    const uint8_t sector = (pac5xxx_tile_register_read(ADDR_DINSIG1) >> 1) & 0x07;
+    ms->sector = sector;
+    ms->angle = ms->config.sector_map[ms->sector];
+}
+
+static inline uint8_t hall_get_sector(const Sensor *s)
+{
+    return ((const HallSensor *)s)->sector;
+}
+
+static inline bool hall_sector_map_is_calibrated(const Sensor *s)
+{
+    return ((const HallSensor *)s)->config.sector_map_calibrated;
+}
+
diff --git a/firmware/src/sensor/ma7xx.c b/firmware/src/sensor/ma7xx.c
new file mode 100644
index 00000000..1d011626
--- /dev/null
+++ b/firmware/src/sensor/ma7xx.c
@@ -0,0 +1,86 @@
+
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  * 
+//  * This program is free software: you can redistribute it and/or modify  
+//  * it under the terms of the GNU General Public License as published by  
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but 
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License 
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include <string.h>
+#include <src/system/system.h>
+#include <src/utils/utils.h>
+#include <src/can/can_endpoints.h>
+#include <src/sensor/sensor.h>
+#include <src/sensor/ma7xx.h>
+
+void ma7xx_make_blank_sensor(Sensor *s)
+{
+    MA7xxSensor *as = (MA7xxSensor *)s;
+    as->config.rate = SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps;
+    s->config.type = SENSOR_TYPE_MA7XX;
+    s->bits = MA7XX_BITS;
+    s->ticks = MA7XX_TICKS;
+    s->normalization_factor = SENSOR_COMMON_RES_TICKS_FLOAT / s->ticks;
+    s->is_calibrated_func = ma7xx_rec_is_calibrated;
+    s->get_raw_angle_func = ma7xx_get_raw_angle;
+    s->init_func = ma7xx_init;
+    s->deinit_func = ma7xx_deinit;
+    s->reset_func = ma7xx_reset;
+    s->update_func = ma7xx_update;
+    s->prepare_func = ma7xx_send_angle_cmd;
+    s->get_errors_func = ma7xx_get_errors;
+    s->get_ss_config_func = ma7xx_get_ss_config;
+}
+
+bool ma7xx_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct, sensors_setup_external_spi_rate_options rate)
+{
+    MA7xxSensorConfig c = {0};
+    c.ssp_port = port;
+    c.ssp_struct = ssp_struct;
+    c.rate = rate;
+    return ma7xx_init_with_config(s, &c);
+}
+
+ bool ma7xx_init_with_config(Sensor *s, const MA7xxSensorConfig *c)
+{
+    MA7xxSensor *ms = (MA7xxSensor *)s;
+    ms->config = *c;
+    return ma7xx_init(s);
+}
+
+ bool ma7xx_init(Sensor *s)
+{
+    MA7xxSensor *ms = (MA7xxSensor *)s;
+    ssp_init(ms->config.ssp_port, SSP_MS_MASTER, 4, SSP_DATA_SIZE_16, SWSEL_SPI, 0, 0);
+    delay_us(16000); // ensure 16ms sensor startup time as per the datasheet
+    ma7xx_send_angle_cmd(s);
+    ma7xx_update(s, false);
+    s->initialized = true;
+    return true;
+}
+
+void ma7xx_deinit(Sensor *s)
+{
+    ssp_deinit(((MA7xxSensor *)s)->config.ssp_port);
+    s->initialized = false;
+}
+
+void ma7xx_reset(Sensor *s)
+{
+    sensor_reset(s);
+}
+
+void ma7xx_get_ss_config(Sensor *s, void* buffer)
+{
+    const MA7xxSensor *ss = ((const MA7xxSensor *)s);
+    memcpy(buffer, &(ss->config), sizeof(ss->config));
+}
diff --git a/firmware/src/sensor/ma7xx.h b/firmware/src/sensor/ma7xx.h
new file mode 100644
index 00000000..b36e0e2a
--- /dev/null
+++ b/firmware/src/sensor/ma7xx.h
@@ -0,0 +1,142 @@
+
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  * 
+//  * This program is free software: you can redistribute it and/or modify  
+//  * it under the terms of the GNU General Public License as published by  
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but 
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License 
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <src/ssp/ssp_func.h>
+#include <src/can/can_endpoints.h>
+#include <src/sensor/sensor.h>
+
+#define MA7XX_BITS (16) // The bits we READ, not the advertised resolution
+#define MA7XX_TICKS (1 << MA7XX_BITS)
+#define MA7XX_MAX_ALLOWED_DELTA     (MA7XX_TICKS / 6)
+#define MA7XX_MAX_ALLOWED_DELTA_ADD (MA7XX_MAX_ALLOWED_DELTA + MA7XX_TICKS)
+#define MA7XX_MAX_ALLOWED_DELTA_SUB (MA7XX_MAX_ALLOWED_DELTA - MA7XX_TICKS)
+#define MA7XX_MIN_ALLOWED_DELTA_ADD (-MA7XX_MAX_ALLOWED_DELTA + MA7XX_TICKS)
+#define MA7XX_MIN_ALLOWED_DELTA_SUB (-MA7XX_MAX_ALLOWED_DELTA - MA7XX_TICKS)
+
+typedef enum {
+    MA_CMD_NOP              = 0x0000,
+    MA_CMD_ANGLE            = 0x0000,
+    MA_CMD_WRITE            = 0x8000,
+    MA_CMD_READ             = 0x4000
+} MA702Command;
+
+typedef struct
+{
+    SSP_TYPE ssp_port;
+    PAC55XX_SSP_TYPEDEF *ssp_struct;
+    sensors_setup_external_spi_rate_options rate;
+} MA7xxSensorConfig;
+
+typedef struct
+{
+    Sensor base;
+    MA7xxSensorConfig config;
+    uint8_t errors;
+	int32_t angle;
+} MA7xxSensor;
+
+void ma7xx_make_blank_sensor(Sensor *s);
+bool ma7xx_init_with_port_and_rate(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct, sensors_setup_external_spi_rate_options rate);
+bool ma7xx_init_with_config(Sensor *s, const MA7xxSensorConfig *c);
+bool ma7xx_init(Sensor *s);
+void ma7xx_deinit(Sensor *s);
+void ma7xx_reset(Sensor *s);
+void ma7xx_get_ss_config(Sensor *s, void* buffer);
+
+static inline bool ma7xx_rec_is_calibrated(const Sensor *s)
+{
+    return s->config.rec_calibrated;
+}
+
+static inline uint8_t ma7xx_get_errors(const Sensor *s)
+{
+    return ((const MA7xxSensor *)s)->errors;
+}
+
+static inline void ma7xx_send_angle_cmd(const Sensor *s)
+{
+	ssp_write_one(((const MA7xxSensor *)s)->config.ssp_struct, MA_CMD_ANGLE);
+}
+
+static inline int32_t ma7xx_get_raw_angle(const Sensor *s)
+{
+    return ((const MA7xxSensor *)s)->angle;
+}
+
+static inline void ma7xx_update(Sensor *s, bool check_error)
+{
+    MA7xxSensor *ms = (MA7xxSensor *)s;
+    const int32_t angle = ssp_read_one(ms->config.ssp_struct);
+
+    if (check_error)
+    {
+    	const int32_t delta = ms->angle - angle;
+		if ( ((delta > MA7XX_MAX_ALLOWED_DELTA) || (delta < -MA7XX_MAX_ALLOWED_DELTA)) &&
+		     ((delta > MA7XX_MAX_ALLOWED_DELTA_ADD) || (delta < MA7XX_MIN_ALLOWED_DELTA_ADD)) &&
+		     ((delta > MA7XX_MAX_ALLOWED_DELTA_SUB) || (delta < MA7XX_MIN_ALLOWED_DELTA_SUB)) )
+		{
+            ms->errors |= SENSORS_SETUP_ONBOARD_ERRORS_READING_UNSTABLE;
+		}
+    }
+    ms->angle = angle;
+}
+
+/**
+ * @brief Write to a register of the sensor
+ * 
+ * @param reg The 5-bit register address
+ * @param value The value to write
+ * @return uint16_t 
+ */
+static inline uint16_t ma7xx_write_reg(const Sensor *s, uint8_t reg, uint8_t value)
+{
+    const MA7xxSensor *ms = (const MA7xxSensor *)s;
+    uint16_t cmd = MA_CMD_WRITE | reg << 8 | value;
+    uint32_t result = ssp_write_one(ms->config.ssp_struct, cmd);
+
+    // Delay at least 20ms to let the encoder write to memory
+    delay_us(20100);
+    result |= ssp_write_one(ms->config.ssp_struct, 0);
+
+    // The encoder returns the value written to memory, so check that it is the same as what we wrote
+    uint8_t retval =  ssp_read_one(ms->config.ssp_struct) >> 8;
+    if ((retval != value) || (result != 0))
+    {
+        return false;
+    }
+    return true;
+}
+
+/**
+ * @brief Read from a register of the sensor
+ * 
+ * @param register The 5-bit register address
+ * @return uint8_t 
+ */
+static inline uint8_t ma7xx_read_reg(const Sensor *s, uint8_t reg)
+{
+    const MA7xxSensor *ms = (const MA7xxSensor *)s;
+    uint16_t cmd[2] = {MA_CMD_READ  | (reg << 8), 0};
+    uint16_t result = ssp_write_multi(ms->config.ssp_struct, cmd, 2u);
+    if (result != 0)
+    {
+        return false;
+    }
+    return ssp_read_one(ms->config.ssp_struct) >> 8;
+}
\ No newline at end of file
diff --git a/firmware/src/sensor/sensor.c b/firmware/src/sensor/sensor.c
new file mode 100644
index 00000000..c07c5704
--- /dev/null
+++ b/firmware/src/sensor/sensor.c
@@ -0,0 +1,93 @@
+
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  * 
+//  * This program is free software: you can redistribute it and/or modify  
+//  * it under the terms of the GNU General Public License as published by  
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but 
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License 
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include <src/controller/controller.h>
+#include <src/observer/observer.h>
+#include <src/sensor/sensor.h>
+
+void sensor_reset(Sensor *s)
+{
+    (void)memset(s->config.rec_table, 0, sizeof(s->config.rec_table));
+	s->config.rec_calibrated = false;
+}
+
+bool sensor_calibrate_eccentricity_compensation(Sensor *s, Observer *o, FrameTransform *xf_motor_to_sensor)
+{
+    // Size below is an arbitrary large number ie > ECN_SIZE * npp
+    float error_ticks[ECN_SIZE * MOTOR_MAX_POLE_PAIRS];
+    const int16_t npp = motor_get_pole_pairs();
+    const int16_t n = ECN_SIZE * npp;
+    const int16_t nconv = 100;
+    const float delta = 2 * PI * npp / (n * nconv);
+    const float e_pos_to_ticks = ((float)SENSOR_COMMON_RES_TICKS) / (2 * PI * npp);
+    float e_pos_ref = 0;
+    const float I_setpoint = motor_get_I_cal();
+    int32_t *lut = s->config.rec_table;
+    set_epos_and_wait(e_pos_ref, I_setpoint);
+    wait_pwm_cycles(5000);
+
+    for (uint32_t i = 0; i < n; i++)
+    {
+        for (int16_t j = 0; j < nconv; j++)
+        {
+            e_pos_ref += delta;
+            set_epos_and_wait(e_pos_ref, I_setpoint);
+        }
+        wait_for_control_loop_interrupt();
+        const float pos_meas_sensor_frame = observer_get_pos_estimate(o);
+        error_ticks[i] = apply_transform(e_pos_ref * e_pos_to_ticks, xf_motor_to_sensor) - pos_meas_sensor_frame;
+    }
+    for (uint32_t i = 0; i < n; i++)
+    {
+        for (int16_t j = 0; j < nconv; j++)
+        {
+            e_pos_ref -= delta;
+            set_epos_and_wait(e_pos_ref, I_setpoint);
+        }
+        wait_for_control_loop_interrupt();
+        const float pos_meas_sensor_frame = observer_get_pos_estimate(o);
+        error_ticks[n - i - 1] += apply_transform(e_pos_ref * e_pos_to_ticks, xf_motor_to_sensor) - pos_meas_sensor_frame;
+    }
+    gate_driver_set_duty_cycle(&three_phase_zero);
+    // temporarily disable the inverter, as the following computations will probably exceed a single PWM cycle
+    gate_driver_disable();
+
+    // FIR filtering and map measurements to lut
+    for (int16_t i=0; i<ECN_SIZE; i++)
+    {
+        float acc = 0;
+        for (int16_t j = 0; j < ECN_SIZE; j++)
+        {
+            int32_t read_idx = -ECN_SIZE / 2 + j + i * npp;
+            if (read_idx < 0)
+            {
+                read_idx += n;
+            }
+            else if (read_idx > n - 1)
+            {
+                read_idx -= n;
+            }
+            acc += error_ticks[read_idx];
+        }
+        acc = acc / ((float)(ECN_SIZE * 2));
+        lut[i] = (int32_t)acc;
+    }
+    gate_driver_enable();
+    wait_pwm_cycles(5000);
+    s->config.rec_calibrated = true;
+    return true;
+}
diff --git a/firmware/src/sensor/sensor.h b/firmware/src/sensor/sensor.h
new file mode 100644
index 00000000..2cba9b89
--- /dev/null
+++ b/firmware/src/sensor/sensor.h
@@ -0,0 +1,154 @@
+
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  * 
+//  * This program is free software: you can redistribute it and/or modify  
+//  * it under the terms of the GNU General Public License as published by  
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but 
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License 
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <src/common.h>
+#include <src/xfs.h>
+#include <src/ssp/ssp_func.h>
+#include <src/motor/motor.h>
+
+#if defined(BOARD_REV_R53)
+#define ONBOARD_SENSOR_SSP_PORT SSPC
+#define ONBOARD_SENSOR_SSP_STRUCT PAC55XX_SSPC
+#define EXTERNAL_SENSOR_SSP_PORT SSPD_PD4567
+#define EXTERNAL_SENSOR_SSP_STRUCT PAC55XX_SSPD
+#elif defined BOARD_REV_R5 || defined BOARD_REV_M5
+#define ONBOARD_SENSOR_SSP_PORT SSPC
+#define ONBOARD_SENSOR_SSP_STRUCT PAC55XX_SSPC
+#define EXTERNAL_SENSOR_SSP_PORT SSPD
+#define EXTERNAL_SENSOR_SSP_STRUCT PAC55XX_SSPD
+#elif defined BOARD_REV_R3
+#define ONBOARD_SENSOR_SSP_PORT SSPD
+#define ONBOARD_SENSOR_SSP_STRUCT PAC55XX_SSPD
+#endif
+
+typedef struct Sensor Sensor;
+typedef struct SensorConfig SensorConfig;
+typedef struct Observer Observer;
+
+typedef bool (*sensor_is_calibrated_func_t)(const Sensor *);
+typedef bool (*sensor_calibrate_func_t)(Sensor *, Observer *);
+typedef int32_t (*sensor_get_raw_angle_func_t)(const Sensor *);
+typedef bool (*sensor_init_func_t)(Sensor *);
+typedef void (*sensor_deinit_func_t)(Sensor *);
+typedef void (*sensor_reset_func_t)(Sensor *);
+typedef void (*sensor_prepare_func_t)(const Sensor *);
+typedef void (*sensor_update_func_t)(Sensor *, bool);
+typedef void (*sensor_get_ss_config_func_t)(Sensor* sensor, void* buffer);
+typedef uint8_t (*sensor_get_errors_func_t)(const Sensor *);
+
+typedef enum {
+    SENSOR_TYPE_INVALID = 0,
+    SENSOR_TYPE_MA7XX = 1,
+    SENSOR_TYPE_AS5047 = 2,
+    SENSOR_TYPE_AMT22 = 3,
+    SENSOR_TYPE_HALL = 4,
+    SENSOR_TYPE_MAX
+} sensor_type_t;
+
+typedef enum {
+    SENSOR_CONNECTION_ONBOARD_SPI = 0,
+    SENSOR_CONNECTION_EXTERNAL_SPI = 1,
+    SENSOR_CONNECTION_HALL = 2,
+    SENSOR_CONNECTION_MAX
+} sensor_connection_t;
+
+struct SensorConfig {
+    uint32_t id;
+    sensor_type_t type;
+    int32_t rec_table[ECN_SIZE];
+    bool rec_calibrated;
+};
+
+struct Sensor { // typedefd earlier
+    SensorConfig config;
+    sensor_is_calibrated_func_t is_calibrated_func;
+    sensor_calibrate_func_t calibrate_func;
+    sensor_get_raw_angle_func_t get_raw_angle_func;
+    sensor_init_func_t init_func;
+    sensor_deinit_func_t deinit_func;
+    sensor_reset_func_t reset_func;
+    sensor_update_func_t update_func;
+    sensor_prepare_func_t prepare_func;
+    sensor_get_ss_config_func_t get_ss_config_func;
+    sensor_get_errors_func_t get_errors_func;
+    uint8_t bits;
+    uint32_t ticks;
+    float normalization_factor;
+    bool initialized : 1;
+    bool prepared : 1;
+    bool updated : 1;
+};
+
+void sensor_reset(Sensor *s);
+bool sensor_calibrate_eccentricity_compensation(Sensor *s, Observer *o, FrameTransform *xf_motor_to_sensor);
+
+
+static inline void sensor_update(Sensor *s, bool check_error)
+{
+    if (s->updated == false)
+    {
+        s->update_func(s, check_error);
+        s->updated = true;
+    }
+}
+
+static inline void sensor_invalidate(Sensor *s)
+{
+    s->updated = false;
+    s->prepared = false;
+}
+
+static inline uint8_t sensor_get_bits(const Sensor *s)
+{
+    return s->bits;
+}
+
+static inline uint32_t sensor_get_ticks(const Sensor *s)
+{
+    return s->ticks;
+}
+
+static inline sensor_type_t sensor_get_type(const Sensor *s)
+{
+    return s->config.type;
+}
+
+static inline void sensor_prepare(Sensor *s)
+{
+    if (s->prepare_func && s->prepared == false)
+    {
+        s->prepare_func(s);
+        s->prepared = true;
+    }
+}
+
+static inline int32_t sensor_get_angle_rectified(const Sensor *s)
+{
+    const uint8_t offset_bits = (sensor_get_bits(s) - ECN_BITS);
+    const int32_t angle = s->get_raw_angle_func(s);
+    const int32_t off_1 = s->config.rec_table[angle>>offset_bits];
+	const int32_t off_2 = s->config.rec_table[((angle>>offset_bits) + 1) % ECN_SIZE];
+	const int32_t off_interp = off_1 + ((off_2 - off_1)* (angle - ((angle>>offset_bits)<<offset_bits))>>offset_bits);
+	return angle + off_interp;
+}
+
+static inline float sensor_get_angle_rectified_normalized(const Sensor *s)
+{
+    return sensor_get_angle_rectified(s) * s->normalization_factor;
+}
\ No newline at end of file
diff --git a/firmware/src/sensor/sensors.c b/firmware/src/sensor/sensors.c
new file mode 100644
index 00000000..6cdbda28
--- /dev/null
+++ b/firmware/src/sensor/sensors.c
@@ -0,0 +1,352 @@
+
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  * 
+//  * This program is free software: you can redistribute it and/or modify  
+//  * it under the terms of the GNU General Public License as published by  
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but 
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License 
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include <src/sensor/sensors.h>
+#include <src/observer/observer.h>
+#include <src/controller/controller.h>
+
+GenSensor sensors[SENSOR_COUNT] = {0};
+
+void sensor_make_blank(Sensor *s)
+{
+    // Here we check the sensor connection, either
+    // ONBOARD, EXTERNAL_SPI or EXTERNAL_HALL. We need
+    // to do this here in order to set up the config
+    // struct accordingly.
+    s->initialized = false;
+    sensor_connection_t connection = sensor_get_connection(s);
+    switch (connection)
+    {
+        case SENSOR_CONNECTION_ONBOARD_SPI:
+            ma7xx_make_blank_sensor(s);
+            break;
+        case SENSOR_CONNECTION_EXTERNAL_SPI:
+            switch (s->config.type)
+            {
+                case SENSOR_TYPE_MA7XX:
+                    ma7xx_make_blank_sensor(s);
+                    break;
+                case SENSOR_TYPE_AS5047:
+                    as5047p_make_blank_sensor(s);
+                    break;
+                case SENSOR_TYPE_AMT22:
+                    amt22_make_blank_sensor(s);
+                    break;
+                default:
+                    ma7xx_make_blank_sensor(s);
+                    break;
+            }
+            break;
+        case SENSOR_CONNECTION_HALL:
+            hall_make_blank_sensor(s);
+            break;
+        default:
+            break;
+    }
+}
+
+bool sensor_init_with_defaults(Sensor *s)
+{
+    sensor_make_blank(s);
+    // Here we check the sensor connection, either
+    // ONBOARD, EXTERNAL_SPI or EXTERNAL_HALL. We need
+    // to do this here in order to set up the config
+    // struct accordingly.
+    sensor_connection_t connection = sensor_get_connection(s);
+    switch (connection)
+    {
+        case SENSOR_CONNECTION_ONBOARD_SPI:
+            return ma7xx_init_with_port_and_rate(s, ONBOARD_SENSOR_SSP_PORT, ONBOARD_SENSOR_SSP_STRUCT, SENSORS_SETUP_EXTERNAL_SPI_RATE_12Mbps);
+            break;
+#if defined BOARD_REV_R5 || defined BOARD_REV_M5
+        case SENSOR_CONNECTION_EXTERNAL_SPI:
+            switch (s->config.type)
+            {
+                case SENSOR_TYPE_MA7XX:
+                    return ma7xx_init_with_port_and_rate(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT, SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps);
+                    break;
+                case SENSOR_TYPE_AS5047:
+                    return as5047p_init_with_port_and_rate(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT, SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps);
+                    break;
+                case SENSOR_TYPE_AMT22:
+                    return amt22_init_with_port_and_rate(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT, SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps);
+                    break;
+                default:
+                    break;
+            }
+            break;
+
+        case SENSOR_CONNECTION_HALL:
+            return hall_init_with_defaults(s);
+            break;
+#endif
+        default:
+            break;
+    }
+    return false;
+}
+
+bool sensor_init_with_configs(Sensor *s, SensorConfig *sc, GenSensorConfig *gsc)
+{
+    sensor_make_blank(s);
+    switch (sc->type)
+    {
+        case SENSOR_TYPE_MA7XX:
+            return ma7xx_init_with_config(s, &(gsc->ma7xx_sensor_config));
+            break;
+        case SENSOR_TYPE_HALL:
+            return hall_init_with_config(s, &(gsc->hall_sensor_config));
+            break;
+        case SENSOR_TYPE_AS5047:
+            return as5047p_init_with_config(s, &(gsc->as5047p_sensor_config));
+            break;
+        case SENSOR_TYPE_AMT22:
+            return amt22_init_with_config(s, &(gsc->amt22_sensor_config));
+            break;
+        default:
+            break;
+    }
+    return false;
+}
+
+void sensors_init_with_defaults(void)
+{
+    sensor_init_with_defaults(&(sensors[0].sensor));
+    sensor_make_blank(&(sensors[1].sensor));
+    sensor_make_blank(&(sensors[2].sensor));
+    commutation_sensor_p = &(sensors[0].sensor);
+    position_sensor_p = &(sensors[0].sensor);
+}
+
+void sensors_get_config(SensorsConfig *config_)
+{
+    // Serialize SensorConfig array
+    for (int i = 0; i < SENSOR_COUNT; ++i)
+    {
+        config_->config[i] = sensors[i].sensor.config;
+    }
+
+    // Serialize GenSensorConfig array
+    for (int i = 0; i < SENSOR_COUNT; ++i)
+    {
+        sensors[i].sensor.get_ss_config_func(&(sensors[i].sensor), &(config_->ss_config[i]));
+    }
+    config_->commutation_connection = sensor_get_connection(commutation_sensor_p);
+    config_->position_connection = sensor_get_connection(position_sensor_p);
+}
+
+void sensors_restore_config(SensorsConfig *config_)
+{
+    sensor_set_pointer_with_connection(&commutation_sensor_p, config_->commutation_connection);
+    sensor_set_pointer_with_connection(&position_sensor_p, config_->position_connection);
+
+    // Restore SensorConfig array
+    for (int i = 0; i < SENSOR_COUNT; ++i)
+    {
+        sensors[i].sensor.config = config_->config[i];
+        if ((sensors[i].sensor.initialized == false) 
+            && (commutation_sensor_p == &(sensors[i].sensor)
+            || position_sensor_p == &(sensors[i].sensor)))
+        {
+            sensor_init_with_configs(&(sensors[i].sensor), &(config_->config[i]), &(config_->ss_config[i]));
+        }
+        else if (sensors[i].sensor.initialized == false)
+        {
+            sensor_make_blank(&(sensors[i].sensor));
+        }
+    }
+}
+
+void commutation_sensor_set_connection(sensor_connection_t new_connection)
+{
+    sensor_set_connection(&(commutation_sensor_p), &(position_sensor_p), new_connection);
+}
+
+void position_sensor_set_connection(sensor_connection_t new_connection)
+{
+    sensor_set_connection(&(position_sensor_p), &(commutation_sensor_p), new_connection);
+}
+
+void sensor_external_spi_set_type_avlos(sensors_setup_external_spi_type_options type)
+{
+    if (type < SENSORS_SETUP_EXTERNAL_SPI_TYPE__MAX
+        && (controller_get_state() == CONTROLLER_STATE_IDLE))
+    {
+        sensor_type_t internal_type;
+        switch (type)
+        {
+            case SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX:
+                internal_type = SENSOR_TYPE_MA7XX;
+                break;
+            case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AS5047:
+                internal_type = SENSOR_TYPE_AS5047;
+                break;
+            case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AMT22:
+                internal_type = SENSOR_TYPE_AMT22;
+                break;
+            default:
+                internal_type = SENSOR_TYPE_MA7XX;
+                break;
+        }
+        frames_reset_calibrated();
+        
+        Sensor *s = &(sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor);
+        if (s->initialized)
+        {
+            s->deinit_func(s);
+        }
+        sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type = internal_type;
+        sensor_init_with_defaults(s);
+    }
+}
+
+void sensor_external_spi_set_rate_avlos(sensors_setup_external_spi_rate_options rate)
+{
+    if (rate < SENSORS_SETUP_EXTERNAL_SPI_RATE__MAX
+        && (controller_get_state() == CONTROLLER_STATE_IDLE))
+    {
+        sensors_setup_external_spi_rate_options current_rate;
+        switch (sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type)
+        {
+            case SENSOR_TYPE_MA7XX:
+                current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].ma7xx_sensor.config.rate;
+                break;
+            case SENSOR_TYPE_AS5047:
+                current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].as5047p_sensor.config.rate;
+                break;
+            case SENSOR_TYPE_AMT22:
+                current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].amt22_sensor.config.rate;
+                break;
+            default:
+                current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].ma7xx_sensor.config.rate;
+                break;
+        }
+        if (rate != current_rate)
+        {
+            sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.deinit_func(&(sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor));
+            switch (sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type)
+            {
+                case SENSOR_TYPE_MA7XX:
+                    sensors[SENSOR_CONNECTION_EXTERNAL_SPI].ma7xx_sensor.config.rate = rate;
+                    break;
+                case SENSOR_TYPE_AS5047:
+                    sensors[SENSOR_CONNECTION_EXTERNAL_SPI].as5047p_sensor.config.rate = rate;
+                    break;
+                case SENSOR_TYPE_AMT22:
+                    current_rate = sensors[SENSOR_CONNECTION_EXTERNAL_SPI].amt22_sensor.config.rate = rate;
+                    break;
+                default:
+                    break;
+            }
+            sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.init_func(&(sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor));
+        }
+    }
+}
+
+void sensor_set_connection(Sensor** target_sensor_p, Sensor** other_sensor_p, sensor_connection_t new_connection)
+{
+    if (controller_get_state() == CONTROLLER_STATE_IDLE
+        && new_connection != sensor_get_connection(*target_sensor_p) 
+        && new_connection >= 0 && new_connection < SENSOR_CONNECTION_MAX)
+    {
+        frames_reset_calibrated();
+
+        if (sensor_get_connection(*target_sensor_p) != sensor_get_connection(*other_sensor_p))
+        {
+            observer_reset_state(observer_get_for_sensor(*target_sensor_p));
+            (*target_sensor_p)->deinit_func(*target_sensor_p);
+        }
+
+        *target_sensor_p = &(sensors[new_connection].sensor);
+
+        if (new_connection != sensor_get_connection(*other_sensor_p))
+        {
+            observer_reset_state(observer_get_for_sensor(*target_sensor_p));
+            sensor_init_with_defaults(*target_sensor_p);
+        }
+    }
+}
+
+bool sensors_calibrate_pole_pair_count_and_transforms(void)
+{
+    observer_reset_state(&commutation_observer);
+    observer_reset_state(&position_observer);
+
+    const float I_setpoint = motor_get_I_cal();
+    set_epos_and_wait(0, I_setpoint);
+    wait_pwm_cycles(CAL_STAY_LEN);
+
+    const float motor_frame_end = CAL_PHASE_TURNS * TWOPI;
+    const float commutation_frame_start = commutation_observer_get_pos_estimate();
+    const float position_frame_start = position_observer_get_pos_estimate();
+
+    // Move to end
+    for (uint32_t i = 0; i < CAL_DIR_LEN; i++)
+    {
+        set_epos_and_wait(motor_frame_end * ((float)i / CAL_DIR_LEN), I_setpoint);
+    }
+
+    set_epos_and_wait(motor_frame_end, I_setpoint);
+    wait_pwm_cycles(CAL_STAY_LEN);
+    const float commutation_frame_end = commutation_observer_get_pos_estimate();
+    const float position_frame_end = position_observer_get_pos_estimate();
+
+    // Find pole pairs if not Hall; otherwise have to be defined manually
+    if (sensor_get_type(commutation_sensor_p) != SENSOR_TYPE_HALL)
+    {
+        if (!motor_find_pole_pairs(SENSOR_COMMON_RES_TICKS, commutation_frame_start, commutation_frame_end, motor_frame_end))
+        {
+            return false;
+        }
+    }
+
+    const float motor_frame_end_ticks = SENSOR_COMMON_RES_TICKS * (motor_frame_end / (TWOPI * motor_get_pole_pairs()));
+
+    // Move back to start epos
+    for (uint32_t i = 0; i < CAL_DIR_LEN; i++)
+    {
+        set_epos_and_wait(motor_frame_end * (1.0f - ((float)i / CAL_DIR_LEN)), I_setpoint);
+    }
+    gate_driver_set_duty_cycle(&three_phase_zero);
+
+    // Derive transforms
+    if (sensor_get_type(commutation_sensor_p) != SENSOR_TYPE_HALL)
+    {
+        frame_set_commutation_sensor_to_motor(derive_dir_transform_2p(commutation_frame_start, 0, commutation_frame_end, motor_frame_end_ticks));
+    }
+    else // commutation sensor is Hall
+    {
+        const FrameTransform t = {.offset = 0.0f, .multiplier = 1.0f};
+        frame_set_commutation_sensor_to_motor(t);
+    }
+
+    if (commutation_sensor_p == position_sensor_p) // same sensor
+    {
+        frame_set_position_sensor_to_motor(*(frame_commutation_sensor_to_motor_p()));
+    }
+    else if (sensor_get_type(position_sensor_p) != SENSOR_TYPE_HALL) // different sensors, not Hall
+    {
+        frame_set_position_sensor_to_motor(derive_transform_2p(position_frame_start, 0, position_frame_end, motor_frame_end_ticks));
+    }
+    else // position sensor is Hall
+    {
+        const FrameTransform t = {.offset = 0.0f, .multiplier = 1.0f};
+        frame_set_position_sensor_to_motor(t);
+    }
+
+    return true;
+}
\ No newline at end of file
diff --git a/firmware/src/sensor/sensors.h b/firmware/src/sensor/sensors.h
new file mode 100644
index 00000000..b9af4439
--- /dev/null
+++ b/firmware/src/sensor/sensors.h
@@ -0,0 +1,212 @@
+
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  * 
+//  * This program is free software: you can redistribute it and/or modify  
+//  * it under the terms of the GNU General Public License as published by  
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but 
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License 
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <src/xfs.h>
+#include <src/sensor/ma7xx.h>
+#include <src/sensor/hall.h>
+#include <src/sensor/as5047.h>
+#include <src/sensor/amt22.h>
+#include <src/sensor/sensor.h>
+
+#define SENSOR_COUNT 3
+
+typedef struct Observer Observer;
+
+extern Observer commutation_observer;
+extern Observer position_observer;
+
+typedef union 
+{
+    Sensor sensor;
+    MA7xxSensor ma7xx_sensor;
+    HallSensor hall_sensor;
+    AS5047PSensor as5047p_sensor;
+    AMT22Sensor amt22_sensor;
+} GenSensor;
+
+typedef union 
+{
+    MA7xxSensorConfig ma7xx_sensor_config;
+    HallSensorConfig hall_sensor_config;
+    AS5047PSensorConfig as5047p_sensor_config;
+    AMT22SensorConfig amt22_sensor_config;
+} GenSensorConfig;
+
+typedef struct
+{
+    SensorConfig config[SENSOR_COUNT];
+    GenSensorConfig ss_config[SENSOR_COUNT];
+    sensor_connection_t commutation_connection;
+    sensor_connection_t position_connection;
+} SensorsConfig;
+
+// The sequence in the `sensors` array is determined so that
+// 0: onboard sensor, 1: external spi and 2: hall sensor
+// index same as the members of `sensor_connection_t`
+extern GenSensor sensors[SENSOR_COUNT];
+
+Sensor *commutation_sensor_p;
+Sensor *position_sensor_p;
+
+static inline sensor_connection_t sensor_get_connection(const Sensor *sensor)
+{
+    // Derive pointer array offset
+    const GenSensor *s = (const GenSensor *)sensor;
+    return s - sensors;
+}
+
+static inline void sensor_set_pointer_with_connection(Sensor **sensor, sensor_connection_t connection)
+{
+    *sensor = (Sensor *)(&(sensors[connection]));
+}
+
+void sensor_set_connection(Sensor** target_sensor, Sensor** other_sensor, sensor_connection_t new_connection);
+
+bool sensor_init_with_defaults(Sensor *s);
+void sensors_init_with_defaults(void);
+void sensors_get_config(SensorsConfig *config_);
+void sensors_restore_config(SensorsConfig *config_);
+
+static inline sensor_connection_t commutation_sensor_get_connection(void)
+{
+    return sensor_get_connection(commutation_sensor_p);
+}
+
+void commutation_sensor_set_connection(sensor_connection_t new_connection);
+
+static inline sensor_connection_t position_sensor_get_connection(void)
+{
+    return sensor_get_connection(position_sensor_p);
+}
+
+void position_sensor_set_connection(sensor_connection_t new_connection);
+bool sensors_calibrate_pole_pair_count_and_transforms(void);
+
+static inline void sensors_reset(void)
+{
+    for (int i=0; i<SENSOR_COUNT; i++)
+    {
+        Sensor *s = &(sensors[i].sensor);
+        s->reset_func(s);
+    }
+}
+
+static inline void sensors_calibrate(void)
+{
+    if (position_sensor_p->calibrate_func)
+    {
+        position_sensor_p->calibrate_func(position_sensor_p, &position_observer);
+    }
+    if ((commutation_sensor_p != position_sensor_p) && (commutation_sensor_p->calibrate_func))
+    {
+        commutation_sensor_p->calibrate_func(commutation_sensor_p, &commutation_observer);
+    }
+
+    sensors_calibrate_pole_pair_count_and_transforms();
+
+    if (sensor_get_type(commutation_sensor_p) != SENSOR_TYPE_HALL)
+    {
+        sensor_calibrate_eccentricity_compensation(commutation_sensor_p, &commutation_observer, frame_motor_to_commutation_sensor_p());
+    }
+    if (commutation_sensor_p != position_sensor_p && sensor_get_type(position_sensor_p) != SENSOR_TYPE_HALL)
+    {
+        sensor_calibrate_eccentricity_compensation(position_sensor_p, &position_observer, frame_motor_to_position_sensor_p());
+    }
+}
+
+static inline sensors_setup_external_spi_type_options sensor_external_spi_get_type_avlos(void)
+{
+    switch (sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type)
+    {
+        case SENSOR_TYPE_MA7XX:
+            return SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX;
+            break;
+        case SENSOR_TYPE_AS5047:
+            return SENSORS_SETUP_EXTERNAL_SPI_TYPE_AS5047;
+            break;
+        case SENSOR_TYPE_AMT22:
+            return SENSORS_SETUP_EXTERNAL_SPI_TYPE_AMT22;
+            break;
+        default:
+            return SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX;
+            break;
+    }
+}
+
+static inline sensors_setup_external_spi_rate_options sensor_external_spi_get_rate_avlos(void)
+{
+    switch (sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type)
+    {
+        case SENSOR_TYPE_MA7XX:
+            return sensors[SENSOR_CONNECTION_EXTERNAL_SPI].ma7xx_sensor.config.rate;
+            break;
+        case SENSOR_TYPE_AS5047:
+            return sensors[SENSOR_CONNECTION_EXTERNAL_SPI].as5047p_sensor.config.rate;
+            break;
+        case SENSOR_TYPE_AMT22:
+            return sensors[SENSOR_CONNECTION_EXTERNAL_SPI].amt22_sensor.config.rate;
+            break;
+        default:
+            return sensors[SENSOR_CONNECTION_EXTERNAL_SPI].ma7xx_sensor.config.rate;
+            break;
+    }
+}
+
+void sensor_external_spi_set_type_avlos(sensors_setup_external_spi_type_options type);
+void sensor_external_spi_set_rate_avlos(sensors_setup_external_spi_rate_options rate);
+
+static inline bool sensor_onboard_get_is_calibrated(void)
+{
+    return sensors[SENSOR_CONNECTION_ONBOARD_SPI].sensor.is_calibrated_func(&(sensors[SENSOR_CONNECTION_ONBOARD_SPI].sensor));
+}
+
+static inline bool sensor_external_spi_get_is_calibrated(void)
+{
+    return sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.is_calibrated_func(&(sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor));
+}
+
+static inline bool sensor_hall_get_is_calibrated(void)
+{
+    return sensors[SENSOR_CONNECTION_HALL].sensor.is_calibrated_func(&(sensors[SENSOR_CONNECTION_HALL].sensor));
+}
+
+static inline uint8_t sensor_onboard_get_errors(void)
+{
+    return sensors[SENSOR_CONNECTION_ONBOARD_SPI].sensor.get_errors_func(&(sensors[SENSOR_CONNECTION_ONBOARD_SPI].sensor));
+}
+
+static inline uint8_t sensor_external_spi_get_errors(void)
+{
+    return sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.get_errors_func(&(sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor));
+}
+
+static inline uint8_t sensor_hall_get_errors(void)
+{
+    return sensors[SENSOR_CONNECTION_HALL].sensor.get_errors_func(&(sensors[SENSOR_CONNECTION_HALL].sensor));
+}
+
+static inline int32_t sensor_commutation_get_raw_angle(void)
+{
+    return commutation_sensor_p->get_raw_angle_func(commutation_sensor_p);
+}
+
+static inline int32_t sensor_position_get_raw_angle(void)
+{
+    return position_sensor_p->get_raw_angle_func(position_sensor_p);
+}
diff --git a/firmware/src/ssp/ssp_func.c b/firmware/src/ssp/ssp_func.c
index e38a9cec..c05aefc3 100644
--- a/firmware/src/ssp/ssp_func.c
+++ b/firmware/src/ssp/ssp_func.c
@@ -598,6 +598,26 @@ void SSPD_IO_Select_PG4567(SSP_MS_TYPE ms_mode)
     PAC55XX_SCC->PGMUXSEL.w |= 0x55550000;                      // Set Port Pin as SSP
 }
 
+// void SSPD_IO_Select_PD45PF45(SSP_MS_TYPE ms_mode)
+// {
+//     if (ms_mode == SSP_MS_MASTER)
+//     {
+//         PAC55XX_GPIOD->MODE.P4 = IO_PUSH_PULL_OUTPUT;           // MOSI
+//         PAC55XX_GPIOD->MODE.P5 = IO_HIGH_IMPEDENCE_INPUT;       // MISO
+//         PAC55XX_GPIOF->MODE.P4 = IO_PUSH_PULL_OUTPUT;           // SCLK
+//         PAC55XX_GPIOF->MODE.P5 = IO_PUSH_PULL_OUTPUT;           // SS
+
+//         PAC55XX_SCC->PDPUEN.P4 = 0;
+//         PAC55XX_SCC->PDPUEN.P5 = 1;
+//         PAC55XX_SCC->PFPUEN.P4 = 0;
+//         PAC55XX_SCC->PFPUEN.P5 = 0;
+//     }
+//     PAC55XX_SCC->PDMUXSEL.w &= 0xFFFF00FF;                      // Clear Port Pin selection
+//     PAC55XX_SCC->PDMUXSEL.w |= 0x00007700;                      // Set Port Pin as SSP
+//     PAC55XX_SCC->PFMUXSEL.w &= 0xFFFF00FF;                      // Clear Port Pin selection
+//     PAC55XX_SCC->PFMUXSEL.w |= 0x00005500;                      // Set Port Pin as SSP
+// }
+
 //==============================================================================
 ///@brief
 ///     Choose the SSP and Enable the IO you want
@@ -641,13 +661,17 @@ void ssp_io_config(SSP_TYPE ssp, SSP_MS_TYPE ms_mode)
 
         case SSPD:
             // Select ssp D peripheral choose one!
-            SSPD_IO_Select_PD4567(ms_mode);
 //            SSPD_IO_Select_PE4567(ms_mode);
-//            SSPD_IO_Select_PF4567(ms_mode);
+            SSPD_IO_Select_PF4567(ms_mode);
 //            SSPD_IO_Select_PG0123(ms_mode);
 //            SSPD_IO_Select_PG4567(ms_mode);
             break;
 
+        case SSPD_PD4567:
+            SSPD_IO_Select_PD4567(ms_mode);
+
+            break;
+
         default:
             break;
     }
@@ -698,7 +722,48 @@ void ssp_interrupt_enable(SSP_TYPE ssp)
     }
 }
 
-void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t cph, uint8_t cpol)
+//==============================================================================
+///@brief
+///     disable the interrupt
+///
+///@param[in]
+///     SSP Type:
+///         SSPA
+///         SSPB
+///         SSPC
+///         SSPD
+///
+//==============================================================================
+void ssp_interrupt_disable(SSP_TYPE ssp)
+{
+    switch (ssp)
+    {
+        case SSPA:
+            NVIC_ClearPendingIRQ(USARTA_IRQn);
+            NVIC_DisableIRQ(USARTA_IRQn);
+            break;
+
+        case SSPB:
+            NVIC_ClearPendingIRQ(USARTB_IRQn);
+            NVIC_DisableIRQ(USARTB_IRQn);
+            break;
+
+        case SSPC:
+            NVIC_ClearPendingIRQ(USARTC_IRQn);
+            NVIC_DisableIRQ(USARTC_IRQn);
+            break;
+
+        case SSPD:
+            NVIC_ClearPendingIRQ(USARTD_IRQn);
+            NVIC_DisableIRQ(USARTD_IRQn);
+            break;
+
+        default:
+            break;
+    }
+}
+
+void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t clkn_div, uint32_t data_size, SSP_SWSEL_TYPE swsel_type, uint8_t cph, uint8_t cpol)
 {
     PAC55XX_SSP_TYPEDEF *ssp_ptr;
 
@@ -736,15 +801,16 @@ void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t cph, uint8_t cpol)
     // SSPCLK = PCLK / ((SSPxCLK.M + 1)*SSPxCLK.N) = 150000000 / ((2+1) x 6) = 8.47MHz
     // SSPCLK = PCLK / ((SSPxCLK.M + 1)*SSPxCLK.N) = 150000000 / ((2+1) x 4) = 12.5MHz
 	ssp_ptr->CLK.M = 2;
-	ssp_ptr->CLK.N = 4;                                     // N must be even value from 2 to 254
+	ssp_ptr->CLK.N = clkn_div;                                     // N must be even value from 2 to 254
 	ssp_ptr->CON.FRF = SSP_FRAME_FORMAT_SPI;                 // Frame Format, SPI frame format
 	ssp_ptr->CON.MS = ms_mode;                               // Master/Slave mode, master mode
 	ssp_ptr->CON.LSBFIRST = SSP_ENDIAN_MSB;                  // Endian Order, MSB transmit 1st
 	ssp_ptr->CON.LBM = SSP_LP_NORMAL;                        // Loopback Mode, no loopback mode
-	ssp_ptr->CON.CPH = cph;                      // Clock Out Phase, SPI captures data at 2nd edge transition of the frame
-	ssp_ptr->CON.CPO = cpol;                // Clock Out Polarity, SPI clock active high
-    ssp_ptr->CON.DSS = SSP_DATA_SIZE_16;                     // Data Size Select, 16 bit data
+	ssp_ptr->CON.CPH = cph;                                  // Clock Out Phase
+	ssp_ptr->CON.CPO = cpol;                                 // Clock Out Polarity
+    ssp_ptr->CON.DSS = data_size;                            // Data Size Select
     ssp_ptr->CON.SOD = SSP_OUTPUT_NOT_DRIVE;                 // Slave Output Disable
+    ssp_ptr->SSCR.SWSEL = swsel_type;                             // Device Select Pin to be controlled by SPI (0) or software (1)
 
     ssp_io_config(ssp, ms_mode);
     ssp_interrupt_enable(ssp);
@@ -760,6 +826,36 @@ void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t cph, uint8_t cpol)
     ssp_ptr->CON.SSPEN = SSP_CONTROL_ENABLE;                 // SSP Enable
 }
 
+void ssp_deinit(SSP_TYPE ssp)
+{
+    PAC55XX_SSP_TYPEDEF *ssp_ptr;
+
+    switch (ssp)
+    {
+        case SSPA:
+            ssp_ptr = PAC55XX_SSPA;
+            break;
+
+        case SSPB:
+            ssp_ptr = PAC55XX_SSPB;
+            break;
+
+        case SSPC:
+            ssp_ptr = PAC55XX_SSPC;
+            break;
+
+        case SSPD:
+            ssp_ptr = PAC55XX_SSPD;
+            break;
+
+        default:
+            ssp_ptr = PAC55XX_SSPD;
+            break;
+    }
+    ssp_interrupt_disable(ssp);
+    ssp_ptr->CON.SSPEN = SSP_CONTROL_DISABLE;
+}
+
 //==============================================================================
 ///@brief
 ///     Write a 16-bit data to SSP manually
diff --git a/firmware/src/ssp/ssp_func.h b/firmware/src/ssp/ssp_func.h
index 587fc79c..24e541a1 100644
--- a/firmware/src/ssp/ssp_func.h
+++ b/firmware/src/ssp/ssp_func.h
@@ -16,22 +16,38 @@
 
 #include "src/common.h"
 
+// SPI mode Clock polarity  Clock phase
+//          (CPOL)          (CPHA)	
+// 0	    0              	0	
+// 1	    0	            1	
+// 2	    1	            0	
+// 3	    1	            1	
+
 #define DF_SSP_BUSY_TICK (25000u)    // use to check the busy tick
 
+// Device Select Enumeration Type
+typedef enum
+{
+    SWSEL_SPI = 0,
+    SWSEL_SW = 1
+} SSP_SWSEL_TYPE;
+
 // Interrupt Enable Enumeration Type
 typedef enum
 {
     SSPA = 0,
     SSPB = 1,
     SSPC = 2,
-    SSPD = 3
+    SSPD = 3,
+    SSPD_PD4567 = 4
 } SSP_TYPE;
 
 
 volatile uint16_t ssp_data[10];
 volatile uint16_t data_num;
 
-extern void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t cph, uint8_t cpol);
+extern void ssp_init(SSP_TYPE ssp, SSP_MS_TYPE ms_mode, uint8_t clkn_div, uint32_t data_size, uint8_t swsel, uint8_t cph, uint8_t cpol);
+extern void ssp_deinit(SSP_TYPE ssp);
 extern uint32_t ssp_write_one(PAC55XX_SSP_TYPEDEF *ssp_ptr, uint16_t data);
 extern uint32_t ssp_write_multi(PAC55XX_SSP_TYPEDEF *ssp_ptr, uint16_t *data, uint32_t byte_num);
 extern uint16_t ssp_read_one(PAC55XX_SSP_TYPEDEF *ssp_ptr);
diff --git a/firmware/src/system/system.c b/firmware/src/system/system.c
index 2a5e4d0b..c212aee2 100644
--- a/firmware/src/system/system.c
+++ b/firmware/src/system/system.c
@@ -19,12 +19,13 @@
 #include <src/utils/utils.h>
 #include <src/rtt/SEGGER_RTT.h>
 #include <src/controller/controller.h>
-#include <src/encoder/encoder.h>
 #include <src/motor/motor.h>
 #include <src/can/can_endpoints.h>
+#include <src/sensor/sensor.h>
+#include <src/observer/observer.h>
 #include <src/system/system.h>
 
-extern char _eram;
+__attribute__((section(".shared"))) volatile uint32_t shared_data;
 
 static SystemState state = {0};
 
@@ -82,9 +83,16 @@ void system_init(void)
     // Vp = 10V , 440mA-540mA, Charge Pump Enable
     pac5xxx_tile_register_write(ADDR_SYSCONF, 0x01);
 
-    // Ensure ADC GP0 register is zero, to bypass DFU mode on next boot
+    // Ensure shared data is 0xFF and ADC GP0 register
+    // is zero, to bypass DFU mode on next boot
+    shared_data = 0xFF;
     pac5xxx_tile_register_write(ADDR_GP0, 0);
 
+    // Configure reporting of mcu cycles
+    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+    DWT->CYCCNT = 0;
+    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+
     // Configure error handling
     SCB->CCR |= 0x10;
 
@@ -105,33 +113,31 @@ TM_RAMFUNC void system_update(void)
     {
         state.errors |= ERRORS_UNDERVOLTAGE;
     }
-    // const uint8_t drv_status = pac5xxx_tile_register_read(ADDR_STATDRV);
-    // if (drv_status > 0)
-    // {
-    //     state.errors |= ((drv_status & 0x56) << 1);
-    // }
-    // const uint8_t drv_fault = pac5xxx_tile_register_read(ADDR_DRV_FLT);
-    // if (drv_fault > 0)
-    // {
-    //     // We use 0x5 mask because we ignore CHARGE_PUMP_FAULT_STAT
-    //     // for the time being, as it seems to always be set, and
-    //     // ERRORS_DRVXX_DISABLE, as it seems to give spurious errors.
-    //     state.errors |= ((drv_fault & 0x5) << 1);
-    // }
 }
 
 void system_reset(void)
 {
-    // GP0 register is already zeroed at `system_init()` 
+    // shared RAM and GP0 register is already zeroed at `system_init()` 
     NVIC_SystemReset();
 }
 
 void system_enter_dfu(void)
 {
+    shared_data = BTL_TRIGGER_PATTERN_RAM;
     pac5xxx_tile_register_write(ADDR_GP0, BTL_TRIGGER_PATTERN);
+
     NVIC_SystemReset();
 }
 
+void system_reset_calibration(void)
+{
+    ADC_reset();
+    sensors_reset();
+    observers_init_with_defaults();
+    motor_reset_calibration();
+    wait_pwm_cycles(5000);
+}
+
 TM_RAMFUNC float system_get_Vbus(void)
 {
     return state.Vbus;
@@ -139,7 +145,10 @@ TM_RAMFUNC float system_get_Vbus(void)
 
 TM_RAMFUNC bool system_get_calibrated(void)
 {
-    return motor_get_calibrated() & encoder_get_calibrated();
+    return (frames_get_calibrated() &&
+            motor_get_calibrated() &&
+            commutation_sensor_p->is_calibrated_func(commutation_sensor_p) &&
+            position_sensor_p->is_calibrated_func(position_sensor_p));
 }
 
 TM_RAMFUNC uint8_t system_get_errors(void)
@@ -147,12 +156,24 @@ TM_RAMFUNC uint8_t system_get_errors(void)
     return state.errors;
 }
 
+TM_RAMFUNC uint8_t system_get_warnings(void)
+{
+    // As the user request rate is less than the update
+    // rate, and the warnings do not affect system integrity
+    // evaluation, it is better to fetch the registers per
+    // request
+    const uint8_t warnings = ((pac5xxx_tile_register_read(ADDR_STATDRV) & 0x56)) |
+                             ((pac5xxx_tile_register_read(ADDR_DRV_FLT) & 0x5));
+    return warnings;
+}
+
 TM_RAMFUNC bool errors_exist(void)
 {
-    return (controller_get_errors() | 
-            encoder_get_errors() | 
-            motor_get_errors() | 
-            planner_get_errors() | 
+    return (controller_get_errors() ||
+            commutation_sensor_p->get_errors_func(commutation_sensor_p) ||
+            position_sensor_p->get_errors_func(position_sensor_p) ||
+            motor_get_errors() ||
+            planner_get_errors() ||
             system_get_errors());
 }
 
diff --git a/firmware/src/system/system.h b/firmware/src/system/system.h
index 5ddf404b..7ebaa358 100644
--- a/firmware/src/system/system.h
+++ b/firmware/src/system/system.h
@@ -37,6 +37,9 @@ void system_init(void);
 void system_update(void);
 void system_reset(void);
 void system_enter_dfu(void);
+void system_reset_calibration(void);
+
+extern const uint32_t config_size;
 
 static inline uint8_t system_get_fw_version_string(char *buffer)
 {
@@ -45,19 +48,26 @@ static inline uint8_t system_get_fw_version_string(char *buffer)
     return size;
 }
 
-inline uint32_t system_get_uid(void)
+static inline uint32_t system_get_uid(void)
 {
     return PAC55XX_INFO1->UNIQUEID[0] ^ PAC55XX_INFO1->UNIQUEID[1] ^ PAC55XX_INFO1->UNIQUEID[2];
 }
 
-inline uint32_t system_get_hw_revision(void)
+static inline uint32_t system_get_hw_revision(void)
 {
     return BOARD_REV_IDX;
 }
 
+static inline uint32_t system_get_config_size(void)
+{
+    return config_size;
+}
+
+void system_reset_calibration(void);
 float system_get_Vbus(void);
 bool system_get_calibrated(void);
 uint8_t system_get_errors(void);
+uint8_t system_get_warnings(void);
 bool errors_exist(void);
 
 #endif /* SYSTEM_SYSTEM_H_ */
diff --git a/firmware/src/timer/timer.c b/firmware/src/timer/timer.c
index 78d84361..3aa255ac 100644
--- a/firmware/src/timer/timer.c
+++ b/firmware/src/timer/timer.c
@@ -18,22 +18,28 @@
 #include "src/common.h"
 #include "timer.h"
 
-void Timer_Init(void)
+void timers_init(void)
 {
-    // Configure Timer A Controls
-    pac5xxx_timer_clock_config(TimerA, TXCTL_CS_ACLK, TXCTL_PS_DIV);                   // Configure timer clock input for ACLK, divider
+    // Timer A -- PWM
+
+    // Timer clock input for ACLK, divider
+    pac5xxx_timer_clock_config(TimerA, TXCTL_CS_ACLK, TXCTL_PS_DIV);       
+    // Timer frequency and count mode            
     pac5xxx_timer_base_config(TimerA, (TIMER_FREQ_HZ/(2*PWM_FREQ_HZ)), AUTO_RELOAD,
-            TxCTL_MODE_UPDOWN, TIMER_SLAVE_SYNC_DISABLE);                               // Configure timer frequency and count mode
+            TxCTL_MODE_UPDOWN, TIMER_SLAVE_SYNC_DISABLE);                               
 
     // Configure Dead time generators
-    PAC55XX_TIMERA->CTL.DTGCLK = BEFORE_ACLK_DIVIDER;                                   // 0--> The DTGCLK is the clock before the TACTL.CLKDIV clock divider.
-                                                                                        // 1--> The DTGCLK is the clock after the TACTL.CLKDIV clock divider.
-
-    pac5xxx_dtg_config2(&(PAC55XX_TIMERA->DTGCTL0), RED_DEATH_TIMET, FED_DEATH_TIMET);  // Configure DTGA0 for phase U
-    pac5xxx_dtg_config2(&(PAC55XX_TIMERA->DTGCTL1), RED_DEATH_TIMET, FED_DEATH_TIMET);  // Configure DTGA1 for phase V
-    pac5xxx_dtg_config2(&(PAC55XX_TIMERA->DTGCTL2), RED_DEATH_TIMET, FED_DEATH_TIMET);  // Configure DTGA2 for phase W
+    // 0--> The DTGCLK is the clock before the TACTL.CLKDIV clock divider.
+    // 1--> The DTGCLK is the clock after the TACTL.CLKDIV clock divider.
+    PAC55XX_TIMERA->CTL.DTGCLK = BEFORE_ACLK_DIVIDER;                                   
+                                                                                        
+    // Configure DTGA0, 1, 2 for phase U, V, W
+    pac5xxx_dtg_config2(&(PAC55XX_TIMERA->DTGCTL0), RED_DEATH_TIMET, FED_DEATH_TIMET);  
+    pac5xxx_dtg_config2(&(PAC55XX_TIMERA->DTGCTL1), RED_DEATH_TIMET, FED_DEATH_TIMET); 
+    pac5xxx_dtg_config2(&(PAC55XX_TIMERA->DTGCTL2), RED_DEATH_TIMET, FED_DEATH_TIMET);
 
     PAC55XX_TIMERA->CCTR4.CTR = 0;
     PAC55XX_TIMERA->CCTR5.CTR = 0;
     PAC55XX_TIMERA->CCTR6.CTR = 0;
+
 }
diff --git a/firmware/src/timer/timer.h b/firmware/src/timer/timer.h
index a2be559e..4edd6717 100644
--- a/firmware/src/timer/timer.h
+++ b/firmware/src/timer/timer.h
@@ -15,8 +15,7 @@
 //  * You should have received a copy of the GNU General Public License 
 //  * along with this program. If not, see <http://www.gnu.org/licenses/>.
 
-#ifndef TIMER_TIMER_H_
-#define TIMER_TIMER_H_
+#pragma once
 
 #define RED_DEATH_TIMET                 250         //Set rising edge death-time, if TACTL.DTGCLK is 0b, 50--> 1us
 #define FED_DEATH_TIMET                 250         //Set failling edge death-time, if TACTL.DTGCLK is 0b, 50--> 1us
@@ -39,6 +38,4 @@ typedef enum
     SINGLE_SHOT                         = 1,        // The timer single shot
 }TXCTL_SINGLE_Type;
 
-void Timer_Init(void);
-
-#endif /* TIMER_TIMER_H_ */
+void timers_init(void);
diff --git a/firmware/src/tm_enums.h b/firmware/src/tm_enums.h
new file mode 100644
index 00000000..dcf35f4d
--- /dev/null
+++ b/firmware/src/tm_enums.h
@@ -0,0 +1,164 @@
+/*
+* This file was automatically generated using Avlos.
+* https://github.com/tinymovr/avlos
+*
+* Any changes to this file will be overwritten when
+* content is regenerated.
+*/
+
+#pragma once
+
+typedef enum
+{
+    AVLOS_RET_NOACTION,
+    AVLOS_RET_READ = 1,
+    AVLOS_RET_WRITE = 2,
+    AVLOS_RET_CALL = 3
+} Avlos_Return;
+
+typedef enum
+{
+    AVLOS_CMD_WRITE,
+    AVLOS_CMD_READ = 1
+} Avlos_Command;
+
+typedef enum
+{
+    ERRORS_NONE = 0,
+    ERRORS_UNDERVOLTAGE = (1 << 0)
+} errors_flags;
+
+typedef enum
+{
+    WARNINGS_NONE = 0,
+    WARNINGS_DRIVER_FAULT = (1 << 0), 
+    WARNINGS_CHARGE_PUMP_FAULT_STAT = (1 << 1), 
+    WARNINGS_CHARGE_PUMP_FAULT = (1 << 2), 
+    WARNINGS_DRV10_DISABLE = (1 << 3), 
+    WARNINGS_DRV32_DISABLE = (1 << 4), 
+    WARNINGS_DRV54_DISABLE = (1 << 5)
+} warnings_flags;
+
+typedef enum
+{
+    SCHEDULER_WARNINGS_NONE = 0,
+    SCHEDULER_WARNINGS_CONTROL_BLOCK_REENTERED = (1 << 0)
+} scheduler_warnings_flags;
+
+typedef enum
+{
+    CONTROLLER_WARNINGS_NONE = 0,
+    CONTROLLER_WARNINGS_VELOCITY_LIMITED = (1 << 0), 
+    CONTROLLER_WARNINGS_CURRENT_LIMITED = (1 << 1), 
+    CONTROLLER_WARNINGS_MODULATION_LIMITED = (1 << 2)
+} controller_warnings_flags;
+
+typedef enum
+{
+    CONTROLLER_ERRORS_NONE = 0,
+    CONTROLLER_ERRORS_CURRENT_LIMIT_EXCEEDED = (1 << 0), 
+    CONTROLLER_ERRORS_PRE_CL_I_SD_EXCEEDED = (1 << 1)
+} controller_errors_flags;
+
+typedef enum
+{
+    MOTOR_ERRORS_NONE = 0,
+    MOTOR_ERRORS_PHASE_RESISTANCE_OUT_OF_RANGE = (1 << 0), 
+    MOTOR_ERRORS_PHASE_INDUCTANCE_OUT_OF_RANGE = (1 << 1), 
+    MOTOR_ERRORS_POLE_PAIRS_CALCULATION_DID_NOT_CONVERGE = (1 << 2), 
+    MOTOR_ERRORS_POLE_PAIRS_OUT_OF_RANGE = (1 << 3), 
+    MOTOR_ERRORS_ABNORMAL_CALIBRATION_VOLTAGE = (1 << 4)
+} motor_errors_flags;
+
+typedef enum
+{
+    SENSORS_SETUP_ONBOARD_ERRORS_NONE = 0,
+    SENSORS_SETUP_ONBOARD_ERRORS_CALIBRATION_FAILED = (1 << 0), 
+    SENSORS_SETUP_ONBOARD_ERRORS_READING_UNSTABLE = (1 << 1)
+} sensors_setup_onboard_errors_flags;
+
+typedef enum
+{
+    SENSORS_SETUP_EXTERNAL_SPI_ERRORS_NONE = 0,
+    SENSORS_SETUP_EXTERNAL_SPI_ERRORS_CALIBRATION_FAILED = (1 << 0), 
+    SENSORS_SETUP_EXTERNAL_SPI_ERRORS_READING_UNSTABLE = (1 << 1)
+} sensors_setup_external_spi_errors_flags;
+
+typedef enum
+{
+    SENSORS_SETUP_HALL_ERRORS_NONE = 0,
+    SENSORS_SETUP_HALL_ERRORS_CALIBRATION_FAILED = (1 << 0), 
+    SENSORS_SETUP_HALL_ERRORS_READING_UNSTABLE = (1 << 1)
+} sensors_setup_hall_errors_flags;
+
+typedef enum
+{
+    TRAJ_PLANNER_ERRORS_NONE = 0,
+    TRAJ_PLANNER_ERRORS_INVALID_INPUT = (1 << 0), 
+    TRAJ_PLANNER_ERRORS_VCRUISE_OVER_LIMIT = (1 << 1)
+} traj_planner_errors_flags;
+
+typedef enum
+{
+    HOMING_WARNINGS_NONE = 0,
+    HOMING_WARNINGS_HOMING_TIMEOUT = (1 << 0)
+} homing_warnings_flags;
+
+typedef enum
+{
+    CONTROLLER_STATE_IDLE = 0,
+    CONTROLLER_STATE_CALIBRATE = 1,
+    CONTROLLER_STATE_CL_CONTROL = 2,
+    CONTROLLER_STATE__MAX
+} controller_state_options;
+
+typedef enum
+{
+    CONTROLLER_MODE_CURRENT = 0,
+    CONTROLLER_MODE_VELOCITY = 1,
+    CONTROLLER_MODE_POSITION = 2,
+    CONTROLLER_MODE_TRAJECTORY = 3,
+    CONTROLLER_MODE_HOMING = 4,
+    CONTROLLER_MODE__MAX
+} controller_mode_options;
+
+typedef enum
+{
+    MOTOR_TYPE_HIGH_CURRENT = 0,
+    MOTOR_TYPE_GIMBAL = 1,
+    MOTOR_TYPE__MAX
+} motor_type_options;
+
+typedef enum
+{
+    SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX = 0,
+    SENSORS_SETUP_EXTERNAL_SPI_TYPE_AS5047 = 1,
+    SENSORS_SETUP_EXTERNAL_SPI_TYPE_AMT22 = 2,
+    SENSORS_SETUP_EXTERNAL_SPI_TYPE__MAX
+} sensors_setup_external_spi_type_options;
+
+typedef enum
+{
+    SENSORS_SETUP_EXTERNAL_SPI_RATE_1_5Mbps = 0,
+    SENSORS_SETUP_EXTERNAL_SPI_RATE_3Mbps = 1,
+    SENSORS_SETUP_EXTERNAL_SPI_RATE_6Mbps = 2,
+    SENSORS_SETUP_EXTERNAL_SPI_RATE_8Mbps = 3,
+    SENSORS_SETUP_EXTERNAL_SPI_RATE_12Mbps = 4,
+    SENSORS_SETUP_EXTERNAL_SPI_RATE__MAX
+} sensors_setup_external_spi_rate_options;
+
+typedef enum
+{
+    SENSORS_SELECT_POSITION_SENSOR_CONNECTION_ONBOARD = 0,
+    SENSORS_SELECT_POSITION_SENSOR_CONNECTION_EXTERNAL_SPI = 1,
+    SENSORS_SELECT_POSITION_SENSOR_CONNECTION_HALL = 2,
+    SENSORS_SELECT_POSITION_SENSOR_CONNECTION__MAX
+} sensors_select_position_sensor_connection_options;
+
+typedef enum
+{
+    SENSORS_SELECT_COMMUTATION_SENSOR_CONNECTION_ONBOARD = 0,
+    SENSORS_SELECT_COMMUTATION_SENSOR_CONNECTION_EXTERNAL_SPI = 1,
+    SENSORS_SELECT_COMMUTATION_SENSOR_CONNECTION_HALL = 2,
+    SENSORS_SELECT_COMMUTATION_SENSOR_CONNECTION__MAX
+} sensors_select_commutation_sensor_connection_options;
diff --git a/firmware/src/uart/uart_interface.c b/firmware/src/uart/uart_interface.c
index 6861c7ab..5166208b 100644
--- a/firmware/src/uart/uart_interface.c
+++ b/firmware/src/uart/uart_interface.c
@@ -36,23 +36,23 @@ void UART_WriteAddr(uint8_t addr, int32_t data)
         controller_set_Iq_setpoint_user_frame(0);
         controller_set_vel_setpoint_user_frame(0);
         controller_set_pos_setpoint_user_frame(data);
-        controller_set_mode(CTRL_POSITION);
+        controller_set_mode(CONTROLLER_MODE_POSITION);
         break;
 
     case 'V': // vel setpoint
         controller_set_Iq_setpoint_user_frame(0);
         controller_set_vel_setpoint_user_frame(data);
-        controller_set_mode(CTRL_VELOCITY);
+        controller_set_mode(CONTROLLER_MODE_VELOCITY);
         controller_set_vel_setpoint_user_frame((float)data);
         break;
 
     case 'I': // current setpoint
-        controller_set_mode(CTRL_CURRENT);
+        controller_set_mode(CONTROLLER_MODE_CURRENT);
         controller_set_Iq_setpoint_user_frame((float)data * ONE_OVER_UART_I_SCALING_FACTOR);
         break;
 
     case 'G': // velocity integrator gain
-        controller_set_vel_integrator_gain((float)data * ONE_OVER_UART_VEL_INT_SCALING_FACTOR);
+        controller_set_vel_integral_gain((float)data * ONE_OVER_UART_VEL_INT_SCALING_FACTOR);
         break;
 
     case 'Y': // Position gain
@@ -125,7 +125,7 @@ int32_t UART_ReadAddr(uint8_t addr)
     break;
 
     case 'p': // pos estimate
-        ret_val = observer_get_pos_estimate_user_frame();
+        ret_val = user_frame_get_pos_estimate();
         break;
 
     case 'P': // pos setpoint
@@ -133,7 +133,7 @@ int32_t UART_ReadAddr(uint8_t addr)
         break;
 
     case 'v': // vel estimate
-        ret_val = (int32_t)observer_get_vel_estimate_user_frame();
+        ret_val = (int32_t)user_frame_get_vel_estimate();
         break;
 
     case 'V': // vel setpoint
@@ -149,7 +149,7 @@ int32_t UART_ReadAddr(uint8_t addr)
         break;
 
     case 'G': // velocity integrator setpoint
-        ret_val = (int32_t)(controller_get_vel_integrator_gain() * UART_VEL_INT_SCALING_FACTOR);
+        ret_val = (int32_t)(controller_get_vel_integral_gain() * UART_VEL_INT_SCALING_FACTOR);
         break;
 
     case 'H': // phase resistance
@@ -181,15 +181,15 @@ int32_t UART_ReadAddr(uint8_t addr)
         break;
 
     case 'Q': // calibrate
-        controller_set_state(STATE_CALIBRATE);
+        controller_set_state(CONTROLLER_STATE_CALIBRATE);
         break;
 
     case 'A': // closed loop
-        controller_set_state(STATE_CL_CONTROL);
+        controller_set_state(CONTROLLER_STATE_CL_CONTROL);
         break;
 
     case 'Z': // idle
-        controller_set_state(STATE_IDLE);
+        controller_set_state(CONTROLLER_STATE_IDLE);
         break;
 
     case 'R': // reset mcu
diff --git a/firmware/src/utils/utils.h b/firmware/src/utils/utils.h
index 41829b65..0c052310 100644
--- a/firmware/src/utils/utils.h
+++ b/firmware/src/utils/utils.h
@@ -22,6 +22,10 @@
 
 #include "src/common.h"
 
+#define DIVIDE_AND_ROUND_UP(numerator, divisor) (((numerator) + (divisor) - 1) / (divisor))
+
+extern void wait_for_control_loop_interrupt(void);
+
 #if __ARM_FEATURE_FMA && __ARM_FP&4 && !__SOFTFP__ && !BROKEN_VFP_ASM
 
 static inline float fast_sqrt(float x)
@@ -42,6 +46,14 @@ static inline float our_fabsf(float x)
 
 #endif
 
+static inline void wait_pwm_cycles(uint32_t cycles)
+{
+    for (uint32_t i = 0; i < cycles; i++)
+    {
+        wait_for_control_loop_interrupt();
+    }
+}
+
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wstrict-aliasing"
 #pragma GCC diagnostic ignored "-Wuninitialized"
@@ -81,7 +93,7 @@ static inline void delay_us(uint32_t us)
     pac_delay_asm(us * 16u);
 }
 
-static inline bool our_clampc(float *d, float min, float max)
+static inline bool our_clampc(float *d, const float min, const float max)
 {
     const float t = *d < min ? min : *d;
     *d = t > max ? max : t;
@@ -103,7 +115,6 @@ static inline float our_floorf(float x)
 	return (float)((int)x - 1);
 }
 
-
 // based on https://github.com/divideconcept/FastTrigo/blob/master/fasttrigo.cpp
 static inline float cos_32s(float x)
 {
@@ -132,21 +143,24 @@ static inline float fast_sin(float angle)
     return fast_cos(halfpi-angle);
 }
 
-static inline uint16_t crc16_ccitt(const uint32_t block[], uint16_t blockLength, uint16_t crc)
-{
-    PAC55XX_CRC->SEED.CRCSEED = crc;
+typedef struct {
+    float sum_current;
+    float sum_current_squared;
+    uint32_t size;
+} Statistics;
 
-    // Compute CRC using 32-bit input on memory that is 32-bit aligned 
-    while(blockLength)    
-    {
-        PAC55XX_CRC->DATAIN = *block++;             // Input a 32-bit word
-        blockLength = blockLength - 4;              // Decrement Length by 4 bytes
-    }
-    
-    __asm__("NOP");
-    __asm__("NOP");
+static inline void update_statistics(Statistics *s, float new_current)
+{
+    s->sum_current += new_current;
+    s->sum_current_squared += new_current * new_current;
+    s->size++;
+}
 
-    return PAC55XX_CRC->OUT.CRCOUT;
+static inline float calculate_standard_deviation(Statistics *s)
+{
+    float mean = s->sum_current / s->size;
+    float mean_squares = s->sum_current_squared / s->size;
+    return fast_sqrt(mean_squares - mean * mean);
 }
 
 // https://github.com/madcowswe/ODrive/blob/3113aedf081cf40e942d25d3b0b36c8806f11f23/Firmware/MotorControl/utils.c
diff --git a/firmware/src/xf1.h b/firmware/src/xf1.h
new file mode 100644
index 00000000..46433e3f
--- /dev/null
+++ b/firmware/src/xf1.h
@@ -0,0 +1,84 @@
+#pragma once
+
+#include <math.h> // For fabsf() - single precision absolute value
+
+#define DEFAULT_TRANSFORM (FrameTransform){.offset = 0.0f, .multiplier = 1.0f}
+
+// Structure to represent a 1D transformation from one frame to another
+typedef struct {
+    float offset;      // Offset to be added or subtracted
+    float multiplier;  // Multiplier to scale and indicate direction
+} FrameTransform;
+
+// Function to apply the transformation
+// Applies the transformation in the form: (value * multiplier) + offset
+static inline float apply_transform(float value, const FrameTransform* transformation) {
+    return (value * transformation->multiplier) + transformation->offset;
+}
+
+// Function to apply the transformation to a velocity (ignoring offset)
+static inline float apply_velocity_transform(float velocity, const FrameTransform* transformation) {
+    return velocity * transformation->multiplier;
+}
+
+// Function to derive the inverse of a given transformation
+static inline FrameTransform derive_inverse_transform(const FrameTransform* original) {
+    FrameTransform inverse;
+    // The inverse multiplier is the reciprocal of the original multiplier
+    inverse.multiplier = 1.0f / original->multiplier;
+    // To find the offset for the inverse, negate the original offset and then scale it by the inverse multiplier
+    inverse.offset = -original->offset * inverse.multiplier;
+    return inverse;
+}
+
+// Function to derive a FrameTransform based on positions in two frames
+static inline FrameTransform derive_transform_2p(float As, float At, float Bs, float Bt) {
+    FrameTransform transformation;
+
+    float sourceDelta = Bs - As;
+    float targetDelta = Bt - At;
+
+    if (sourceDelta == 0) {
+        // If there's no change in source, the multiplier should be set to 1
+        // and the offset is simply the difference between target and source positions
+        transformation.multiplier = 1.0f;
+        transformation.offset = At - As;
+    } else {
+        // Calculate multiplier, incorporating direction based on the delta values
+        transformation.multiplier = targetDelta / sourceDelta;
+        // Calculate offset using one pair of points
+        transformation.offset = At - (As * transformation.multiplier);
+    }
+
+    return transformation;
+}
+
+static inline FrameTransform derive_dir_transform_2p(float As, float At, float Bs, float Bt) {
+    FrameTransform transform;
+
+    // Calculate the direction of the movement in both frames
+    float sourceDirection = Bs - As;
+    float targetDirection = Bt - At;
+
+    // Determine the multiplier based on the direction comparison
+    transform.multiplier = (sourceDirection * targetDirection > 0) ? 1.0f : -1.0f;
+
+    // Use the first pair of points and the determined multiplier to calculate the offset
+    // If the multiplier is 1.0, offset = At - As; if -1.0, offset = At + As
+    transform.offset = At - (As * transform.multiplier);
+
+    return transform;
+}
+
+// Function to combine two transformations
+static inline FrameTransform combine_transforms(const FrameTransform* first, const FrameTransform* second) {
+    FrameTransform combined;
+    
+    // The combined multiplier is the product of the two multipliers
+    combined.multiplier = first->multiplier * second->multiplier;
+    
+    // The combined offset is the first offset plus the second offset scaled by the first multiplier
+    combined.offset = first->offset + (second->offset * first->multiplier);
+    
+    return combined;
+}
diff --git a/firmware/src/xfs.c b/firmware/src/xfs.c
new file mode 100644
index 00000000..286acfa6
--- /dev/null
+++ b/firmware/src/xfs.c
@@ -0,0 +1,15 @@
+
+
+#include <src/xfs.h>
+
+FramesConfig frames = {
+    .position_sensor_to_user = DEFAULT_TRANSFORM,
+    .user_to_position_sensor = DEFAULT_TRANSFORM,
+    .position_sensor_to_motor = DEFAULT_TRANSFORM,
+    .motor_to_position_sensor = DEFAULT_TRANSFORM,
+    .commutation_sensor_to_motor = DEFAULT_TRANSFORM,
+    .motor_to_commutation_sensor = DEFAULT_TRANSFORM,
+    .motor_to_user = DEFAULT_TRANSFORM,
+    .user_to_motor = DEFAULT_TRANSFORM,
+    .calibrated = default_calibrated
+};
diff --git a/firmware/src/xfs.h b/firmware/src/xfs.h
new file mode 100644
index 00000000..a38a416b
--- /dev/null
+++ b/firmware/src/xfs.h
@@ -0,0 +1,159 @@
+//  * This file is part of the Tinymovr-Firmware distribution
+//  * (https://github.com/yconst/tinymovr-firmware).
+//  * Copyright (c) 2020-2023 Ioannis Chatzikonstantinou.
+//  * 
+//  * This program is free software: you can redistribute it and/or modify  
+//  * it under the terms of the GNU General Public License as published by  
+//  * the Free Software Foundation, version 3.
+//  *
+//  * This program is distributed in the hope that it will be useful, but 
+//  * WITHOUT ANY WARRANTY; without even the implied warranty of 
+//  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
+//  * General Public License for more details.
+//  *
+//  * You should have received a copy of the GNU General Public License 
+//  * along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <stdbool.h>
+#include <src/xf1.h>
+
+typedef enum {
+    none_calibrated = 0,
+    position_sensor_to_user_calibrated = 1,
+    user_to_position_sensor_calibrated = 1 << 1,
+    position_sensor_to_motor_calibrated = 1 << 2,
+    motor_to_position_sensor_calibrated = 1 << 3,
+    commutation_sensor_to_motor_calibrated = 1 << 4,
+    motor_to_commutation_sensor_calibrated = 1 << 5,
+    motor_to_user_calibrated = 1 << 6,
+    user_to_motor_calibrated = 1 << 7,
+    default_calibrated = (position_sensor_to_user_calibrated | user_to_position_sensor_calibrated),
+    all_calibrated = 0xFF
+} FramesOrder;
+
+
+typedef struct {
+	FrameTransform position_sensor_to_user;
+	FrameTransform user_to_position_sensor;
+	FrameTransform position_sensor_to_motor;
+	FrameTransform motor_to_position_sensor;
+	FrameTransform commutation_sensor_to_motor;
+	FrameTransform motor_to_commutation_sensor;
+	FrameTransform motor_to_user;
+	FrameTransform user_to_motor;
+    FramesOrder calibrated;
+} FramesConfig;
+
+extern FramesConfig frames;
+
+static inline bool frames_get_calibrated(void)
+{
+    return (frames.calibrated == all_calibrated);
+}
+
+static inline void frames_reset_calibrated(void)
+{
+    frames.calibrated = default_calibrated;
+}
+
+static inline void frames_get_config(FramesConfig *_config)
+{
+	*_config = frames;
+}
+
+static inline void frames_restore_config(FramesConfig *_config)
+{
+    frames = *_config;
+}
+
+static inline FrameTransform *frame_position_sensor_to_user_p(void)
+{
+    return &(frames.position_sensor_to_user);
+}
+
+static inline FrameTransform *frame_user_to_position_sensor_p(void)
+{
+    return &(frames.user_to_position_sensor);
+}
+
+static inline FrameTransform *frame_position_sensor_to_motor_p(void)
+{
+    return &(frames.position_sensor_to_motor);
+}
+
+static inline FrameTransform *frame_motor_to_position_sensor_p(void)
+{
+    return &(frames.motor_to_position_sensor);
+}
+
+static inline FrameTransform *frame_commutation_sensor_to_motor_p(void)
+{
+    return &(frames.commutation_sensor_to_motor);
+}
+
+static inline FrameTransform *frame_motor_to_commutation_sensor_p(void)
+{
+    return &(frames.motor_to_commutation_sensor);
+}
+
+static inline FrameTransform *frame_motor_to_user_p(void)
+{
+    return &(frames.motor_to_user);
+}
+
+static inline FrameTransform *frame_user_to_motor_p(void)
+{
+    return &(frames.user_to_motor);
+}
+
+static inline float frame_user_to_position_sensor_get_offset(void)
+{
+	return frame_user_to_position_sensor_p()->offset;
+}
+
+static inline float frame_user_to_position_sensor_get_multiplier(void)
+{
+	return frame_user_to_position_sensor_p()->multiplier;
+}
+
+static inline void frame_user_to_position_sensor_set_offset(float value)
+{
+    frames.user_to_position_sensor.offset = value;
+    frames.position_sensor_to_user = derive_inverse_transform(frame_user_to_position_sensor_p());
+    frames.user_to_motor = combine_transforms(frame_user_to_position_sensor_p(), frame_position_sensor_to_motor_p());
+    frames.motor_to_user = derive_inverse_transform(frame_user_to_motor_p());
+    frames.calibrated |= (position_sensor_to_user_calibrated | 
+        user_to_position_sensor_calibrated | motor_to_user_calibrated | user_to_motor_calibrated);
+}
+
+static inline void frame_user_to_position_sensor_set_multiplier(float value)
+{
+    if (value != 0)
+    {
+        frames.user_to_position_sensor.multiplier = value;
+        frames.position_sensor_to_user = derive_inverse_transform(frame_user_to_position_sensor_p());
+        frames.user_to_motor = combine_transforms(frame_user_to_position_sensor_p(), frame_position_sensor_to_motor_p());
+        frames.motor_to_user = derive_inverse_transform(frame_user_to_motor_p());
+        frames.calibrated |= (position_sensor_to_user_calibrated | 
+            user_to_position_sensor_calibrated | motor_to_user_calibrated | user_to_motor_calibrated);
+    }
+}
+
+static inline void frame_set_commutation_sensor_to_motor(const FrameTransform value)
+{
+    frames.commutation_sensor_to_motor = value;
+    frames.motor_to_commutation_sensor = derive_inverse_transform(frame_commutation_sensor_to_motor_p());
+    frames.calibrated |= (commutation_sensor_to_motor_calibrated | motor_to_commutation_sensor_calibrated);
+}
+
+static inline void frame_set_position_sensor_to_motor(const FrameTransform value)
+{
+    frames.position_sensor_to_motor = value;
+    frames.motor_to_position_sensor = derive_inverse_transform(&value);
+    frames.user_to_motor = combine_transforms(frame_user_to_position_sensor_p(), frame_position_sensor_to_motor_p());
+    frames.motor_to_user = derive_inverse_transform(frame_user_to_motor_p());
+    frames.calibrated |= (position_sensor_to_motor_calibrated |
+        motor_to_position_sensor_calibrated | motor_to_user_calibrated | user_to_motor_calibrated);
+}
diff --git a/studio/Python/Manifest.in b/studio/Python/Manifest.in
index 39735dd7..e6349fea 100644
--- a/studio/Python/Manifest.in
+++ b/studio/Python/Manifest.in
@@ -1,15 +1,3 @@
 include README.md
-include tinymovr/resources/icons/call.png
-include tinymovr/resources/icons/call_dark.png
-include tinymovr/resources/icons/call@2x.png
-include tinymovr/resources/icons/call_dark@2x.png
-include tinymovr/resources/icons/empty.png
-include tinymovr/resources/icons/empty@2x.png
-include tinymovr/resources/icons/empty_dark.png
-include tinymovr/resources/icons/empty_dark@2x.png
-include tinymovr/specs/tinymovr_1_3_x.yaml
-include tinymovr/specs/tinymovr_1_4_x.yaml
-include tinymovr/specs/tinymovr_1_5_x.yaml
-include tinymovr/specs/tinymovr_1_6_x.yaml
-include tinymovr/specs/dfu_1_0_x.yaml
-include tinymovr/specs/dfu_1_1_x.yaml
+recursive-include tinymovr/resources/icons *.png
+recursive-include tinymovr/specs *.yaml
diff --git a/studio/Python/pyproject.toml b/studio/Python/pyproject.toml
new file mode 100644
index 00000000..2902195a
--- /dev/null
+++ b/studio/Python/pyproject.toml
@@ -0,0 +1,14 @@
+
+[build-system]
+requires = ["setuptools"]
+build-backend = "setuptools.build_meta"
+
+[tool.pytest.ini_options]
+markers = [
+    "hitl_default: marks default hardware in the loop tests",
+    "sensor_amt22: marks tests that need the amt22 sensor",
+    "sensor_as5047: marks tests that need the as5047 sensor",
+    "sensor_hall: marks tests that need a hall effect sensored motor",
+    "eol: marks tests for end-of-line testing",
+    "dfu: marks tests for dfu testing",
+]
\ No newline at end of file
diff --git a/studio/Python/setup.py b/studio/Python/setup.py
index a97e4c85..a97887bd 100644
--- a/studio/Python/setup.py
+++ b/studio/Python/setup.py
@@ -60,6 +60,7 @@
         "docopt",
         "flatten-dict",
         "pint",
+        "pretty_errors"
     ],
     extras_require={"gui": ["pyside6", "pyqtgraph>=0.13.3"]},
     entry_points={
diff --git a/studio/Python/tinymovr/__init__.py b/studio/Python/tinymovr/__init__.py
index 4828124e..9f937723 100644
--- a/studio/Python/tinymovr/__init__.py
+++ b/studio/Python/tinymovr/__init__.py
@@ -1,1 +1,2 @@
-from tinymovr.tee import init_tee, destroy_tee, get_tee
\ No newline at end of file
+from tinymovr.bus_manager import BusManager
+from tinymovr.bus_router import init_router, destroy_router, get_router
diff --git a/studio/Python/tinymovr/bus_manager.py b/studio/Python/tinymovr/bus_manager.py
new file mode 100644
index 00000000..65b37ea1
--- /dev/null
+++ b/studio/Python/tinymovr/bus_manager.py
@@ -0,0 +1,77 @@
+"""
+Tinymovr BusManager Module
+Copyright Ioannis Chatzikonstantinou 2020-2023
+
+Implements a BusRouter class to distribute incoming traffic according to rules
+
+This program is free software: you can redistribute it and/or modify it under
+the terms of the GNU General Public License as published by the Free Software
+Foundation, either version 3 of the License, or (at your option) any later
+version.
+This program is distributed in the hope that it will be useful, but WITHOUT
+ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+You should have received a copy of the GNU General Public License along with
+this program. If not, see <http://www.gnu.org/licenses/>.
+"""
+
+from can.exceptions import CanOperationError, CanInitializationError
+
+class BusManager:
+    def __init__(self, bus_class, bus_params, logger, bus_exceptions=(CanOperationError, CanInitializationError)):
+        self.bus = None
+        self.bus_class = bus_class
+        self.bus_params = bus_params
+        self.bus_exceptions = bus_exceptions
+        self.logger = logger
+        self.attempt_reconnect()
+
+    def attempt_reconnect(self):
+        try:
+            bus = self.bus_class(**self.bus_params)
+            # Flush bus to discard buffered data
+            # Also acts as check for bus exception
+            self.flush_rx_buffer(bus)
+            self.bus = bus
+            self.logger.info("Bus connected")
+        except self.bus_exceptions as e:
+            self.bus = None
+
+    def is_connected(self):
+        return self.bus != None
+
+    def recv(self, timeout):
+        try:
+            return self.bus.recv(timeout=timeout)
+        except self.bus_exceptions as e:
+            self.logger.warn(e)
+            self.bus = None
+            self.attempt_reconnect()
+        except AttributeError:
+            self.attempt_reconnect()
+    
+    def shutdown(self):
+        try:
+            self.bus.shutdown()
+            self.bus = None
+        except AttributeError:
+            pass
+        
+    def send(self, frame):
+        try:
+            self.bus.send(frame)
+        except self.bus_exceptions as e:
+            self.logger.warn(e)
+            self.attempt_reconnect()
+        except AttributeError:
+            self.attempt_reconnect()
+
+    def flush_rx_buffer(self, bus=None, trials=100):
+        """
+        Flush the RX buffer of a bus
+        """
+        if bus == None:
+            bus = self.bus
+        for i in range(trials):
+            if not bus.recv(timeout=0.001):
+                return
\ No newline at end of file
diff --git a/studio/Python/tinymovr/bus_router.py b/studio/Python/tinymovr/bus_router.py
new file mode 100644
index 00000000..a963420a
--- /dev/null
+++ b/studio/Python/tinymovr/bus_router.py
@@ -0,0 +1,152 @@
+"""
+Tinymovr BusRouter Module
+Copyright Ioannis Chatzikonstantinou 2020-2023
+
+Implements a BusRouter class to distribute incoming traffic according to rules
+
+This program is free software: you can redistribute it and/or modify it under
+the terms of the GNU General Public License as published by the Free Software
+Foundation, either version 3 of the License, or (at your option) any later
+version.
+This program is distributed in the hope that it will be useful, but WITHOUT
+ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+You should have received a copy of the GNU General Public License along with
+this program. If not, see <http://www.gnu.org/licenses/>.
+"""
+
+
+import warnings
+from enum import Enum
+from threading import Thread, Lock
+from tinymovr.bus_manager import BusManager
+
+
+bus_router = None
+
+
+class Client:
+    def __init__(self, filter_cb, recv_cb):
+        self.filter_cb = filter_cb
+        self.recv_cb = recv_cb
+
+
+class BusRouterState(Enum):
+    INIT = 0
+    RUNNING = 1
+    STOPPING = 2
+    STOPPED = 3
+
+
+class BusRouter:
+    """
+    Distribute incoming messages based on the boolean result
+    of a filter callback.
+
+    python-can does not allow filtering messages per recipient therefore
+    this class.
+
+    Also implements a simple forwarding mechanism for sending messages, to
+    simplify interfacing with CAN bus objects.
+    """
+
+    def __init__(self, bus_manager, timeout, logger):
+        self.bus_manager = bus_manager
+        self.timeout = timeout
+        self.logger = logger
+        self.clients = []
+        self.running = True
+        self.update_thread = Thread(target=self.run, daemon=True)
+        self.update_thread.start()
+
+    def run(self):
+        while self.running:
+            frame = self.bus_manager.recv(self.timeout)
+            if frame:
+                for client in self.clients:
+                    if client.filter_cb(frame):
+                        client.recv_cb(frame)
+
+    def add_client(self, filter_cb, recv_cb):
+        self.clients.append(Client(filter_cb, recv_cb))
+
+    def stop(self):
+        self.running = False
+        if self.update_thread.is_alive():
+            self.update_thread.join()
+
+    def shutdown(self):
+        self.stop()
+        self.bus_manager.shutdown()
+
+    def send(self, frame):
+        """
+        Send a frame by forwarding to the bus manager object
+        """
+        self.bus_manager.send(frame)
+
+
+def init_router(bus_class, bus_params, logger, timeout=0.1):
+    """
+    Initializes a bus router using a python-can bus instance
+    """
+    global bus_router
+    if bus_router == None:
+        bus_manager = BusManager(bus_class, bus_params, logger=logger)
+        bus_router = BusRouter(bus_manager, timeout, logger=logger)
+    else:
+        logger.warning("Bus Router already exists")
+    return bus_router
+
+
+def destroy_router():
+    """
+    Destroys the existing bus router, stopping its thread.
+    """
+    global bus_router
+    if bus_router != None:
+        bus_router.shutdown()
+        bus_router = None
+
+
+def get_router():
+    """
+    Get the current bus router object
+    """
+    return bus_router
+
+
+# Deprecated functions
+
+
+def init_tee(bus, timeout=0.1):
+    """
+    Initializes a tee using a python-can bus instance
+    """
+    raise RuntimeError(
+        "init_tee() is no longer supported. Please use init_router() instead."
+    )
+
+
+def destroy_tee():
+    """
+    Destroys the existing tee, stopping its thread.
+    """
+    warnings.warn(
+        "destroy_tee() is deprecated and will be removed in future versions. Please use destroy_router() instead.",
+        DeprecationWarning,
+        stacklevel=2,
+    )
+    destroy_router()
+
+
+def get_tee():
+    """
+    Get the current tee object
+    """
+    warnings.warn(
+        "get_tee() is deprecated and will be removed in future versions. Please use get_router() instead.",
+        DeprecationWarning,
+        stacklevel=2,
+    )
+    get_router()
diff --git a/studio/Python/tinymovr/channel.py b/studio/Python/tinymovr/channel.py
index 42cb07fc..8fd5765a 100644
--- a/studio/Python/tinymovr/channel.py
+++ b/studio/Python/tinymovr/channel.py
@@ -19,7 +19,7 @@
 import can
 from functools import cached_property
 from avlos.channel import BaseChannel
-from tinymovr.tee import get_tee
+from tinymovr.bus_router import get_router
 from tinymovr.constants import (
     CAN_DEV_MASK,
     CAN_EP_SIZE,
@@ -43,7 +43,7 @@ def __init__(self, node_id, compare_hash = 0):
         self.queue = []
         self.lock = Lock()
         self.evt = Event()
-        get_tee().add(self._filter_frame, self._recv_cb)
+        get_router().add_client(self._filter_frame, self._recv_cb)
 
     def _filter_frame(self, frame):
         return not frame.is_remote_frame and ids_from_arbitration(frame.arbitration_id)[2] == self.node_id
@@ -60,10 +60,10 @@ def send(self, data, ep_id):
         """
         Send a CAN frame to a specific endpoint. The `rtr` flag is set
         based on whether data is provided, and the frame is sent via the
-        global tee instance.
+        global bus router instance.
         """
         rtr = False if data and len(data) else True
-        get_tee().send(self.create_frame(ep_id, rtr, data))
+        get_router().send(self.create_frame(ep_id, rtr, data))
 
     def recv(self, ep_id, timeout=1.0):
         """
diff --git a/studio/Python/tinymovr/cli.py b/studio/Python/tinymovr/cli.py
index 03a7ea16..5df57a1c 100644
--- a/studio/Python/tinymovr/cli.py
+++ b/studio/Python/tinymovr/cli.py
@@ -19,8 +19,8 @@
 from traitlets.config import Config
 from docopt import docopt
 
-from tinymovr import init_tee, destroy_tee
-from tinymovr.discovery import Discovery
+from tinymovr import init_router, destroy_router
+from tinymovr.device_discovery import DeviceDiscovery
 from tinymovr.constants import app_name
 from tinymovr.config import get_bus_config, configure_logging, add_spec
 
@@ -62,8 +62,7 @@ def spawn():
     bitrate = int(arguments["--bitrate"])
 
     if not channel:
-        params = get_bus_config(buses)
-        params["bitrate"] = bitrate
+        params = get_bus_config(buses, bitrate=bitrate)
     else:
         params = {"bustype": buses[0], "channel": channel, "bitrate": bitrate}
 
@@ -85,15 +84,16 @@ def node_disappeared(node_id):
 
     print(app_name + " " + str(version))
 
-    init_tee(can.Bus(**params))
-    dsc = Discovery(node_appeared, node_disappeared, logger)
+    #TODO: router init should not happen in CLI spawn function
+    init_router(can.Bus, params, logger=logger)
+    dsc = DeviceDiscovery(node_appeared, node_disappeared, logger)
     print("Listening for nodes...")
 
     c = Config()
     c.TerminalIPythonApp.display_banner = False
     IPython.start_ipython(argv=[], config=c, user_ns=user_ns)
     logger.debug("Exiting...")
-    destroy_tee()
+    destroy_router()
 
 
 if __name__ == "__main__":
diff --git a/studio/Python/tinymovr/config/__init__.py b/studio/Python/tinymovr/config/__init__.py
index 87c5bf66..c10439ec 100644
--- a/studio/Python/tinymovr/config/__init__.py
+++ b/studio/Python/tinymovr/config/__init__.py
@@ -1,4 +1,5 @@
 from tinymovr.config.config import (
+    IncompatibleSpecVersionError,
     get_bus_config,
     configure_logging,
     create_device,
diff --git a/studio/Python/tinymovr/config/config.py b/studio/Python/tinymovr/config/config.py
index edb2eedc..454f272b 100644
--- a/studio/Python/tinymovr/config/config.py
+++ b/studio/Python/tinymovr/config/config.py
@@ -25,13 +25,23 @@
 from tinymovr.codec import DataType
 from tinymovr.channel import CANChannel
 
+
+class IncompatibleSpecVersionError(Exception):
+    def __init__(self, hash_value):
+        self.hash_value = hash_value
+        super().__init__(
+            f"Device found, but incompatible (no device spec found for hash {hash_value})."
+        )
+
+
 specs = {}
 
 # We added the following as a temporary solution
 # after a hash was modified without modifying the
 # spec (we are suspecting dependency upgrade
 # breaking hash computation)
-hash_aliases = { 3526126264 : [4118115615]}
+hash_aliases = {3526126264: [4118115615]}
+
 
 def get_device_spec(hash, logger=None):
     if logger is None:
@@ -40,15 +50,20 @@ def get_device_spec(hash, logger=None):
     try:
         return specs["hash_uint32"][hash]
     except KeyError:
-        for hash_alias in hash_aliases[hash]:
+        for hash_alias in hash_aliases.get(hash, []):
             try:
                 spec = specs["hash_uint32"][hash_alias]
-                logger.debug("Found compatible spec via hash alias {} for device hash {}", hash_alias, hash)
+                logger.debug(
+                    "Found compatible spec via hash alias {} for device hash {}".format(
+                        hash_alias, hash
+                    )
+                )
                 return spec
             except KeyError:
                 pass
     return None
 
+
 def init_specs_dict():
     global specs
     specs = {"hash_uint32": {}}
@@ -65,7 +80,11 @@ def add_spec(spec, logger=None):
     tmp_node = deserialize(spec)
     hash_value = tmp_node.hash_uint32
     if hash_value in specs["hash_uint32"]:
-        logger.warning("Provided spec with hash {} already exists in hash/name dictionary".format(hash_value))
+        logger.warning(
+            "Provided spec with hash {} already exists in hash/name dictionary".format(
+                hash_value
+            )
+        )
     else:
         specs["hash_uint32"][hash_value] = spec
 
@@ -73,7 +92,7 @@ def add_spec(spec, logger=None):
 init_specs_dict()
 
 
-def get_bus_config(suggested_types=None):
+def get_bus_config(suggested_types=None, bitrate=None):
     """
     Get the bus configuration (interface, channel) for
     the first of the suggested interface types. Present
@@ -81,7 +100,10 @@ def get_bus_config(suggested_types=None):
     """
     configs = can.interface.detect_available_configs(suggested_types)
     try:
-        return configs[0]
+        config = configs[0]
+        if bitrate:
+            configs[0]["bitrate"] = bitrate
+        return config
     except IndexError as exc:
         raise can.CanInitializationError("No active interface found") from exc
 
@@ -109,7 +131,7 @@ def create_device(node_id, logger=None):
     chan.compare_hash = protocol_hash & 0xFF
 
     if not device_spec:
-        raise ValueError(f"Device found, but incompatible (no device spec found for hash {protocol_hash}).")
+        raise IncompatibleSpecVersionError(protocol_hash)
 
     node = deserialize(device_spec)
     node._channel = chan
@@ -133,11 +155,7 @@ def create_device_with_hash_msg(heartbeat_msg, logger=None):
     chan.compare_hash = hash & 0xFF
 
     if not device_spec:
-        # TODO: This message is more descriptive than the
-        # previous ("no device spec found for hash #"), but
-        # may not always be accurate. Still we keep it in
-        # order to better guide the user.4
-        raise ValueError(f"Device found, but incompatible (no device spec found for hash {hash}).")
+        raise IncompatibleSpecVersionError(hash)
 
     node = deserialize(device_spec)
 
diff --git a/studio/Python/tinymovr/discovery.py b/studio/Python/tinymovr/device_discovery.py
similarity index 91%
rename from studio/Python/tinymovr/discovery.py
rename to studio/Python/tinymovr/device_discovery.py
index f4f946f0..7cd8a004 100644
--- a/studio/Python/tinymovr/discovery.py
+++ b/studio/Python/tinymovr/device_discovery.py
@@ -1,5 +1,5 @@
 """
-Tinymovr Discovery Module
+Tinymovr DeviceDiscovery Module
 Copyright Ioannis Chatzikonstantinou 2020-2023
 
 Implements a class to discover nodes on the CAN bus based on
@@ -19,12 +19,12 @@
 import time
 from threading import Thread, Event
 from tinymovr.channel import ResponseError
-from tinymovr.tee import get_tee
+from tinymovr.bus_router import get_router
 from tinymovr.constants import HEARTBEAT_BASE
-from tinymovr.config import create_device_with_hash_msg
+from tinymovr.config import create_device_with_hash_msg, IncompatibleSpecVersionError
 
 
-class Discovery:
+class DeviceDiscovery:
     """
     Discover Tinymovr instances on the CAN bus using the periodically
     transmitted heartbeat frame.
@@ -41,7 +41,7 @@ def __init__(self, appeared_cb, disappeared_cb, logger, lost_timeout=2.0):
 
         self.reset()
 
-        get_tee().add(
+        get_router().add_client(
             lambda msg: HEARTBEAT_BASE == msg.arbitration_id & HEARTBEAT_BASE,
             self._recv_cb,
         )
@@ -72,7 +72,7 @@ def _recv_cb(self, frame):
                 self._append_to_queue((node, node_id))
             except ResponseError as e:
                 self.logger.error(e)
-            except ValueError as e:
+            except IncompatibleSpecVersionError as e:
                 self.logger.error(e)
                 self.incompatible_nodes.add(node_id)
             self.pending_nodes.remove(node_id)
diff --git a/studio/Python/tinymovr/dfu.py b/studio/Python/tinymovr/dfu.py
index 299bbaf6..4027e928 100644
--- a/studio/Python/tinymovr/dfu.py
+++ b/studio/Python/tinymovr/dfu.py
@@ -1,12 +1,13 @@
 """
 Usage:
-    dfu.py --node_id=ID [--bin=PATH | --recovery] [--no-reset] [--bus=<bus>] [--chan=<chan>] [--bitrate=<bitrate>]
+    dfu.py --node_id=ID [--bin=PATH | --recovery] [--no-reset] [--force] [--bus=<bus>] [--chan=<chan>] [--bitrate=<bitrate>]
 
 Options:
     --node_id=ID The CAN Node ID of the device in DFU mode.
     --bin=PATH   The path of the .bin file to upload.
     --recovery   Perform recovery procedure for inaccessible DFU bootloader.
     --no-reset   Do not perform a reset following successful flashing.
+    --force      Force flashing even if device memory matches the .bin file.
     --bus=<bus>  One or more interfaces to use, first available is used [default: canine,slcan_disco].
     --chan=<chan>  The bus device "channel".
     --bitrate=<bitrate>  CAN bitrate [default: 1000000].
@@ -23,8 +24,8 @@
 import IPython
 from traitlets.config import Config
 from docopt import docopt
-from tinymovr.tee import init_tee, destroy_tee
-from tinymovr.config import get_bus_config, create_device
+from tinymovr.bus_router import init_router, destroy_router
+from tinymovr.config import get_bus_config, create_device, configure_logging
 from tinymovr.channel import ResponseError
 
 """
@@ -89,6 +90,7 @@ def upload_bin(device, bin_path):
     """
     total_size = os.path.getsize(bin_path)  # Get the total size of .bin file
     uploaded_size = 0
+
     print("\nErasing flash...")
     try:
         # Assume device.erase_all can take hash_validation and attempt to call it
@@ -100,6 +102,7 @@ def upload_bin(device, bin_path):
         print("\nError while erasing!")
         return
     print("Done.")
+    
     with Progress() as progress:
         task2 = progress.add_task("[orange]Flashing...", total=total_size)
         with open(bin_path, "rb") as bin_file:
@@ -146,9 +149,10 @@ def spawn():
     channel = arguments["--chan"]
     bitrate = int(arguments["--bitrate"])
 
+    logger = configure_logging()
+
     if not channel:
-        params = get_bus_config(buses)
-        params["bitrate"] = bitrate
+        params = get_bus_config(buses, bitrate=bitrate)
     else:
         params = {"bustype": buses[0], "channel": channel, "bitrate": bitrate}
 
@@ -156,7 +160,7 @@ def spawn():
 
         input("Please power off the device and then press any key to continue...")
         print("Now power on the device.")
-        init_tee(can.Bus(**params), timeout=1.0)
+        init_router(can.Bus, params, logger=logger, timeout=1.0)
         while True:
             try:
                 device = create_device(node_id=node_id)
@@ -166,7 +170,7 @@ def spawn():
                 pass
     else:
     
-        init_tee(can.Bus(**params), timeout=1.0)
+        init_router(can.Bus, params, logger=logger, timeout=1.0)
         device = create_device(node_id=node_id)
 
         if not bin_path:
@@ -178,7 +182,7 @@ def spawn():
 
         # If an existing .bin file is specified, upload it to the device
         elif bin_path:
-            if compare_bin_w_device(device, bin_path):
+            if (not arguments["--force"]) and compare_bin_w_device(device, bin_path):
                 print("\nDevice memory matches the .bin file. Skipping flashing.")
             else:
                 upload_bin(device, bin_path)
@@ -186,7 +190,7 @@ def spawn():
                 if not arguments["--no-reset"]:
                     print("Resetting device...")
                     device.reset()
-    destroy_tee()
+    destroy_router()
 
 
 if __name__ == "__main__":
diff --git a/studio/Python/tinymovr/gui/__init__.py b/studio/Python/tinymovr/gui/__init__.py
index 9b05a53a..ea6f8ae6 100644
--- a/studio/Python/tinymovr/gui/__init__.py
+++ b/studio/Python/tinymovr/gui/__init__.py
@@ -7,10 +7,14 @@
     display_file_open_dialog,
     display_file_save_dialog,
     magnitude_of,
+    StreamRedirector,
+    QTextBrowserLogger,
     TimedGetter,
     check_selected_items,
     get_dynamic_attrs,
-    is_dark_mode
+    is_dark_mode,
+    strtobool,
+    configure_pretty_errors
 )
 from tinymovr.gui.widgets import (
     NodeTreeWidgetItem,
@@ -19,7 +23,8 @@
     OptionsTreeWidgetItem,
     PlaceholderQTreeWidget,
     IconComboBoxWidget,
-    ArgumentInputDialog
+    ArgumentInputDialog,
+    BoolTreeWidgetItem
 )
 from tinymovr.gui.worker import Worker
 from tinymovr.gui.window import MainWindow
diff --git a/studio/Python/tinymovr/gui/gui.py b/studio/Python/tinymovr/gui/gui.py
index 99539b00..e6473123 100644
--- a/studio/Python/tinymovr/gui/gui.py
+++ b/studio/Python/tinymovr/gui/gui.py
@@ -16,8 +16,10 @@
 import sys
 import yaml
 import pkg_resources
+from importlib_resources import files
 from docopt import docopt
 from PySide6.QtWidgets import QApplication
+from PySide6.QtGui import QIcon, QPixmap
 from tinymovr.gui import MainWindow, app_stylesheet, app_stylesheet_dark, is_dark_mode
 from tinymovr.constants import app_name
 from tinymovr.config import configure_logging, add_spec
@@ -54,10 +56,15 @@ def spawn():
             add_spec(spec_data, logger)
 
     app = QApplication(sys.argv)
+
+    icon_path = files("tinymovr").joinpath("resources/icons/app_icon.png")
+    app.setWindowIcon(QIcon(str(icon_path)))
+
     if is_dark_mode():
         app.setStyleSheet(app_stylesheet_dark)
     else:
         app.setStyleSheet(app_stylesheet)
     w = MainWindow(app, arguments, logger)
+
     w.show()
     sys.exit(app.exec_())
diff --git a/studio/Python/tinymovr/gui/helpers.py b/studio/Python/tinymovr/gui/helpers.py
index 2097787a..ae1202df 100644
--- a/studio/Python/tinymovr/gui/helpers.py
+++ b/studio/Python/tinymovr/gui/helpers.py
@@ -16,16 +16,18 @@
 """
 
 import time
+import logging
+from datetime import datetime
 import os
 import enum
 import pint
 from PySide6 import QtGui
-from PySide6.QtGui import QGuiApplication, QPalette
+from PySide6.QtGui import QGuiApplication, QPalette, QTextCursor
 from PySide6.QtWidgets import QMessageBox, QFileDialog
 from avlos.definitions import RemoteAttribute, RemoteEnum, RemoteBitmask
+import pretty_errors
 import tinymovr
 
-
 app_stylesheet = """
 
 /* --------------------------------------- QPushButton -----------------------------------*/
@@ -201,6 +203,8 @@
 
 app_stylesheet_dark = """
 
+QSplitter::handle { background-color: black; }
+
 /* --------------------------------------- QPushButton -----------------------------------*/
 
     QPushButton {
@@ -464,6 +468,67 @@ def magnitude_of(val):
     return val
 
 
+class StreamRedirector(object):
+    """
+    A class to redirect writes from a stream to a QPlainTextEdit, including pretty_errors handling and timestamps.
+    """
+    def __init__(self, widget):
+        self.widget = widget
+        self.buffer = ''
+
+    def write(self, message):
+        # Timestamp the message
+        timestamp = datetime.now().strftime('%Y-%m-%d %H:%M:%S')
+        message_with_timestamp = f"[{timestamp}] {message}"
+
+        # Redirect to the QPlainTextEdit widget
+        self.widget.moveCursor(QtGui.QTextCursor.End)
+        self.widget.insertPlainText(message_with_timestamp)
+        self.widget.ensureCursorVisible()
+
+    def flush(self):
+        pass
+
+
+class QTextBrowserLogger(logging.Handler):
+    """A logging handler that directs logging output to a QTextBrowser widget."""
+    def __init__(self, widget):
+        super().__init__()
+        self.widget = widget
+        self.widget.setReadOnly(True)
+        self.setFormatter(logging.Formatter('%(levelname)s: %(message)s'))
+
+    def emit(self, record):
+        msg = self.format(record)
+        self.widget.append(self.colorize_message(record.levelno, msg))
+
+    def colorize_message(self, level, message):
+        color = {
+            logging.DEBUG: "lightgreen",
+            logging.INFO: "lightblue",
+            logging.WARNING: "orange",
+            logging.ERROR: "red",
+            logging.CRITICAL: "purple"
+        }.get(level, "black")
+        timestamp = datetime.now().strftime('%H:%M:%S')
+        return f'<font color="gray">[{timestamp}]</font> <font color="{color}">{message}</font>'
+    
+
+def configure_pretty_errors():
+    pretty_errors.configure(
+        separator_character='*',
+        filename_display=pretty_errors.FILENAME_EXTENDED,
+        line_number_first=True,
+        display_link=True,
+        lines_before=5,
+        lines_after=2,
+        line_color=pretty_errors.RED + '> ' + pretty_errors.default_config.line_color,
+        code_color='  ' + pretty_errors.default_config.code_color,
+        truncate_code=True,  # Truncate code lines to not overflow in the GUI
+        display_locals=True
+    )
+
+
 class TimedGetter:
     """
     An interface class that maintains timing
@@ -545,3 +610,19 @@ def get_dynamic_attrs(attr_dict):
             dynamic_attrs.extend(get_dynamic_attrs(attr.remote_attributes))
 
     return dynamic_attrs
+
+
+def strtobool (val):
+    """
+    Convert a string representation of truth to true (1) or false (0).
+    True values are 'y', 'yes', 't', 'true', 'on', and '1'; false values
+    are 'n', 'no', 'f', 'false', 'off', and '0'.  Raises ValueError if
+    'val' is anything else.
+    """
+    val = val.lower()
+    if val in ('y', 'yes', 't', 'true', 'on', '1'):
+        return 1
+    elif val in ('n', 'no', 'f', 'false', 'off', '0'):
+        return 0
+    else:
+        raise ValueError("invalid truth value %r" % (val,))
\ No newline at end of file
diff --git a/studio/Python/tinymovr/gui/widgets.py b/studio/Python/tinymovr/gui/widgets.py
index 21f5d225..1eaa3671 100644
--- a/studio/Python/tinymovr/gui/widgets.py
+++ b/studio/Python/tinymovr/gui/widgets.py
@@ -31,11 +31,12 @@
     QLineEdit,
     QPushButton,
     QFormLayout,
+    QCheckBox
 )
 from pint.errors import UndefinedUnitError
 from avlos import get_registry
 from avlos.datatypes import DataType
-from tinymovr.gui.helpers import load_icon, load_pixmap, format_value
+from tinymovr.gui.helpers import load_icon, load_pixmap, format_value, strtobool
 
 
 class NodeTreeWidgetItem(QTreeWidgetItem):
@@ -239,12 +240,40 @@ def _add_to_tree_cb(self):
         )
         self.treeWidget().setItemWidget(self, 1, self.combo_box_container)
 
+    def set_text(self, value):
+        self.combo_box_container.combo.setCurrentIndex(self._tm_node.options[value])
+
     @QtCore.Slot()
     def _on_combobox_changed(self, index):
         self._tm_node.set_value(index)
         self.combo_box_container.combo.setCurrentIndex(self._tm_node.get_value())
 
 
+class BoolTreeWidgetItem(EdgeTreeWidgetItem):
+
+    def __init__(self, name, node, *args, **kwargs):
+        super().__init__(name, node, *args, **kwargs)
+        self._checkbox = QCheckBox()
+        # Set the initial state of the checkbox based on the attribute value
+        self._checkbox.setChecked(node.get_value())
+        if not self._tm_node.setter_name:
+            self._checkbox.setEnabled(False)
+        else:
+            self._checkbox.stateChanged.connect(self._on_checkbox_state_changed)
+
+    def _add_to_tree_cb(self):
+        self.treeWidget().setItemWidget(self, 1, self._checkbox)
+
+    def set_text(self, value):
+        self._checkbox.setChecked(strtobool(value))
+
+    @QtCore.Slot()
+    def _on_checkbox_state_changed(self, state):
+        # Convert the checkbox state to a boolean and set the attribute value
+        value = state == QtCore.Qt.Checked
+        self._tm_node.set_value(value)
+
+
 class PlaceholderQTreeWidget(QTreeWidget):
     """
     A custom QTreeWidget with support for displaying a placeholder image
diff --git a/studio/Python/tinymovr/gui/window.py b/studio/Python/tinymovr/gui/window.py
index bee0cf0e..1870c695 100644
--- a/studio/Python/tinymovr/gui/window.py
+++ b/studio/Python/tinymovr/gui/window.py
@@ -15,6 +15,7 @@
 this program. If not, see <http://www.gnu.org/licenses/>.
 """
 
+import sys
 import time
 import logging
 import pkg_resources
@@ -28,11 +29,13 @@
     QMenuBar,
     QWidget,
     QFrame,
-    QHBoxLayout,
     QVBoxLayout,
     QHeaderView,
     QLabel,
     QMessageBox,
+    QTreeWidgetItem,
+    QSplitter,
+    QTextBrowser,
 )
 from PySide6.QtGui import QAction
 import pyqtgraph as pg
@@ -40,6 +43,7 @@
 from tinymovr.channel import ResponseError as ChannelResponseError
 from tinymovr.config import get_bus_config
 from avlos import get_registry
+from avlos.datatypes import DataType
 from avlos.json_codec import AvlosEncoder
 from tinymovr.gui import (
     NodeTreeWidgetItem,
@@ -48,28 +52,33 @@
     OptionsTreeWidgetItem,
     Worker,
     PlaceholderQTreeWidget,
+    BoolTreeWidgetItem,
+    StreamRedirector,
+    QTextBrowserLogger,
     format_value,
     display_file_open_dialog,
     display_file_save_dialog,
     magnitude_of,
     check_selected_items,
+    configure_pretty_errors,
 )
 
 
 class MainWindow(QMainWindow):
     TreeItemCheckedSignal = Signal(dict)
+    updateVisibleAttrsSignal = Signal(set)
 
     def __init__(self, app, arguments, logger):
         super(MainWindow, self).__init__()
 
+        self.time_window = 10
+
         # set units default format
         get_registry().default_format = ".6f~"
 
         self.start_time = time.time()
-        if logger is None:
-            self.logger = logging.getLogger("tinymovr")
-        else:
-            self.logger = logger
+        self.logger = logger if logger is not None else logging.getLogger("tinymovr")
+        configure_pretty_errors()
 
         self.attr_widgets_by_id = {}
         self.graphs_by_id = {}
@@ -81,6 +90,7 @@ def __init__(self, app, arguments, logger):
 
         self.file_menu = QMenu("File")
         self.help_menu = QMenu("Help")
+        self.view_menu = QMenu("View")
 
         self.export_action = QAction("Export Config...", self)
         self.import_action = QAction("Import Config", self)
@@ -92,22 +102,81 @@ def __init__(self, app, arguments, logger):
         self.file_menu.addAction(self.import_action)
         self.help_menu.addAction(self.about_action)
 
+        self.toggle_tree_action = QAction(
+            "Hide Tree", self
+        )  # Assume tree is visible initially
+        self.toggle_tree_action.triggered.connect(self.toggle_tree)
+        self.toggle_console_action = QAction(
+            "Hide Console", self
+        )  # Assume console is visible initially
+        self.toggle_console_action.triggered.connect(self.toggle_console)
+
+        self.clear_console_action = QAction("Clear Console", self)
+        self.clear_console_action.triggered.connect(self.clear_console)
+        self.view_menu.addAction(self.clear_console_action)
+
+        self.view_menu.addAction(self.toggle_tree_action)
+        self.view_menu.addAction(self.toggle_console_action)
+
+        self.time_window_menu = QMenu("Set Time Window")
+
+        self.time_window_10s_action = QAction("10 seconds", self)
+        self.time_window_10s_action.triggered.connect(lambda: self.set_time_window(10))
+
+        self.time_window_30s_action = QAction("30 seconds", self)
+        self.time_window_30s_action.triggered.connect(lambda: self.set_time_window(30))
+
+        self.time_window_60s_action = QAction("60 seconds", self)
+        self.time_window_60s_action.triggered.connect(lambda: self.set_time_window(60))
+
+        self.time_window_menu.addAction(self.time_window_10s_action)
+        self.time_window_menu.addAction(self.time_window_30s_action)
+        self.time_window_menu.addAction(self.time_window_60s_action)
+
+        self.view_menu.addMenu(self.time_window_menu)
+
+        self.timer_rate_menu = QMenu("Set Timer Rate")
+
+        self.timer_rate_12_5_action = QAction("12.5 Hz", self)
+        self.timer_rate_12_5_action.triggered.connect(lambda: self.set_timer_rate(80))
+
+        self.timer_rate_25_action = QAction("25 Hz", self)
+        self.timer_rate_25_action.triggered.connect(lambda: self.set_timer_rate(40))
+
+        self.timer_rate_50_action = QAction("50 Hz", self)
+        self.timer_rate_50_action.triggered.connect(lambda: self.set_timer_rate(20))
+
+        self.timer_rate_100_action = QAction("100 Hz", self)
+        self.timer_rate_100_action.triggered.connect(lambda: self.set_timer_rate(10))
+
+        self.timer_rate_menu.addAction(self.timer_rate_12_5_action)
+        self.timer_rate_menu.addAction(self.timer_rate_25_action)
+        self.timer_rate_menu.addAction(self.timer_rate_50_action)
+        self.timer_rate_menu.addAction(self.timer_rate_100_action)
+
+        self.view_menu.addMenu(self.timer_rate_menu)
+
         self.menu_bar.addMenu(self.file_menu)
+        self.menu_bar.addMenu(self.view_menu)
         self.menu_bar.addMenu(self.help_menu)
         self.setMenuBar(self.menu_bar)
 
-        # Setup the tree widget
         self.tree_widget = PlaceholderQTreeWidget()
         self.tree_widget.itemChanged.connect(self.item_changed)
+        self.tree_widget.itemExpanded.connect(self.update_visible_attrs)
+        self.tree_widget.itemCollapsed.connect(self.update_visible_attrs)
         self.tree_widget.setHeaderLabels(["Attribute", "Value"])
 
         self.status_label = QLabel()
         self.status_label.setStyleSheet("margin: 5px;")
 
+        self.splitter = QSplitter(QtCore.Qt.Horizontal)
+        self.splitter.setHandleWidth(0)
+        self.splitter.splitterMoved.connect(self.check_tree_visibility)
+
         self.left_frame = QFrame(self)
         self.left_layout = QVBoxLayout()
         self.left_layout.addWidget(self.tree_widget)
-        self.left_layout.addWidget(self.status_label)
         self.left_layout.setSpacing(0)
         self.left_layout.setContentsMargins(0, 0, 0, 0)
         self.left_frame.setLayout(self.left_layout)
@@ -121,9 +190,24 @@ def __init__(self, app, arguments, logger):
         self.right_layout.setContentsMargins(0, 0, 0, 0)
         self.right_frame.setLayout(self.right_layout)
 
-        main_layout = QHBoxLayout()
-        main_layout.addWidget(self.left_frame)
-        main_layout.addWidget(self.right_frame)
+        self.splitter.addWidget(self.left_frame)
+        self.splitter.addWidget(self.right_frame)
+
+        self.console = QTextBrowser()
+        self.console.setReadOnly(True)
+        self.log_handler = QTextBrowserLogger(self.console)
+        self.logger.addHandler(self.log_handler)
+
+        self.main_splitter = QSplitter(QtCore.Qt.Vertical)
+        self.main_splitter.setHandleWidth(0)
+        self.main_splitter.showEvent = self.on_show_event
+        self.main_splitter.splitterMoved.connect(self.check_console_visibility)
+        self.main_splitter.addWidget(self.splitter)
+        self.main_splitter.addWidget(self.console)
+
+        main_layout = QVBoxLayout()
+        main_layout.addWidget(self.main_splitter)
+        main_layout.addWidget(self.status_label)
         main_layout.setSpacing(0)
         main_layout.setContentsMargins(0, 0, 0, 0)
 
@@ -140,8 +224,7 @@ def __init__(self, app, arguments, logger):
         self.max_timeouts = int(arguments["--max-timeouts"])
 
         if channel == None:
-            params = get_bus_config(buses)
-            params["bitrate"] = bitrate
+            params = get_bus_config(buses, bitrate=bitrate)
         else:
             params = {"bustype": buses[0], "channel": channel, "bitrate": bitrate}
 
@@ -158,15 +241,33 @@ def __init__(self, app, arguments, logger):
         self.worker.updateTimingsSignal.connect(
             self.timings_updated, QtCore.Qt.QueuedConnection
         )
+        self.updateVisibleAttrsSignal.connect(self.worker.update_visible_attrs)
         app.aboutToQuit.connect(self.about_to_quit)
 
-        self.timer = QTimer(self)
-        self.timer.timeout.connect(self.worker._update)
-        self.timer.start(40)
+        self.worker_update_timer = QTimer(self)
+        self.worker_update_timer.timeout.connect(self.worker._update)
+        self.worker_update_timer.start(40)
+
+        self.visibility_update_timer = QTimer(self)
+        self.visibility_update_timer.timeout.connect(self.update_visible_attrs)
+        self.visibility_update_timer.start(1000)
+
+    def on_show_event(self, event):
+        total_height = self.splitter.size().height()
+        top_percentage = 0.75  # 75% for the top widget
+        bottom_percentage = 0.25  # 25% for the bottom widget
+
+        top_size = int(total_height * top_percentage)
+        bottom_size = int(total_height * bottom_percentage)
+
+        self.main_splitter.setSizes([top_size, bottom_size])
+
+        super(MainWindow, self).showEvent(event)
 
     @QtCore.Slot()
     def about_to_quit(self):
-        self.timer.stop()
+        self.visibility_update_timer.stop()
+        self.worker_update_timer.stop()
         self.worker.stop()
 
     @QtCore.Slot()
@@ -208,6 +309,19 @@ def add_graph_for_attr(self, attr):
         }
         self.right_layout.addWidget(graph_widget)
 
+    @QtCore.Slot()
+    def update_visible_attrs(self):
+        """
+        Collects names of visible attributes and emits a signal with these names.
+        """
+        visible_attrs = {
+            attr_name
+            for attr_name, widget_info in self.attr_widgets_by_id.items()
+            if self.is_widget_visible(widget_info["widget"])
+            and self.is_item_visible_in_viewport(widget_info["widget"])
+        }
+        self.updateVisibleAttrsSignal.emit(visible_attrs)
+
     @QtCore.Slot()
     def regen_tree(self, devices_by_name):
         """
@@ -237,7 +351,10 @@ def parse_node(self, node, name):
             if hasattr(node, "options"):
                 widget = OptionsTreeWidgetItem(name, node)
             elif hasattr(node, "get_value"):
-                widget = AttrTreeWidgetItem(name, node)
+                if node.dtype == DataType.BOOL:
+                    widget = BoolTreeWidgetItem(name, node)
+                else:
+                    widget = AttrTreeWidgetItem(name, node)
             self.attr_widgets_by_id[node.full_name] = {
                 "node": node,
                 "widget": widget,
@@ -265,16 +382,21 @@ def attrs_updated(self, data):
                 self.logger.warn("Attribute widget disappeared while updating")
             if attr_name in self.graphs_by_id:
                 graph_info = self.graphs_by_id[attr_name]
+                current_time = time.time() - self.start_time
                 x = graph_info["data"]["x"]
                 y = graph_info["data"]["y"]
-                if len(x) >= 200:
+
+                # Remove data points outside the time window
+                while len(x) > 0 and current_time - x[0] > self.time_window:
                     x.pop(0)
                     y.pop(0)
-                x.append(time.time() - self.start_time)
+
+                x.append(current_time)
                 y.append(magnitude_of(val))
                 graph_info["data_line"].setData(x, y)
                 graph_info["widget"].update()
 
+
     @QtCore.Slot()
     def timings_updated(self, timings_dict):
         meas_freq = timings_dict["meas_freq"]
@@ -326,3 +448,113 @@ def show_about_box(self):
                 app_str
             ),
         )
+
+    def toggle_tree(self):
+        """
+        Toggle the visibility of the tree based on actual size.
+        """
+        tree_size = self.splitter.sizes()[0]
+        if tree_size > 0:
+            self.tree_widget.setVisible(False)
+            self.splitter.setSizes([0, self.splitter.size().width()])
+            self.toggle_tree_action.setText("Show Tree")
+        else:
+            self.tree_widget.setVisible(True)
+            total_size = self.splitter.size().width()
+            left_size = int(total_size * 0.25)
+            right_size = int(total_size * 0.75)
+            self.splitter.setSizes([left_size, right_size])
+            self.toggle_tree_action.setText("Hide Tree")
+
+    def toggle_console(self):
+        """
+        Toggle the visibility of the console based on actual size.
+        """
+        console_height = self.main_splitter.sizes()[-1]
+        if console_height > 0:
+            self.console.setVisible(False)
+            self.main_splitter.setSizes([self.main_splitter.size().height(), 0])
+            self.toggle_console_action.setText("Show Console")
+        else:
+            self.console.setVisible(True)
+            total_height = self.main_splitter.size().height()
+            top_height = int(total_height * 0.75)
+            bottom_height = int(total_height * 0.25)
+            self.main_splitter.setSizes([top_height, bottom_height])
+            self.toggle_console_action.setText("Hide Console")
+
+    def clear_console(self):
+        """
+        Clear the console output.
+        """
+        self.console.clear()
+
+    def check_tree_visibility(self, pos, index):
+        """
+        Check tree visibility after splitter is moved and update the action text.
+        """
+        tree_size = self.splitter.sizes()[
+            0
+        ]  # Assuming tree is always the first widget in the splitter
+        if tree_size == 0:
+            self.toggle_tree_action.setText("Show Tree")
+        else:
+            self.toggle_tree_action.setText("Hide Tree")
+
+    def check_console_visibility(self, pos, index):
+        """
+        Check console visibility after splitter is moved and update the action text.
+        """
+        console_size = self.main_splitter.sizes()[
+            -1
+        ]  # Assuming console is always the last widget in the splitter
+        if console_size == 0:
+            self.toggle_console_action.setText("Show Console")
+        else:
+            self.toggle_console_action.setText("Hide Console")
+
+    def is_widget_visible(self, widget):
+        """
+        Check if the given widget is visible, i.e., not hidden and all its
+        ancestor widgets are expanded.
+        """
+        if widget.isHidden():
+            return False
+        parent = widget.parent()
+        while parent is not None:
+            if isinstance(parent, QTreeWidgetItem) and not parent.isExpanded():
+                return False
+            parent = parent.parent()
+        return True
+
+    def is_item_visible_in_viewport(self, item):
+        """
+        Check if the QTreeWidgetItem is visible in the viewport of the QTreeWidget.
+        """
+        # Get the item's rectangle in tree widget coordinates
+        rect = self.tree_widget.visualItemRect(item)
+
+        # Check if the rectangle is within the visible region of the tree widget
+        visible_region = self.tree_widget.visibleRegion()
+        return visible_region.contains(rect)
+
+    def set_timer_rate(self, interval_ms):
+        """
+        Set the interval of the worker update timer.
+        :param interval_ms: Timer interval in milliseconds.
+        """
+        self.worker_update_timer.setInterval(interval_ms)
+        self.logger.info(f"Worker update timer set to {1000 / interval_ms:.1f} Hz")
+
+    def set_time_window(self, time_window):
+        """
+        Set the time window for the plots.
+        :param time_window: Time window in seconds.
+        """
+        self.time_window = time_window
+        self.logger.info(f"Time window set to {time_window} seconds")
+        # # Clear existing data to apply the new time window
+        # for graph_info in self.graphs_by_id.values():
+        #     graph_info["data"]["x"].clear()
+        #     graph_info["data"]["y"].clear()
+        #     graph_info["data_line"].setData([], [])
diff --git a/studio/Python/tinymovr/gui/worker.py b/studio/Python/tinymovr/gui/worker.py
index 3932d9a6..eb9066c2 100644
--- a/studio/Python/tinymovr/gui/worker.py
+++ b/studio/Python/tinymovr/gui/worker.py
@@ -20,9 +20,10 @@
 import can
 from PySide6 import QtCore
 from PySide6.QtCore import QObject
+from tinymovr.channel import ResponseError
 from tinymovr.gui import TimedGetter, get_dynamic_attrs
-from tinymovr.tee import init_tee, destroy_tee
-from tinymovr.discovery import Discovery
+from tinymovr.bus_router import init_router, destroy_router
+from tinymovr.device_discovery import DeviceDiscovery
 from tinymovr.constants import base_node_name
 
 
@@ -32,24 +33,29 @@ class Worker(QObject):
     regenSignal = QtCore.Signal(dict)
     handleErrorSignal = QtCore.Signal(object)
 
-    def __init__(self, busparams, logger):
+    def __init__(self, bus_params, logger, bus_class=can.Bus, refresh_period=0.3):
         super().__init__()
         self.logger = logger
         self.mutx = QtCore.QMutex()
-        init_tee(can.Bus(**busparams))
+        #TODO: router init should not happen in GUI worker
+        init_router(bus_class, bus_params, logger=logger)
         self._init_containers()
-        self.dsc = Discovery(
+        self.dsc = DeviceDiscovery(
             self._device_appeared, self._device_disappeared, self.logger
         )
         self.timed_getter = TimedGetter()
 
+        self.refresh_period = refresh_period
+
         self.dt_update = 1
         self.dt_load = 0
         self.t_last_update = time.time()
 
+        self.visible_attrs = set()
+
     @QtCore.Slot()
     def stop(self):
-        destroy_tee()
+        destroy_router()
 
     def force_regen(self):
         self.mutx.lock()
@@ -57,6 +63,7 @@ def force_regen(self):
         self.mutx.unlock()
 
     def reset(self):
+        self.logger.info("Resetting Studio...")
         self.mutx.lock()
         self.dsc.reset()
         self._init_containers()
@@ -78,18 +85,19 @@ def _get_attr_values(self):
             except Exception as e:
                 self.handleErrorSignal.emit(e)
         start_time = time.time()
-        self.dynamic_attrs.sort(
+        attrs_to_update = [attr for attr in self.dynamic_attrs if attr.full_name in self.visible_attrs]
+        attrs_to_update.sort(
             key=lambda attr: self.dynamic_attrs_last_update[attr.full_name]
             if attr.full_name in self.dynamic_attrs_last_update
             else 0
         )
-        for attr in self.dynamic_attrs:
+        for attr in attrs_to_update:
             t = (
                 self.dynamic_attrs_last_update[attr.full_name]
                 if attr.full_name in self.dynamic_attrs_last_update
                 else 0
             )
-            if (attr.full_name not in vals) and (start_time - t > 0.5):
+            if (attr.full_name not in vals) and (start_time - t > self.refresh_period):
                 try:
                     vals[attr.full_name] = self.timed_getter.get_value(attr.get_value)
                     self.dynamic_attrs_last_update[attr.full_name] = start_time
@@ -97,6 +105,13 @@ def _get_attr_values(self):
                     self.handleErrorSignal.emit(e)
                 break
         return vals
+    
+    @QtCore.Slot(set)
+    def update_visible_attrs(self, attrs):
+        """
+        Update the set of visible attributes based on the signal from the main window.
+        """
+        self.visible_attrs = attrs
 
     def _init_containers(self):
         self.active_attrs = set()
@@ -126,7 +141,11 @@ def _update(self):
 
     def _device_appeared(self, device, node_id):
         self.mutx.lock()
-        display_name = "{}{}".format(device.name, node_id)
+        try:
+            display_name = "{}{}".format(device.name, node_id)
+            self.logger.info("Found {} (uid {})".format(display_name, device.uid))
+        except ResponseError as e:
+            self.handleErrorSignal.emit(e)
         self.devices_by_name[display_name] = device
         self.names_by_id[node_id] = display_name
         device.name = display_name
diff --git a/studio/Python/tinymovr/resources/icons/app_icon.png b/studio/Python/tinymovr/resources/icons/app_icon.png
new file mode 100644
index 00000000..1065b49d
Binary files /dev/null and b/studio/Python/tinymovr/resources/icons/app_icon.png differ
diff --git a/studio/Python/tinymovr/specs/tinymovr_2_0_x.yaml b/studio/Python/tinymovr/specs/tinymovr_2_0_x.yaml
new file mode 100644
index 00000000..a1ad30f2
--- /dev/null
+++ b/studio/Python/tinymovr/specs/tinymovr_2_0_x.yaml
@@ -0,0 +1,622 @@
+
+name: tm
+remote_attributes:
+  - name: protocol_hash
+    dtype: uint32
+    getter_name: _avlos_get_proto_hash
+    summary: The Avlos protocol hash.
+  - name: uid
+    dtype: uint32
+    getter_name: system_get_uid
+    summary: The unique device ID, unique to each PAC55xx chip produced.
+  - name: fw_version
+    dtype: string
+    getter_name: system_get_fw_version_string
+    summary: The firmware version.
+  - name: hw_revision
+    dtype: uint32
+    getter_name: system_get_hw_revision
+    summary: The hardware revision.
+  - name: Vbus
+    dtype: float
+    unit: volt
+    meta: {dynamic: True}
+    getter_name: system_get_Vbus
+    summary: The measured bus voltage.
+  - name: Ibus
+    dtype: float
+    unit: ampere
+    meta: {dynamic: True}
+    getter_name: controller_get_Ibus_est
+    summary: The estimated bus current. Only estimates current drawn by motor.
+  - name: power
+    dtype: float
+    unit: watt
+    meta: {dynamic: True}
+    getter_name: controller_get_power_est
+    summary: The estimated power. Only estimates power drawn by motor.
+  - name: temp
+    dtype: float
+    unit: degC
+    meta: {dynamic: True}
+    getter_name: ADC_get_mcu_temp
+    summary: The internal temperature of the PAC55xx MCU.
+  - name: calibrated
+    dtype: bool
+    meta: {dynamic: True}
+    getter_name: system_get_calibrated
+    summary: Whether the system has been calibrated.
+  - name: errors
+    flags: [UNDERVOLTAGE]
+    meta: {dynamic: True}
+    getter_name: system_get_errors
+    summary: Any system errors, as a bitmask
+  - name: warnings
+    flags: [DRIVER_FAULT, CHARGE_PUMP_FAULT_STAT, CHARGE_PUMP_FAULT, DRV10_DISABLE, DRV32_DISABLE, DRV54_DISABLE]
+    meta: {dynamic: True}
+    getter_name: system_get_warnings
+    summary: Any system warnings, as a bitmask
+  - name: save_config
+    summary: Save configuration to non-volatile memory.
+    caller_name: nvm_save_config
+    dtype: void
+    arguments: []
+  - name: erase_config
+    summary: Erase the config stored in non-volatile memory and reset the device.
+    caller_name: nvm_erase
+    dtype: void
+    arguments: []
+    meta: {reload_data: True}
+  - name: reset
+    summary: Reset the device.
+    caller_name: system_reset
+    dtype: void
+    arguments: []
+    meta: {reload_data: True}
+  - name: enter_dfu
+    summary: Enter DFU mode.
+    caller_name: system_enter_dfu
+    dtype: void
+    arguments: []
+    meta: {reload_data: True}
+  - name: config_size
+    summary: Size (in bytes) of the configuration object.
+    getter_name: system_get_config_size
+    dtype: uint32
+  - name: scheduler
+    remote_attributes:
+      - name: load
+        summary: Processor load in ticks per PWM cycle.
+        getter_name: scheduler_get_load
+        meta: {dynamic: True}
+        dtype: uint32
+      - name: warnings
+        flags: [CONTROL_BLOCK_REENTERED]
+        meta: {dynamic: True}
+        getter_name: scheduler_get_warnings
+        summary: Any scheduler warnings, as a bitmask
+  - name: controller
+    remote_attributes:
+      - name: state
+        options: [IDLE, CALIBRATE, CL_CONTROL]
+        meta: {dynamic: True}
+        getter_name: controller_get_state
+        setter_name: controller_set_state
+        summary: The state of the controller.
+      - name: mode
+        options: [CURRENT, VELOCITY, POSITION, TRAJECTORY, HOMING]
+        meta: {dynamic: True}
+        getter_name: controller_get_mode
+        setter_name: controller_set_mode
+        summary: The control mode of the controller.
+      - name: warnings
+        meta: {dynamic: True}
+        flags: [VELOCITY_LIMITED, CURRENT_LIMITED, MODULATION_LIMITED]
+        getter_name: controller_get_warnings
+        summary: Any controller warnings, as a bitmask
+      - name: errors
+        meta: {dynamic: True}
+        flags: [CURRENT_LIMIT_EXCEEDED]
+        getter_name: controller_get_errors
+        summary: Any controller errors, as a bitmask
+      - name: position
+        remote_attributes:
+          - name: setpoint
+            dtype: float
+            unit: tick
+            meta: {jog_step: 100}
+            getter_name: controller_get_pos_setpoint_user_frame
+            setter_name: controller_set_pos_setpoint_user_frame
+            summary: The position setpoint in the user reference frame.
+          - name: p_gain
+            dtype: float
+            meta: {export: True}
+            getter_name: controller_get_pos_gain
+            setter_name: controller_set_pos_gain
+            summary: The proportional gain of the position controller.
+      - name: velocity
+        remote_attributes:
+          - name: setpoint
+            dtype: float
+            unit: tick/sec
+            meta: {jog_step: 200}
+            getter_name: controller_get_vel_setpoint_user_frame
+            setter_name: controller_set_vel_setpoint_user_frame
+            summary: The velocity setpoint in the user reference frame.
+          - name: limit
+            dtype: float
+            unit: tick/sec
+            meta: {export: True}
+            getter_name: controller_get_vel_limit
+            setter_name: controller_set_vel_limit
+            summary: The velocity limit.
+          - name: p_gain
+            dtype: float
+            meta: {export: True}
+            getter_name: controller_get_vel_gain
+            setter_name: controller_set_vel_gain
+            summary: The proportional gain of the velocity controller.
+          - name: i_gain
+            dtype: float
+            meta: {export: True}
+            getter_name: controller_get_vel_integral_gain
+            setter_name: controller_set_vel_integral_gain
+            summary: The integral gain of the velocity controller.
+          - name: deadband
+            dtype: float
+            unit: tick
+            meta: {export: True}
+            getter_name: controller_get_vel_integral_deadband
+            setter_name: controller_set_vel_integral_deadband
+            rst_target: integrator-deadband
+            summary: The deadband of the velocity integrator. A region around the position setpoint where the velocity integrator is not updated.
+          - name: increment
+            dtype: float
+            meta: {export: True}
+            getter_name: controller_get_vel_increment
+            setter_name: controller_set_vel_increment
+            summary: Max velocity setpoint increment (ramping) rate. Set to 0 to disable.
+      - name: current
+        remote_attributes:
+          - name: Iq_setpoint
+            dtype: float
+            unit: ampere
+            meta: {jog_step: 0.005}
+            getter_name: controller_get_Iq_setpoint_user_frame
+            setter_name: controller_set_Iq_setpoint_user_frame
+            summary: The Iq setpoint in the user reference frame.
+          - name: Id_setpoint
+            dtype: float
+            unit: ampere
+            meta: {dynamic: True}
+            getter_name: controller_get_Id_setpoint_user_frame
+            summary: The Id setpoint in the user reference frame.
+          - name: Iq_limit
+            dtype: float
+            unit: ampere
+            getter_name: controller_get_Iq_limit
+            setter_name: controller_set_Iq_limit
+            summary: The Iq limit.
+          - name: Iq_estimate
+            dtype: float
+            unit: ampere
+            meta: {dynamic: True}
+            getter_name: controller_get_Iq_estimate_user_frame
+            summary: The Iq estimate in the user reference frame.
+          - name: bandwidth
+            dtype: float
+            unit: Hz
+            meta: {export: True}
+            getter_name: controller_get_I_bw
+            setter_name: controller_set_I_bw
+            summary: The current controller bandwidth.
+          - name: Iq_p_gain
+            dtype: float
+            getter_name: controller_get_Iq_gain
+            summary: The current controller proportional gain.
+          - name: max_Ibus_regen
+            dtype: float
+            unit: ampere
+            getter_name: controller_get_max_Ibus_regen
+            setter_name: controller_set_max_Ibus_regen
+            summary: The max current allowed to be fed back to the power source before flux braking activates.
+          - name: max_Ibrake
+            dtype: float
+            unit: ampere
+            meta: {export: True}
+            getter_name: controller_get_max_Ibrake
+            setter_name: controller_set_max_Ibrake
+            summary: The max current allowed to be dumped to the motor windings during flux braking. Set to zero to deactivate flux braking.
+      - name: voltage
+        remote_attributes:
+          - name: Vq_setpoint
+            dtype: float
+            unit: volt
+            meta: {dynamic: True}
+            getter_name: controller_get_Vq_setpoint_user_frame
+            summary: The Vq setpoint.
+      - name: calibrate
+        summary: Calibrate the device.
+        caller_name: controller_calibrate
+        dtype: void
+        arguments: []
+      - name: idle
+        summary: Set idle mode, disabling the driver.
+        caller_name: controller_idle
+        dtype: void
+        arguments: []
+      - name: position_mode
+        summary: Set position control mode.
+        caller_name: controller_position_mode
+        dtype: void
+        arguments: []
+      - name: velocity_mode
+        summary: Set velocity control mode.
+        caller_name: controller_velocity_mode
+        dtype: void
+        arguments: []
+      - name: current_mode
+        summary: Set current control mode.
+        caller_name: controller_current_mode
+        dtype: void
+        arguments: []
+      - name: set_pos_vel_setpoints
+        summary: Set the position and velocity setpoints in the user reference frame in one go, and retrieve the position estimate
+        caller_name: controller_set_pos_vel_setpoints_user_frame
+        dtype: float
+        arguments:
+          - name: pos_setpoint
+            dtype: float
+            unit: tick
+          - name: vel_setpoint
+            dtype: float
+            unit: tick
+  - name: comms
+    remote_attributes:
+    - name: can
+      remote_attributes:
+      - name: rate
+        dtype: uint32
+        meta: {export: True}
+        getter_name: CAN_get_kbit_rate
+        setter_name: CAN_set_kbit_rate
+        rst_target: api-can-rate
+        summary: The baud rate of the CAN interface.
+      - name: id
+        dtype: uint32
+        meta: {export: True, reload_data: True}
+        getter_name: CAN_get_ID
+        setter_name: CAN_set_ID
+        summary: The ID of the CAN interface.
+      - name: heartbeat
+        dtype: bool
+        getter_name: CAN_get_send_heartbeat
+        setter_name: CAN_set_send_heartbeat
+        summary: Toggle sending of heartbeat messages.
+  - name: motor
+    remote_attributes:
+      - name: R
+        dtype: float
+        unit: ohm
+        meta: {dynamic: True, export: True}
+        getter_name: motor_get_phase_resistance
+        setter_name: motor_set_phase_resistance
+        summary: The motor Resistance value.
+      - name: L
+        dtype: float
+        unit: henry
+        meta: {dynamic: True, export: True}
+        getter_name: motor_get_phase_inductance
+        setter_name: motor_set_phase_inductance
+        summary: The motor Inductance value.
+      - name: pole_pairs
+        dtype: uint8
+        meta: {dynamic: True, export: True}
+        getter_name: motor_get_pole_pairs
+        setter_name: motor_set_pole_pairs
+        summary: The motor pole pair count.
+      - name: type
+        options: [HIGH_CURRENT, GIMBAL]
+        meta: {export: True}
+        getter_name: motor_get_is_gimbal
+        setter_name: motor_set_is_gimbal
+        summary: The type of the motor. Either high current or gimbal.
+      - name: calibrated
+        dtype: bool
+        meta: {dynamic: True}
+        getter_name: motor_get_calibrated
+        summary: Whether the motor has been calibrated.
+      - name: I_cal
+        dtype: float
+        unit: ampere
+        meta: {export: True}
+        getter_name: motor_get_I_cal
+        setter_name: motor_set_I_cal
+        summary: The calibration current.
+      - name: errors
+        flags: [PHASE_RESISTANCE_OUT_OF_RANGE, PHASE_INDUCTANCE_OUT_OF_RANGE,INVALID_POLE_PAIRS]
+        meta: {dynamic: True}
+        getter_name: motor_get_errors
+        summary: Any motor/calibration errors, as a bitmask
+  - name: sensors
+    remote_attributes:
+      - name: user_frame
+        remote_attributes:
+          - name: position_estimate
+            dtype: float
+            unit: ticks
+            meta: {dynamic: True}
+            getter_name: user_frame_get_pos_estimate
+            summary: The filtered position estimate in the user reference frame.
+          - name: velocity_estimate
+            dtype: float
+            unit: ticks/second
+            meta: {dynamic: True}
+            getter_name: user_frame_get_vel_estimate
+            summary: The filtered velocity estimate in the user reference frame.
+          - name: offset
+            dtype: float
+            unit: ticks
+            getter_name: frame_user_to_position_sensor_get_offset
+            setter_name: frame_user_to_position_sensor_set_offset
+            summary: The user defined offset.
+          - name: multiplier
+            dtype: float
+            getter_name: frame_user_to_position_sensor_get_multiplier
+            setter_name: frame_user_to_position_sensor_set_multiplier
+            summary: The user defined multipler.
+      - name: setup
+        remote_attributes:
+          - name: onboard
+            remote_attributes:
+              - name: calibrated
+                dtype: bool
+                meta: {dynamic: True}
+                getter_name: sensor_onboard_get_is_calibrated
+                summary: Whether the sensor has been calibrated.
+              - name: errors
+                flags: [CALIBRATION_FAILED, READING_UNSTABLE]
+                meta: {dynamic: True}
+                getter_name: sensor_onboard_get_errors
+                summary: Any sensor errors, as a bitmask
+          - name: external_spi
+            remote_attributes:
+              - name: type
+                options: [MA7XX, AS5047, AMT22]
+                meta: {export: True}
+                getter_name: sensor_external_spi_get_type_avlos
+                setter_name: sensor_external_spi_set_type_avlos
+                summary: The type of the external sensor.
+              - name: rate
+                options: [1_5Mbps, 3Mbps, 6Mbps, 8Mbps, 12Mbps]
+                meta: {export: True, dynamic: True}
+                getter_name: sensor_external_spi_get_rate_avlos
+                setter_name: sensor_external_spi_set_rate_avlos
+                summary: The rate of the external sensor.
+              - name: calibrated
+                dtype: bool
+                meta: {dynamic: True}
+                getter_name: sensor_external_spi_get_is_calibrated
+                summary: Whether the sensor has been calibrated.
+              - name: errors
+                flags: [CALIBRATION_FAILED, READING_UNSTABLE]
+                meta: {dynamic: True}
+                getter_name: sensor_external_spi_get_errors
+                summary: Any sensor errors, as a bitmask
+          - name: hall
+            remote_attributes:
+              - name: calibrated
+                dtype: bool
+                meta: {dynamic: True}
+                getter_name: sensor_hall_get_is_calibrated
+                summary: Whether the sensor has been calibrated.
+              - name: errors
+                flags: [CALIBRATION_FAILED, READING_UNSTABLE]
+                meta: {dynamic: True}
+                getter_name: sensor_hall_get_errors
+                summary: Any sensor errors, as a bitmask
+      - name: select
+        remote_attributes:
+          - name: position_sensor
+            remote_attributes:
+              - name: connection
+                options: [ONBOARD, EXTERNAL_SPI, HALL]
+                meta: {export: True}
+                getter_name: position_sensor_get_connection
+                setter_name: position_sensor_set_connection
+                summary: The position sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL.
+              - name: bandwidth
+                dtype: float
+                unit: Hz
+                meta: {export: True}
+                getter_name: position_observer_get_bandwidth
+                setter_name: position_observer_set_bandwidth
+                summary: The position sensor observer bandwidth.
+              - name: raw_angle
+                dtype: int32
+                meta: {dynamic: True}
+                getter_name: sensor_position_get_raw_angle
+                summary: The raw position sensor angle.
+              - name: position_estimate
+                dtype: float
+                unit: ticks
+                meta: {dynamic: True}
+                getter_name: position_observer_get_pos_estimate
+                summary: The filtered position estimate in the position sensor reference frame.
+              - name: velocity_estimate
+                dtype: float
+                unit: ticks/second
+                meta: {dynamic: True}
+                getter_name: position_observer_get_vel_estimate
+                summary: The filtered velocity estimate in the position sensor reference frame.
+          - name: commutation_sensor
+            remote_attributes:
+              - name: connection
+                options: [ONBOARD, EXTERNAL_SPI, HALL]
+                meta: {export: True}
+                getter_name: commutation_sensor_get_connection
+                setter_name: commutation_sensor_set_connection
+                summary: The commutation sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL.
+              - name: bandwidth
+                dtype: float
+                unit: Hz
+                meta: {export: True}
+                getter_name: commutation_observer_get_bandwidth
+                setter_name: commutation_observer_set_bandwidth
+                summary: The commutation sensor observer bandwidth.
+              - name: raw_angle
+                dtype: int32
+                meta: {dynamic: True}
+                getter_name: sensor_commutation_get_raw_angle
+                summary: The raw commutation sensor angle.
+              - name: position_estimate
+                dtype: float
+                unit: ticks
+                meta: {dynamic: True}
+                getter_name: commutation_observer_get_pos_estimate
+                summary: The filtered position estimate in the commutation sensor reference frame.
+              - name: velocity_estimate
+                dtype: float
+                unit: ticks/second
+                meta: {dynamic: True}
+                getter_name: commutation_observer_get_vel_estimate
+                summary: The filtered velocity estimate in the commutation sensor reference frame.
+  - name: traj_planner
+    remote_attributes:
+      - name: max_accel
+        dtype: float
+        unit: ticks/s
+        meta: {export: True}
+        getter_name: planner_get_max_accel
+        setter_name: planner_set_max_accel
+        summary: The max allowed acceleration of the generated trajectory.
+      - name: max_decel
+        dtype: float
+        unit: ticks/second/second
+        meta: {export: True}
+        getter_name: planner_get_max_decel
+        setter_name: planner_set_max_decel
+        summary: The max allowed deceleration of the generated trajectory.
+      - name: max_vel
+        dtype: float
+        unit: ticks/second
+        meta: {export: True}
+        getter_name: planner_get_max_vel
+        setter_name: planner_set_max_vel
+        summary: The max allowed cruise velocity of the generated trajectory.
+      - name: t_accel
+        dtype: float
+        unit: second
+        meta: {export: True}
+        getter_name: planner_get_deltat_accel
+        setter_name: planner_set_deltat_accel
+        summary: In time mode, the acceleration time of the generated trajectory.
+      - name: t_decel
+        dtype: float
+        unit: second
+        meta: {export: True}
+        getter_name: planner_get_deltat_decel
+        setter_name: planner_set_deltat_decel
+        summary: In time mode, the deceleration time of the generated trajectory.
+      - name: t_total
+        dtype: float
+        unit: second
+        meta: {export: True}
+        getter_name: planner_get_deltat_total
+        setter_name: planner_set_deltat_total
+        summary: In time mode, the total time of the generated trajectory.
+      - name: move_to
+        summary: Move to target position in the user reference frame respecting velocity and acceleration limits.
+        caller_name: planner_move_to_vlimit
+        dtype: void
+        arguments:
+          - name: pos_setpoint
+            dtype: float
+            unit: tick
+      - name: move_to_tlimit
+        summary: Move to target position in the user reference frame respecting time limits for each sector.
+        caller_name: planner_move_to_tlimit
+        dtype: void
+        arguments:
+          - name: pos_setpoint
+            dtype: float
+            unit: tick
+      - name: errors
+        flags: [INVALID_INPUT, VCRUISE_OVER_LIMIT]
+        getter_name: planner_get_errors
+        summary: Any errors in the trajectory planner, as a bitmask
+  - name: homing
+    remote_attributes:
+      - name: velocity
+        dtype: float
+        unit: ticks/s
+        meta: {export: True}
+        getter_name: homing_planner_get_homing_velocity
+        setter_name: homing_planner_set_homing_velocity
+        summary: The velocity at which the motor performs homing.
+      - name: max_homing_t
+        dtype: float
+        unit: s
+        meta: {export: True}
+        getter_name: homing_planner_get_max_homing_t
+        setter_name: homing_planner_set_max_homing_t
+        summary: The maximum time the motor is allowed to travel before homing times out and aborts.
+      - name: retract_dist
+        dtype: float
+        unit: ticks
+        meta: {export: True}
+        getter_name: homing_planner_get_retract_distance
+        setter_name: homing_planner_set_retract_distance
+        summary: The retraction distance the motor travels after the endstop has been found.
+      - name: warnings
+        meta: {dynamic: True}
+        flags: [HOMING_TIMEOUT]
+        getter_name: homing_planner_get_warnings
+        summary: Any homing warnings, as a bitmask
+      - name: stall_detect
+        remote_attributes:
+          - name: velocity
+            dtype: float
+            unit: ticks/s
+            meta: {export: True}
+            getter_name: homing_planner_get_max_stall_vel
+            setter_name: homing_planner_set_max_stall_vel
+            summary: The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered.
+          - name: delta_pos
+            dtype: float
+            unit: ticks
+            meta: {export: True}
+            getter_name: homing_planner_get_max_stall_delta_pos
+            setter_name: homing_planner_set_max_stall_delta_pos
+            summary: The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered.
+          - name: t
+            dtype: float
+            unit: s
+            meta: {export: True}
+            getter_name: homing_planner_get_max_stall_t
+            setter_name: homing_planner_set_max_stall_t
+            summary: The time to remain in stall detection mode before the motor is considered stalled.
+      - name: home
+        summary: Perform the homing operation.
+        caller_name: homing_planner_home
+        dtype: void
+        arguments: []
+  - name: watchdog
+    remote_attributes:
+      - name: enabled
+        dtype: bool
+        getter_name: Watchdog_get_enabled
+        setter_name: Watchdog_set_enabled
+        summary: Whether the watchdog is enabled or not.
+      - name: triggered
+        dtype: bool
+        meta: {dynamic: True}
+        getter_name: Watchdog_triggered
+        summary: Whether the watchdog has been triggered or not.
+      - name: timeout
+        dtype: float
+        unit: s
+        meta: {export: True}
+        getter_name: Watchdog_get_timeout_seconds
+        setter_name: Watchdog_set_timeout_seconds
+        summary: The watchdog timeout period.
diff --git a/studio/Python/tinymovr/specs/tinymovr_2_1_x.yaml b/studio/Python/tinymovr/specs/tinymovr_2_1_x.yaml
new file mode 100644
index 00000000..600fa8e8
--- /dev/null
+++ b/studio/Python/tinymovr/specs/tinymovr_2_1_x.yaml
@@ -0,0 +1,622 @@
+
+name: tm
+remote_attributes:
+  - name: protocol_hash
+    dtype: uint32
+    getter_name: _avlos_get_proto_hash
+    summary: The Avlos protocol hash.
+  - name: uid
+    dtype: uint32
+    getter_name: system_get_uid
+    summary: The unique device ID, unique to each PAC55xx chip produced.
+  - name: fw_version
+    dtype: string
+    getter_name: system_get_fw_version_string
+    summary: The firmware version.
+  - name: hw_revision
+    dtype: uint32
+    getter_name: system_get_hw_revision
+    summary: The hardware revision.
+  - name: Vbus
+    dtype: float
+    unit: volt
+    meta: {dynamic: True}
+    getter_name: system_get_Vbus
+    summary: The measured bus voltage.
+  - name: Ibus
+    dtype: float
+    unit: ampere
+    meta: {dynamic: True}
+    getter_name: controller_get_Ibus_est
+    summary: The estimated bus current. Only estimates current drawn by motor.
+  - name: power
+    dtype: float
+    unit: watt
+    meta: {dynamic: True}
+    getter_name: controller_get_power_est
+    summary: The estimated power. Only estimates power drawn by motor.
+  - name: temp
+    dtype: float
+    unit: degC
+    meta: {dynamic: True}
+    getter_name: ADC_get_mcu_temp
+    summary: The internal temperature of the PAC55xx MCU.
+  - name: calibrated
+    dtype: bool
+    meta: {dynamic: True}
+    getter_name: system_get_calibrated
+    summary: Whether the system has been calibrated.
+  - name: errors
+    flags: [UNDERVOLTAGE]
+    meta: {dynamic: True}
+    getter_name: system_get_errors
+    summary: Any system errors, as a bitmask
+  - name: warnings
+    flags: [DRIVER_FAULT, CHARGE_PUMP_FAULT_STAT, CHARGE_PUMP_FAULT, DRV10_DISABLE, DRV32_DISABLE, DRV54_DISABLE]
+    meta: {dynamic: True}
+    getter_name: system_get_warnings
+    summary: Any system warnings, as a bitmask
+  - name: save_config
+    summary: Save configuration to non-volatile memory.
+    caller_name: nvm_save_config
+    dtype: void
+    arguments: []
+  - name: erase_config
+    summary: Erase the config stored in non-volatile memory and reset the device.
+    caller_name: nvm_erase
+    dtype: void
+    arguments: []
+    meta: {reload_data: True}
+  - name: reset
+    summary: Reset the device.
+    caller_name: system_reset
+    dtype: void
+    arguments: []
+    meta: {reload_data: True}
+  - name: enter_dfu
+    summary: Enter DFU mode.
+    caller_name: system_enter_dfu
+    dtype: void
+    arguments: []
+    meta: {reload_data: True}
+  - name: config_size
+    summary: Size (in bytes) of the configuration object.
+    getter_name: system_get_config_size
+    dtype: uint32
+  - name: scheduler
+    remote_attributes:
+      - name: load
+        summary: Processor load in ticks per PWM cycle.
+        getter_name: scheduler_get_load
+        meta: {dynamic: True}
+        dtype: uint32
+      - name: warnings
+        flags: [CONTROL_BLOCK_REENTERED]
+        meta: {dynamic: True}
+        getter_name: scheduler_get_warnings
+        summary: Any scheduler warnings, as a bitmask
+  - name: controller
+    remote_attributes:
+      - name: state
+        options: [IDLE, CALIBRATE, CL_CONTROL]
+        meta: {dynamic: True}
+        getter_name: controller_get_state
+        setter_name: controller_set_state
+        summary: The state of the controller.
+      - name: mode
+        options: [CURRENT, VELOCITY, POSITION, TRAJECTORY, HOMING]
+        meta: {dynamic: True}
+        getter_name: controller_get_mode
+        setter_name: controller_set_mode
+        summary: The control mode of the controller.
+      - name: warnings
+        meta: {dynamic: True}
+        flags: [VELOCITY_LIMITED, CURRENT_LIMITED, MODULATION_LIMITED]
+        getter_name: controller_get_warnings
+        summary: Any controller warnings, as a bitmask
+      - name: errors
+        meta: {dynamic: True}
+        flags: [CURRENT_LIMIT_EXCEEDED]
+        getter_name: controller_get_errors
+        summary: Any controller errors, as a bitmask
+      - name: position
+        remote_attributes:
+          - name: setpoint
+            dtype: float
+            unit: tick
+            meta: {jog_step: 100}
+            getter_name: controller_get_pos_setpoint_user_frame
+            setter_name: controller_set_pos_setpoint_user_frame
+            summary: The position setpoint in the user reference frame.
+          - name: p_gain
+            dtype: float
+            meta: {export: True}
+            getter_name: controller_get_pos_gain
+            setter_name: controller_set_pos_gain
+            summary: The proportional gain of the position controller.
+      - name: velocity
+        remote_attributes:
+          - name: setpoint
+            dtype: float
+            unit: tick/sec
+            meta: {jog_step: 200}
+            getter_name: controller_get_vel_setpoint_user_frame
+            setter_name: controller_set_vel_setpoint_user_frame
+            summary: The velocity setpoint in the user reference frame.
+          - name: limit
+            dtype: float
+            unit: tick/sec
+            meta: {export: True}
+            getter_name: controller_get_vel_limit
+            setter_name: controller_set_vel_limit
+            summary: The velocity limit.
+          - name: p_gain
+            dtype: float
+            meta: {export: True}
+            getter_name: controller_get_vel_gain
+            setter_name: controller_set_vel_gain
+            summary: The proportional gain of the velocity controller.
+          - name: i_gain
+            dtype: float
+            meta: {export: True}
+            getter_name: controller_get_vel_integral_gain
+            setter_name: controller_set_vel_integral_gain
+            summary: The integral gain of the velocity controller.
+          - name: deadband
+            dtype: float
+            unit: tick
+            meta: {export: True}
+            getter_name: controller_get_vel_integral_deadband
+            setter_name: controller_set_vel_integral_deadband
+            rst_target: integrator-deadband
+            summary: The deadband of the velocity integrator. A region around the position setpoint where the velocity integrator is not updated.
+          - name: increment
+            dtype: float
+            meta: {export: True}
+            getter_name: controller_get_vel_increment
+            setter_name: controller_set_vel_increment
+            summary: Max velocity setpoint increment (ramping) rate. Set to 0 to disable.
+      - name: current
+        remote_attributes:
+          - name: Iq_setpoint
+            dtype: float
+            unit: ampere
+            meta: {jog_step: 0.005}
+            getter_name: controller_get_Iq_setpoint_user_frame
+            setter_name: controller_set_Iq_setpoint_user_frame
+            summary: The Iq setpoint in the user reference frame.
+          - name: Id_setpoint
+            dtype: float
+            unit: ampere
+            meta: {dynamic: True}
+            getter_name: controller_get_Id_setpoint_user_frame
+            summary: The Id setpoint in the user reference frame.
+          - name: Iq_limit
+            dtype: float
+            unit: ampere
+            getter_name: controller_get_Iq_limit
+            setter_name: controller_set_Iq_limit
+            summary: The Iq limit.
+          - name: Iq_estimate
+            dtype: float
+            unit: ampere
+            meta: {dynamic: True}
+            getter_name: controller_get_Iq_estimate_user_frame
+            summary: The Iq estimate in the user reference frame.
+          - name: bandwidth
+            dtype: float
+            unit: Hz
+            meta: {export: True}
+            getter_name: controller_get_I_bw
+            setter_name: controller_set_I_bw
+            summary: The current controller bandwidth.
+          - name: Iq_p_gain
+            dtype: float
+            getter_name: controller_get_Iq_gain
+            summary: The current controller proportional gain.
+          - name: max_Ibus_regen
+            dtype: float
+            unit: ampere
+            getter_name: controller_get_max_Ibus_regen
+            setter_name: controller_set_max_Ibus_regen
+            summary: The max current allowed to be fed back to the power source before flux braking activates.
+          - name: max_Ibrake
+            dtype: float
+            unit: ampere
+            meta: {export: True}
+            getter_name: controller_get_max_Ibrake
+            setter_name: controller_set_max_Ibrake
+            summary: The max current allowed to be dumped to the motor windings during flux braking. Set to zero to deactivate flux braking.
+      - name: voltage
+        remote_attributes:
+          - name: Vq_setpoint
+            dtype: float
+            unit: volt
+            meta: {dynamic: True}
+            getter_name: controller_get_Vq_setpoint_user_frame
+            summary: The Vq setpoint.
+      - name: calibrate
+        summary: Calibrate the device.
+        caller_name: controller_calibrate
+        dtype: void
+        arguments: []
+      - name: idle
+        summary: Set idle mode, disabling the driver.
+        caller_name: controller_idle
+        dtype: void
+        arguments: []
+      - name: position_mode
+        summary: Set position control mode.
+        caller_name: controller_position_mode
+        dtype: void
+        arguments: []
+      - name: velocity_mode
+        summary: Set velocity control mode.
+        caller_name: controller_velocity_mode
+        dtype: void
+        arguments: []
+      - name: current_mode
+        summary: Set current control mode.
+        caller_name: controller_current_mode
+        dtype: void
+        arguments: []
+      - name: set_pos_vel_setpoints
+        summary: Set the position and velocity setpoints in the user reference frame in one go, and retrieve the position estimate
+        caller_name: controller_set_pos_vel_setpoints_user_frame
+        dtype: float
+        arguments:
+          - name: pos_setpoint
+            dtype: float
+            unit: tick
+          - name: vel_setpoint
+            dtype: float
+            unit: tick
+  - name: comms
+    remote_attributes:
+    - name: can
+      remote_attributes:
+      - name: rate
+        dtype: uint32
+        meta: {export: True}
+        getter_name: CAN_get_kbit_rate
+        setter_name: CAN_set_kbit_rate
+        rst_target: api-can-rate
+        summary: The baud rate of the CAN interface.
+      - name: id
+        dtype: uint32
+        meta: {export: True, reload_data: True}
+        getter_name: CAN_get_ID
+        setter_name: CAN_set_ID
+        summary: The ID of the CAN interface.
+      - name: heartbeat
+        dtype: bool
+        getter_name: CAN_get_send_heartbeat
+        setter_name: CAN_set_send_heartbeat
+        summary: Toggle sending of heartbeat messages.
+  - name: motor
+    remote_attributes:
+      - name: R
+        dtype: float
+        unit: ohm
+        meta: {dynamic: True, export: True}
+        getter_name: motor_get_phase_resistance
+        setter_name: motor_set_phase_resistance
+        summary: The motor Resistance value.
+      - name: L
+        dtype: float
+        unit: henry
+        meta: {dynamic: True, export: True}
+        getter_name: motor_get_phase_inductance
+        setter_name: motor_set_phase_inductance
+        summary: The motor Inductance value.
+      - name: pole_pairs
+        dtype: uint8
+        meta: {dynamic: True, export: True}
+        getter_name: motor_get_pole_pairs
+        setter_name: motor_set_pole_pairs
+        summary: The motor pole pair count.
+      - name: type
+        options: [HIGH_CURRENT, GIMBAL]
+        meta: {export: True}
+        getter_name: motor_get_is_gimbal
+        setter_name: motor_set_is_gimbal
+        summary: The type of the motor. Either high current or gimbal.
+      - name: calibrated
+        dtype: bool
+        meta: {dynamic: True}
+        getter_name: motor_get_calibrated
+        summary: Whether the motor has been calibrated.
+      - name: I_cal
+        dtype: float
+        unit: ampere
+        meta: {export: True}
+        getter_name: motor_get_I_cal
+        setter_name: motor_set_I_cal
+        summary: The calibration current.
+      - name: errors
+        flags: [PHASE_RESISTANCE_OUT_OF_RANGE, PHASE_INDUCTANCE_OUT_OF_RANGE, POLE_PAIRS_CALCULATION_DID_NOT_CONVERGE, POLE_PAIRS_OUT_OF_RANGE]
+        meta: {dynamic: True}
+        getter_name: motor_get_errors
+        summary: Any motor/calibration errors, as a bitmask
+  - name: sensors
+    remote_attributes:
+      - name: user_frame
+        remote_attributes:
+          - name: position_estimate
+            dtype: float
+            unit: ticks
+            meta: {dynamic: True}
+            getter_name: user_frame_get_pos_estimate
+            summary: The filtered position estimate in the user reference frame.
+          - name: velocity_estimate
+            dtype: float
+            unit: ticks/second
+            meta: {dynamic: True}
+            getter_name: user_frame_get_vel_estimate
+            summary: The filtered velocity estimate in the user reference frame.
+          - name: offset
+            dtype: float
+            unit: ticks
+            getter_name: frame_user_to_position_sensor_get_offset
+            setter_name: frame_user_to_position_sensor_set_offset
+            summary: The user defined offset.
+          - name: multiplier
+            dtype: float
+            getter_name: frame_user_to_position_sensor_get_multiplier
+            setter_name: frame_user_to_position_sensor_set_multiplier
+            summary: The user defined multipler.
+      - name: setup
+        remote_attributes:
+          - name: onboard
+            remote_attributes:
+              - name: calibrated
+                dtype: bool
+                meta: {dynamic: True}
+                getter_name: sensor_onboard_get_is_calibrated
+                summary: Whether the sensor has been calibrated.
+              - name: errors
+                flags: [CALIBRATION_FAILED, READING_UNSTABLE]
+                meta: {dynamic: True}
+                getter_name: sensor_onboard_get_errors
+                summary: Any sensor errors, as a bitmask
+          - name: external_spi
+            remote_attributes:
+              - name: type
+                options: [MA7XX, AS5047, AMT22]
+                meta: {export: True}
+                getter_name: sensor_external_spi_get_type_avlos
+                setter_name: sensor_external_spi_set_type_avlos
+                summary: The type of the external sensor.
+              - name: rate
+                options: [1_5Mbps, 3Mbps, 6Mbps, 8Mbps, 12Mbps]
+                meta: {export: True, dynamic: True}
+                getter_name: sensor_external_spi_get_rate_avlos
+                setter_name: sensor_external_spi_set_rate_avlos
+                summary: The rate of the external sensor.
+              - name: calibrated
+                dtype: bool
+                meta: {dynamic: True}
+                getter_name: sensor_external_spi_get_is_calibrated
+                summary: Whether the sensor has been calibrated.
+              - name: errors
+                flags: [CALIBRATION_FAILED, READING_UNSTABLE]
+                meta: {dynamic: True}
+                getter_name: sensor_external_spi_get_errors
+                summary: Any sensor errors, as a bitmask
+          - name: hall
+            remote_attributes:
+              - name: calibrated
+                dtype: bool
+                meta: {dynamic: True}
+                getter_name: sensor_hall_get_is_calibrated
+                summary: Whether the sensor has been calibrated.
+              - name: errors
+                flags: [CALIBRATION_FAILED, READING_UNSTABLE]
+                meta: {dynamic: True}
+                getter_name: sensor_hall_get_errors
+                summary: Any sensor errors, as a bitmask
+      - name: select
+        remote_attributes:
+          - name: position_sensor
+            remote_attributes:
+              - name: connection
+                options: [ONBOARD, EXTERNAL_SPI, HALL]
+                meta: {export: True}
+                getter_name: position_sensor_get_connection
+                setter_name: position_sensor_set_connection
+                summary: The position sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL.
+              - name: bandwidth
+                dtype: float
+                unit: Hz
+                meta: {export: True}
+                getter_name: position_observer_get_bandwidth
+                setter_name: position_observer_set_bandwidth
+                summary: The position sensor observer bandwidth.
+              - name: raw_angle
+                dtype: int32
+                meta: {dynamic: True}
+                getter_name: sensor_position_get_raw_angle
+                summary: The raw position sensor angle.
+              - name: position_estimate
+                dtype: float
+                unit: ticks
+                meta: {dynamic: True}
+                getter_name: position_observer_get_pos_estimate
+                summary: The filtered position estimate in the position sensor reference frame.
+              - name: velocity_estimate
+                dtype: float
+                unit: ticks/second
+                meta: {dynamic: True}
+                getter_name: position_observer_get_vel_estimate
+                summary: The filtered velocity estimate in the position sensor reference frame.
+          - name: commutation_sensor
+            remote_attributes:
+              - name: connection
+                options: [ONBOARD, EXTERNAL_SPI, HALL]
+                meta: {export: True}
+                getter_name: commutation_sensor_get_connection
+                setter_name: commutation_sensor_set_connection
+                summary: The commutation sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL.
+              - name: bandwidth
+                dtype: float
+                unit: Hz
+                meta: {export: True}
+                getter_name: commutation_observer_get_bandwidth
+                setter_name: commutation_observer_set_bandwidth
+                summary: The commutation sensor observer bandwidth.
+              - name: raw_angle
+                dtype: int32
+                meta: {dynamic: True}
+                getter_name: sensor_commutation_get_raw_angle
+                summary: The raw commutation sensor angle.
+              - name: position_estimate
+                dtype: float
+                unit: ticks
+                meta: {dynamic: True}
+                getter_name: commutation_observer_get_pos_estimate
+                summary: The filtered position estimate in the commutation sensor reference frame.
+              - name: velocity_estimate
+                dtype: float
+                unit: ticks/second
+                meta: {dynamic: True}
+                getter_name: commutation_observer_get_vel_estimate
+                summary: The filtered velocity estimate in the commutation sensor reference frame.
+  - name: traj_planner
+    remote_attributes:
+      - name: max_accel
+        dtype: float
+        unit: ticks/s
+        meta: {export: True}
+        getter_name: planner_get_max_accel
+        setter_name: planner_set_max_accel
+        summary: The max allowed acceleration of the generated trajectory.
+      - name: max_decel
+        dtype: float
+        unit: ticks/second/second
+        meta: {export: True}
+        getter_name: planner_get_max_decel
+        setter_name: planner_set_max_decel
+        summary: The max allowed deceleration of the generated trajectory.
+      - name: max_vel
+        dtype: float
+        unit: ticks/second
+        meta: {export: True}
+        getter_name: planner_get_max_vel
+        setter_name: planner_set_max_vel
+        summary: The max allowed cruise velocity of the generated trajectory.
+      - name: t_accel
+        dtype: float
+        unit: second
+        meta: {export: True}
+        getter_name: planner_get_deltat_accel
+        setter_name: planner_set_deltat_accel
+        summary: In time mode, the acceleration time of the generated trajectory.
+      - name: t_decel
+        dtype: float
+        unit: second
+        meta: {export: True}
+        getter_name: planner_get_deltat_decel
+        setter_name: planner_set_deltat_decel
+        summary: In time mode, the deceleration time of the generated trajectory.
+      - name: t_total
+        dtype: float
+        unit: second
+        meta: {export: True}
+        getter_name: planner_get_deltat_total
+        setter_name: planner_set_deltat_total
+        summary: In time mode, the total time of the generated trajectory.
+      - name: move_to
+        summary: Move to target position in the user reference frame respecting velocity and acceleration limits.
+        caller_name: planner_move_to_vlimit
+        dtype: void
+        arguments:
+          - name: pos_setpoint
+            dtype: float
+            unit: tick
+      - name: move_to_tlimit
+        summary: Move to target position in the user reference frame respecting time limits for each sector.
+        caller_name: planner_move_to_tlimit
+        dtype: void
+        arguments:
+          - name: pos_setpoint
+            dtype: float
+            unit: tick
+      - name: errors
+        flags: [INVALID_INPUT, VCRUISE_OVER_LIMIT]
+        getter_name: planner_get_errors
+        summary: Any errors in the trajectory planner, as a bitmask
+  - name: homing
+    remote_attributes:
+      - name: velocity
+        dtype: float
+        unit: ticks/s
+        meta: {export: True}
+        getter_name: homing_planner_get_homing_velocity
+        setter_name: homing_planner_set_homing_velocity
+        summary: The velocity at which the motor performs homing.
+      - name: max_homing_t
+        dtype: float
+        unit: s
+        meta: {export: True}
+        getter_name: homing_planner_get_max_homing_t
+        setter_name: homing_planner_set_max_homing_t
+        summary: The maximum time the motor is allowed to travel before homing times out and aborts.
+      - name: retract_dist
+        dtype: float
+        unit: ticks
+        meta: {export: True}
+        getter_name: homing_planner_get_retract_distance
+        setter_name: homing_planner_set_retract_distance
+        summary: The retraction distance the motor travels after the endstop has been found.
+      - name: warnings
+        meta: {dynamic: True}
+        flags: [HOMING_TIMEOUT]
+        getter_name: homing_planner_get_warnings
+        summary: Any homing warnings, as a bitmask
+      - name: stall_detect
+        remote_attributes:
+          - name: velocity
+            dtype: float
+            unit: ticks/s
+            meta: {export: True}
+            getter_name: homing_planner_get_max_stall_vel
+            setter_name: homing_planner_set_max_stall_vel
+            summary: The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered.
+          - name: delta_pos
+            dtype: float
+            unit: ticks
+            meta: {export: True}
+            getter_name: homing_planner_get_max_stall_delta_pos
+            setter_name: homing_planner_set_max_stall_delta_pos
+            summary: The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered.
+          - name: t
+            dtype: float
+            unit: s
+            meta: {export: True}
+            getter_name: homing_planner_get_max_stall_t
+            setter_name: homing_planner_set_max_stall_t
+            summary: The time to remain in stall detection mode before the motor is considered stalled.
+      - name: home
+        summary: Perform the homing operation.
+        caller_name: homing_planner_home
+        dtype: void
+        arguments: []
+  - name: watchdog
+    remote_attributes:
+      - name: enabled
+        dtype: bool
+        getter_name: Watchdog_get_enabled
+        setter_name: Watchdog_set_enabled
+        summary: Whether the watchdog is enabled or not.
+      - name: triggered
+        dtype: bool
+        meta: {dynamic: True}
+        getter_name: Watchdog_triggered
+        summary: Whether the watchdog has been triggered or not.
+      - name: timeout
+        dtype: float
+        unit: s
+        meta: {export: True}
+        getter_name: Watchdog_get_timeout_seconds
+        setter_name: Watchdog_set_timeout_seconds
+        summary: The watchdog timeout period.
diff --git a/studio/Python/tinymovr/specs/tinymovr_2_3_x.yaml b/studio/Python/tinymovr/specs/tinymovr_2_3_x.yaml
new file mode 100644
index 00000000..04c926a2
--- /dev/null
+++ b/studio/Python/tinymovr/specs/tinymovr_2_3_x.yaml
@@ -0,0 +1,622 @@
+
+name: tm
+remote_attributes:
+  - name: protocol_hash
+    dtype: uint32
+    getter_name: _avlos_get_proto_hash
+    summary: The Avlos protocol hash.
+  - name: uid
+    dtype: uint32
+    getter_name: system_get_uid
+    summary: The unique device ID, unique to each PAC55xx chip produced.
+  - name: fw_version
+    dtype: string
+    getter_name: system_get_fw_version_string
+    summary: The firmware version.
+  - name: hw_revision
+    dtype: uint32
+    getter_name: system_get_hw_revision
+    summary: The hardware revision.
+  - name: Vbus
+    dtype: float
+    unit: volt
+    meta: {dynamic: True}
+    getter_name: system_get_Vbus
+    summary: The measured bus voltage.
+  - name: Ibus
+    dtype: float
+    unit: ampere
+    meta: {dynamic: True}
+    getter_name: controller_get_Ibus_est
+    summary: The estimated bus current. Only estimates current drawn by motor.
+  - name: power
+    dtype: float
+    unit: watt
+    meta: {dynamic: True}
+    getter_name: controller_get_power_est
+    summary: The estimated power. Only estimates power drawn by motor.
+  - name: temp
+    dtype: float
+    unit: degC
+    meta: {dynamic: True}
+    getter_name: ADC_get_mcu_temp
+    summary: The internal temperature of the PAC55xx MCU.
+  - name: calibrated
+    dtype: bool
+    meta: {dynamic: True}
+    getter_name: system_get_calibrated
+    summary: Whether the system has been calibrated.
+  - name: errors
+    flags: [UNDERVOLTAGE]
+    meta: {dynamic: True}
+    getter_name: system_get_errors
+    summary: Any system errors, as a bitmask
+  - name: warnings
+    flags: [DRIVER_FAULT, CHARGE_PUMP_FAULT_STAT, CHARGE_PUMP_FAULT, DRV10_DISABLE, DRV32_DISABLE, DRV54_DISABLE]
+    meta: {dynamic: True}
+    getter_name: system_get_warnings
+    summary: Any system warnings, as a bitmask
+  - name: save_config
+    summary: Save configuration to non-volatile memory.
+    caller_name: nvm_save_config
+    dtype: void
+    arguments: []
+  - name: erase_config
+    summary: Erase the config stored in non-volatile memory and reset the device.
+    caller_name: nvm_erase_and_reset
+    dtype: void
+    arguments: []
+    meta: {reload_data: True}
+  - name: reset
+    summary: Reset the device.
+    caller_name: system_reset
+    dtype: void
+    arguments: []
+    meta: {reload_data: True}
+  - name: enter_dfu
+    summary: Enter DFU mode.
+    caller_name: system_enter_dfu
+    dtype: void
+    arguments: []
+    meta: {reload_data: True}
+  - name: config_size
+    summary: Size (in bytes) of the configuration object.
+    getter_name: system_get_config_size
+    dtype: uint32
+  - name: scheduler
+    remote_attributes:
+      - name: load
+        summary: Processor load in ticks per PWM cycle.
+        getter_name: scheduler_get_load
+        meta: {dynamic: True}
+        dtype: uint32
+      - name: warnings
+        flags: [CONTROL_BLOCK_REENTERED]
+        meta: {dynamic: True}
+        getter_name: scheduler_get_warnings
+        summary: Any scheduler warnings, as a bitmask
+  - name: controller
+    remote_attributes:
+      - name: state
+        options: [IDLE, CALIBRATE, CL_CONTROL]
+        meta: {dynamic: True}
+        getter_name: controller_get_state
+        setter_name: controller_set_state
+        summary: The state of the controller.
+      - name: mode
+        options: [CURRENT, VELOCITY, POSITION, TRAJECTORY, HOMING]
+        meta: {dynamic: True}
+        getter_name: controller_get_mode
+        setter_name: controller_set_mode
+        summary: The control mode of the controller.
+      - name: warnings
+        meta: {dynamic: True}
+        flags: [VELOCITY_LIMITED, CURRENT_LIMITED, MODULATION_LIMITED]
+        getter_name: controller_get_warnings
+        summary: Any controller warnings, as a bitmask
+      - name: errors
+        meta: {dynamic: True}
+        flags: [CURRENT_LIMIT_EXCEEDED, PRE_CL_I_SD_EXCEEDED]
+        getter_name: controller_get_errors
+        summary: Any controller errors, as a bitmask
+      - name: position
+        remote_attributes:
+          - name: setpoint
+            dtype: float
+            unit: tick
+            meta: {jog_step: 100}
+            getter_name: controller_get_pos_setpoint_user_frame
+            setter_name: controller_set_pos_setpoint_user_frame
+            summary: The position setpoint in the user reference frame.
+          - name: p_gain
+            dtype: float
+            meta: {export: True}
+            getter_name: controller_get_pos_gain
+            setter_name: controller_set_pos_gain
+            summary: The proportional gain of the position controller.
+      - name: velocity
+        remote_attributes:
+          - name: setpoint
+            dtype: float
+            unit: tick/sec
+            meta: {jog_step: 200}
+            getter_name: controller_get_vel_setpoint_user_frame
+            setter_name: controller_set_vel_setpoint_user_frame
+            summary: The velocity setpoint in the user reference frame.
+          - name: limit
+            dtype: float
+            unit: tick/sec
+            meta: {export: True}
+            getter_name: controller_get_vel_limit
+            setter_name: controller_set_vel_limit
+            summary: The velocity limit.
+          - name: p_gain
+            dtype: float
+            meta: {export: True}
+            getter_name: controller_get_vel_gain
+            setter_name: controller_set_vel_gain
+            summary: The proportional gain of the velocity controller.
+          - name: i_gain
+            dtype: float
+            meta: {export: True}
+            getter_name: controller_get_vel_integral_gain
+            setter_name: controller_set_vel_integral_gain
+            summary: The integral gain of the velocity controller.
+          - name: deadband
+            dtype: float
+            unit: tick
+            meta: {export: True}
+            getter_name: controller_get_vel_integral_deadband
+            setter_name: controller_set_vel_integral_deadband
+            rst_target: integrator-deadband
+            summary: The deadband of the velocity integrator. A region around the position setpoint where the velocity integrator is not updated.
+          - name: increment
+            dtype: float
+            meta: {export: True}
+            getter_name: controller_get_vel_increment
+            setter_name: controller_set_vel_increment
+            summary: Max velocity setpoint increment (ramping) rate. Set to 0 to disable.
+      - name: current
+        remote_attributes:
+          - name: Iq_setpoint
+            dtype: float
+            unit: ampere
+            meta: {jog_step: 0.005}
+            getter_name: controller_get_Iq_setpoint_user_frame
+            setter_name: controller_set_Iq_setpoint_user_frame
+            summary: The Iq setpoint in the user reference frame.
+          - name: Id_setpoint
+            dtype: float
+            unit: ampere
+            meta: {dynamic: True}
+            getter_name: controller_get_Id_setpoint_user_frame
+            summary: The Id setpoint in the user reference frame.
+          - name: Iq_limit
+            dtype: float
+            unit: ampere
+            getter_name: controller_get_Iq_limit
+            setter_name: controller_set_Iq_limit
+            summary: The Iq limit.
+          - name: Iq_estimate
+            dtype: float
+            unit: ampere
+            meta: {dynamic: True}
+            getter_name: controller_get_Iq_estimate_user_frame
+            summary: The Iq estimate in the user reference frame.
+          - name: bandwidth
+            dtype: float
+            unit: Hz
+            meta: {export: True}
+            getter_name: controller_get_I_bw
+            setter_name: controller_set_I_bw
+            summary: The current controller bandwidth.
+          - name: Iq_p_gain
+            dtype: float
+            getter_name: controller_get_Iq_gain
+            summary: The current controller proportional gain.
+          - name: max_Ibus_regen
+            dtype: float
+            unit: ampere
+            getter_name: controller_get_max_Ibus_regen
+            setter_name: controller_set_max_Ibus_regen
+            summary: The max current allowed to be fed back to the power source before flux braking activates.
+          - name: max_Ibrake
+            dtype: float
+            unit: ampere
+            meta: {export: True}
+            getter_name: controller_get_max_Ibrake
+            setter_name: controller_set_max_Ibrake
+            summary: The max current allowed to be dumped to the motor windings during flux braking. Set to zero to deactivate flux braking.
+      - name: voltage
+        remote_attributes:
+          - name: Vq_setpoint
+            dtype: float
+            unit: volt
+            meta: {dynamic: True}
+            getter_name: controller_get_Vq_setpoint_user_frame
+            summary: The Vq setpoint.
+      - name: calibrate
+        summary: Calibrate the device.
+        caller_name: controller_calibrate
+        dtype: void
+        arguments: []
+      - name: idle
+        summary: Set idle mode, disabling the driver.
+        caller_name: controller_idle
+        dtype: void
+        arguments: []
+      - name: position_mode
+        summary: Set position control mode.
+        caller_name: controller_position_mode
+        dtype: void
+        arguments: []
+      - name: velocity_mode
+        summary: Set velocity control mode.
+        caller_name: controller_velocity_mode
+        dtype: void
+        arguments: []
+      - name: current_mode
+        summary: Set current control mode.
+        caller_name: controller_current_mode
+        dtype: void
+        arguments: []
+      - name: set_pos_vel_setpoints
+        summary: Set the position and velocity setpoints in the user reference frame in one go, and retrieve the position estimate
+        caller_name: controller_set_pos_vel_setpoints_user_frame
+        dtype: float
+        arguments:
+          - name: pos_setpoint
+            dtype: float
+            unit: tick
+          - name: vel_setpoint
+            dtype: float
+            unit: tick
+  - name: comms
+    remote_attributes:
+    - name: can
+      remote_attributes:
+      - name: rate
+        dtype: uint32
+        meta: {export: True}
+        getter_name: CAN_get_kbit_rate
+        setter_name: CAN_set_kbit_rate
+        rst_target: api-can-rate
+        summary: The baud rate of the CAN interface.
+      - name: id
+        dtype: uint32
+        meta: {export: True, reload_data: True}
+        getter_name: CAN_get_ID
+        setter_name: CAN_set_ID
+        summary: The ID of the CAN interface.
+      - name: heartbeat
+        dtype: bool
+        getter_name: CAN_get_send_heartbeat
+        setter_name: CAN_set_send_heartbeat
+        summary: Toggle sending of heartbeat messages.
+  - name: motor
+    remote_attributes:
+      - name: R
+        dtype: float
+        unit: ohm
+        meta: {dynamic: True, export: True}
+        getter_name: motor_get_phase_resistance
+        setter_name: motor_set_phase_resistance
+        summary: The motor Resistance value.
+      - name: L
+        dtype: float
+        unit: henry
+        meta: {dynamic: True, export: True}
+        getter_name: motor_get_phase_inductance
+        setter_name: motor_set_phase_inductance
+        summary: The motor Inductance value.
+      - name: pole_pairs
+        dtype: uint8
+        meta: {dynamic: True, export: True}
+        getter_name: motor_get_pole_pairs
+        setter_name: motor_set_pole_pairs
+        summary: The motor pole pair count.
+      - name: type
+        options: [HIGH_CURRENT, GIMBAL]
+        meta: {export: True}
+        getter_name: motor_get_is_gimbal
+        setter_name: motor_set_is_gimbal
+        summary: The type of the motor. Either high current or gimbal.
+      - name: calibrated
+        dtype: bool
+        meta: {dynamic: True}
+        getter_name: motor_get_calibrated
+        summary: Whether the motor has been calibrated.
+      - name: I_cal
+        dtype: float
+        unit: ampere
+        meta: {export: True}
+        getter_name: motor_get_I_cal
+        setter_name: motor_set_I_cal
+        summary: The calibration current.
+      - name: errors
+        flags: [PHASE_RESISTANCE_OUT_OF_RANGE, PHASE_INDUCTANCE_OUT_OF_RANGE, POLE_PAIRS_CALCULATION_DID_NOT_CONVERGE, POLE_PAIRS_OUT_OF_RANGE, ABNORMAL_CALIBRATION_VOLTAGE]
+        meta: {dynamic: True}
+        getter_name: motor_get_errors
+        summary: Any motor/calibration errors, as a bitmask
+  - name: sensors
+    remote_attributes:
+      - name: user_frame
+        remote_attributes:
+          - name: position_estimate
+            dtype: float
+            unit: ticks
+            meta: {dynamic: True}
+            getter_name: user_frame_get_pos_estimate
+            summary: The filtered position estimate in the user reference frame.
+          - name: velocity_estimate
+            dtype: float
+            unit: ticks/second
+            meta: {dynamic: True}
+            getter_name: user_frame_get_vel_estimate
+            summary: The filtered velocity estimate in the user reference frame.
+          - name: offset
+            dtype: float
+            unit: ticks
+            getter_name: frame_user_to_position_sensor_get_offset
+            setter_name: frame_user_to_position_sensor_set_offset
+            summary: The user defined offset.
+          - name: multiplier
+            dtype: float
+            getter_name: frame_user_to_position_sensor_get_multiplier
+            setter_name: frame_user_to_position_sensor_set_multiplier
+            summary: The user defined multipler.
+      - name: setup
+        remote_attributes:
+          - name: onboard
+            remote_attributes:
+              - name: calibrated
+                dtype: bool
+                meta: {dynamic: True}
+                getter_name: sensor_onboard_get_is_calibrated
+                summary: Whether the sensor has been calibrated.
+              - name: errors
+                flags: [CALIBRATION_FAILED, READING_UNSTABLE]
+                meta: {dynamic: True}
+                getter_name: sensor_onboard_get_errors
+                summary: Any sensor errors, as a bitmask
+          - name: external_spi
+            remote_attributes:
+              - name: type
+                options: [MA7XX, AS5047, AMT22]
+                meta: {export: True}
+                getter_name: sensor_external_spi_get_type_avlos
+                setter_name: sensor_external_spi_set_type_avlos
+                summary: The type of the external sensor.
+              - name: rate
+                options: [1_5Mbps, 3Mbps, 6Mbps, 8Mbps, 12Mbps]
+                meta: {export: True, dynamic: True}
+                getter_name: sensor_external_spi_get_rate_avlos
+                setter_name: sensor_external_spi_set_rate_avlos
+                summary: The rate of the external sensor.
+              - name: calibrated
+                dtype: bool
+                meta: {dynamic: True}
+                getter_name: sensor_external_spi_get_is_calibrated
+                summary: Whether the sensor has been calibrated.
+              - name: errors
+                flags: [CALIBRATION_FAILED, READING_UNSTABLE]
+                meta: {dynamic: True}
+                getter_name: sensor_external_spi_get_errors
+                summary: Any sensor errors, as a bitmask
+          - name: hall
+            remote_attributes:
+              - name: calibrated
+                dtype: bool
+                meta: {dynamic: True}
+                getter_name: sensor_hall_get_is_calibrated
+                summary: Whether the sensor has been calibrated.
+              - name: errors
+                flags: [CALIBRATION_FAILED, READING_UNSTABLE]
+                meta: {dynamic: True}
+                getter_name: sensor_hall_get_errors
+                summary: Any sensor errors, as a bitmask
+      - name: select
+        remote_attributes:
+          - name: position_sensor
+            remote_attributes:
+              - name: connection
+                options: [ONBOARD, EXTERNAL_SPI, HALL]
+                meta: {export: True}
+                getter_name: position_sensor_get_connection
+                setter_name: position_sensor_set_connection
+                summary: The position sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL.
+              - name: bandwidth
+                dtype: float
+                unit: Hz
+                meta: {export: True}
+                getter_name: position_observer_get_bandwidth
+                setter_name: position_observer_set_bandwidth
+                summary: The position sensor observer bandwidth.
+              - name: raw_angle
+                dtype: int32
+                meta: {dynamic: True}
+                getter_name: sensor_position_get_raw_angle
+                summary: The raw position sensor angle.
+              - name: position_estimate
+                dtype: float
+                unit: ticks
+                meta: {dynamic: True}
+                getter_name: position_observer_get_pos_estimate
+                summary: The filtered position estimate in the position sensor reference frame.
+              - name: velocity_estimate
+                dtype: float
+                unit: ticks/second
+                meta: {dynamic: True}
+                getter_name: position_observer_get_vel_estimate
+                summary: The filtered velocity estimate in the position sensor reference frame.
+          - name: commutation_sensor
+            remote_attributes:
+              - name: connection
+                options: [ONBOARD, EXTERNAL_SPI, HALL]
+                meta: {export: True}
+                getter_name: commutation_sensor_get_connection
+                setter_name: commutation_sensor_set_connection
+                summary: The commutation sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL.
+              - name: bandwidth
+                dtype: float
+                unit: Hz
+                meta: {export: True}
+                getter_name: commutation_observer_get_bandwidth
+                setter_name: commutation_observer_set_bandwidth
+                summary: The commutation sensor observer bandwidth.
+              - name: raw_angle
+                dtype: int32
+                meta: {dynamic: True}
+                getter_name: sensor_commutation_get_raw_angle
+                summary: The raw commutation sensor angle.
+              - name: position_estimate
+                dtype: float
+                unit: ticks
+                meta: {dynamic: True}
+                getter_name: commutation_observer_get_pos_estimate
+                summary: The filtered position estimate in the commutation sensor reference frame.
+              - name: velocity_estimate
+                dtype: float
+                unit: ticks/second
+                meta: {dynamic: True}
+                getter_name: commutation_observer_get_vel_estimate
+                summary: The filtered velocity estimate in the commutation sensor reference frame.
+  - name: traj_planner
+    remote_attributes:
+      - name: max_accel
+        dtype: float
+        unit: ticks/s
+        meta: {export: True}
+        getter_name: planner_get_max_accel
+        setter_name: planner_set_max_accel
+        summary: The max allowed acceleration of the generated trajectory.
+      - name: max_decel
+        dtype: float
+        unit: ticks/second/second
+        meta: {export: True}
+        getter_name: planner_get_max_decel
+        setter_name: planner_set_max_decel
+        summary: The max allowed deceleration of the generated trajectory.
+      - name: max_vel
+        dtype: float
+        unit: ticks/second
+        meta: {export: True}
+        getter_name: planner_get_max_vel
+        setter_name: planner_set_max_vel
+        summary: The max allowed cruise velocity of the generated trajectory.
+      - name: t_accel
+        dtype: float
+        unit: second
+        meta: {export: True}
+        getter_name: planner_get_deltat_accel
+        setter_name: planner_set_deltat_accel
+        summary: In time mode, the acceleration time of the generated trajectory.
+      - name: t_decel
+        dtype: float
+        unit: second
+        meta: {export: True}
+        getter_name: planner_get_deltat_decel
+        setter_name: planner_set_deltat_decel
+        summary: In time mode, the deceleration time of the generated trajectory.
+      - name: t_total
+        dtype: float
+        unit: second
+        meta: {export: True}
+        getter_name: planner_get_deltat_total
+        setter_name: planner_set_deltat_total
+        summary: In time mode, the total time of the generated trajectory.
+      - name: move_to
+        summary: Move to target position in the user reference frame respecting velocity and acceleration limits.
+        caller_name: planner_move_to_vlimit
+        dtype: void
+        arguments:
+          - name: pos_setpoint
+            dtype: float
+            unit: tick
+      - name: move_to_tlimit
+        summary: Move to target position in the user reference frame respecting time limits for each sector.
+        caller_name: planner_move_to_tlimit
+        dtype: void
+        arguments:
+          - name: pos_setpoint
+            dtype: float
+            unit: tick
+      - name: errors
+        flags: [INVALID_INPUT, VCRUISE_OVER_LIMIT]
+        getter_name: planner_get_errors
+        summary: Any errors in the trajectory planner, as a bitmask
+  - name: homing
+    remote_attributes:
+      - name: velocity
+        dtype: float
+        unit: ticks/s
+        meta: {export: True}
+        getter_name: homing_planner_get_homing_velocity
+        setter_name: homing_planner_set_homing_velocity
+        summary: The velocity at which the motor performs homing.
+      - name: max_homing_t
+        dtype: float
+        unit: s
+        meta: {export: True}
+        getter_name: homing_planner_get_max_homing_t
+        setter_name: homing_planner_set_max_homing_t
+        summary: The maximum time the motor is allowed to travel before homing times out and aborts.
+      - name: retract_dist
+        dtype: float
+        unit: ticks
+        meta: {export: True}
+        getter_name: homing_planner_get_retract_distance
+        setter_name: homing_planner_set_retract_distance
+        summary: The retraction distance the motor travels after the endstop has been found.
+      - name: warnings
+        meta: {dynamic: True}
+        flags: [HOMING_TIMEOUT]
+        getter_name: homing_planner_get_warnings
+        summary: Any homing warnings, as a bitmask
+      - name: stall_detect
+        remote_attributes:
+          - name: velocity
+            dtype: float
+            unit: ticks/s
+            meta: {export: True}
+            getter_name: homing_planner_get_max_stall_vel
+            setter_name: homing_planner_set_max_stall_vel
+            summary: The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered.
+          - name: delta_pos
+            dtype: float
+            unit: ticks
+            meta: {export: True}
+            getter_name: homing_planner_get_max_stall_delta_pos
+            setter_name: homing_planner_set_max_stall_delta_pos
+            summary: The velocity below which (and together with `stall_detect.delta_pos`) stall detection mode is triggered.
+          - name: t
+            dtype: float
+            unit: s
+            meta: {export: True}
+            getter_name: homing_planner_get_max_stall_t
+            setter_name: homing_planner_set_max_stall_t
+            summary: The time to remain in stall detection mode before the motor is considered stalled.
+      - name: home
+        summary: Perform the homing operation.
+        caller_name: homing_planner_home
+        dtype: void
+        arguments: []
+  - name: watchdog
+    remote_attributes:
+      - name: enabled
+        dtype: bool
+        getter_name: Watchdog_get_enabled
+        setter_name: Watchdog_set_enabled
+        summary: Whether the watchdog is enabled or not.
+      - name: triggered
+        dtype: bool
+        meta: {dynamic: True}
+        getter_name: Watchdog_triggered
+        summary: Whether the watchdog has been triggered or not.
+      - name: timeout
+        dtype: float
+        unit: s
+        meta: {export: True}
+        getter_name: Watchdog_get_timeout_seconds
+        setter_name: Watchdog_set_timeout_seconds
+        summary: The watchdog timeout period.
diff --git a/studio/Python/tinymovr/tee.py b/studio/Python/tinymovr/tee.py
deleted file mode 100644
index 1a67cdb5..00000000
--- a/studio/Python/tinymovr/tee.py
+++ /dev/null
@@ -1,132 +0,0 @@
-"""
-Tinymovr Tee Module
-Copyright Ioannis Chatzikonstantinou 2020-2023
-
-Implements a Tee class to distribute incoming traffic according to rules
-
-This program is free software: you can redistribute it and/or modify it under
-the terms of the GNU General Public License as published by the Free Software
-Foundation, either version 3 of the License, or (at your option) any later
-version.
-This program is distributed in the hope that it will be useful, but WITHOUT
-ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
-You should have received a copy of the GNU General Public License along with
-this program. If not, see <http://www.gnu.org/licenses/>.
-"""
-
-
-import time
-from enum import Enum
-from threading import Thread, Lock
-
-
-tee = None
-
-
-class Client:
-    def __init__(self, filter_cb, recv_cb):
-        self.filter_cb = filter_cb
-        self.recv_cb = recv_cb
-
-
-class TeeState(Enum):
-    INIT = 0
-    RUNNING = 1
-    STOPPING = 2
-    STOPPED = 3
-
-
-class Tee:
-    """
-    Distribute incoming messages based on the boolean result
-    of a filter callback.
-
-    python-can does not allow filtering messages per recipient therefore
-    this class.
-
-    Also implements a simple forwarding mechanism for sending messages, to
-    simplify interfacing with CAN bus objects.
-    """
-
-    def __init__(self, bus, timeout):
-        self.bus = bus
-        self.flush_rx_buffer()
-        self.timeout = timeout
-        self.clients = []
-        self.state = TeeState.INIT
-        self.update_thread = Thread(target=self.update, daemon=True)
-        self.update_thread.start()
-
-    def add(self, filter_cb, recv_cb):
-        self.clients.append(Client(filter_cb, recv_cb))
-
-    def update(self):
-        """
-        Continuously update the state of the Tee object, checking
-        for messages and distributing them to the appropriate clients
-        based on filter criteria until the state changes to STOPPING.
-        """
-        self.state = TeeState.RUNNING
-        while TeeState.RUNNING == self.state:
-            self._update_once()
-        assert TeeState.STOPPING == self.state
-        self.state = TeeState.STOPPED
-
-    def _update_once(self):
-        """
-        Try to receive a message from the bus object and if successful,
-        tests reception of each tee instance in the global index.
-        """
-        frame = self.bus.recv(timeout=self.timeout)
-        if frame:
-            for client in self.clients:
-                if client.filter_cb(frame):
-                    client.recv_cb(frame)
-
-    def send(self, frame):
-        """
-        Send a frame by forwarding to the bus object
-        """
-        self.bus.send(frame)
-
-    def stop(self):
-        self.state = TeeState.STOPPING
-        while TeeState.STOPPING == self.state:
-            time.sleep(0.01)
-        assert TeeState.STOPPED == self.state
-
-    def flush_rx_buffer(self, trials=100):
-        """
-        Flush the RX buffer of a bus
-        """
-        for i in range(trials):
-            if not self.bus.recv(timeout=0.001):
-                return
-
-
-def init_tee(bus, timeout=0.1):
-    """
-    Initializes a tee using a python-can bus instance
-    """
-    global tee
-    assert None == tee
-    tee = Tee(bus, timeout)
-    return tee
-
-
-def destroy_tee():
-    """
-    Destroys the existing tee, stopping its thread.
-    """
-    global tee
-    assert None != tee
-    tee.stop()
-    tee = None
-
-
-def get_tee():
-    """
-    Get the current tee object
-    """
-    return tee


Test patch:
diff --git a/studio/Python/tests/test_amt22.py b/studio/Python/tests/test_amt22.py
new file mode 100644
index 00000000..d716003a
--- /dev/null
+++ b/studio/Python/tests/test_amt22.py
@@ -0,0 +1,159 @@
+"""
+Tinymovr Board Config Tests
+Copyright Ioannis Chatzikonstantinou 2020-2023
+
+Tests saving and loading Tinymovr configuration to/from NVRAM.
+
+This program is free software: you can redistribute it and/or modify it under
+the terms of the GNU General Public License as published by the Free Software
+Foundation, either version 3 of the License, or (at your option) any later
+version.
+This program is distributed in the hope that it will be useful, but WITHOUT
+ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+You should have received a copy of the GNU General Public License along with
+this program. If not, see <http://www.gnu.org/licenses/>.
+"""
+
+import random
+import time
+
+from avlos.unit_field import get_registry
+
+import unittest
+import pytest
+from tests import TMTestCase
+
+ureg = get_registry()
+A = ureg.ampere
+tick = ureg.ticks
+s = ureg.second
+tsleep = 0.80
+
+
+class TestAMT22(TMTestCase):
+
+    @pytest.mark.sensor_amt22
+    def test_a_position_control_before_after_save_and_load_config(self):
+        """
+        Test position control after saving and loading config.
+        WARNING: This will perform one NVRAM write and two erase cycles.
+        """
+        self.check_state(0)
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+        self.configure_sensors(self.tm.sensors.setup.external_spi.type.AMT22)
+        self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.EXTERNAL_SPI)
+
+        self.try_calibrate()
+
+        self.tm.controller.position.p_gain = 9
+        self.tm.controller.velocity.p_gain = 3e-5
+        self.tm.controller.velocity.i_gain = 0
+        self.tm.controller.velocity.limit = 200000
+
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(10):
+            new_pos = random.uniform(-1000, 1000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=200 * tick,
+            )
+
+        self.tm.controller.idle()
+
+        time.sleep(0.1)
+
+        self.tm.save_config()
+        
+        time.sleep(0.2)
+
+        self.reset_and_wait()
+        
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(10):
+            new_pos = random.uniform(-1000, 1000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=200 * tick,
+            )
+
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+    @pytest.mark.sensor_amt22
+    def test_b_position_control_following_sensor_change(self):
+        """
+        Test position control before and after changing sensor connection and type.
+        This test will alter the sensor configuration from an external SPI connection
+        to an onboard connection, and verify if the position control maintains accuracy.
+        """
+        self.check_state(0)
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+        # Initially configure with external SPI sensor
+        self.configure_sensors(self.tm.sensors.setup.external_spi.type.AMT22)
+        self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.EXTERNAL_SPI)
+        self.try_calibrate()
+
+        # Set initial controller gains
+        self.tm.controller.position.p_gain = 9
+        self.tm.controller.velocity.p_gain = 3e-5
+        self.tm.controller.velocity.i_gain = 0
+        self.tm.controller.velocity.limit = 200000
+
+        # Start with external SPI sensor
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(5):
+            new_pos = random.uniform(-1000, 1000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=200 * tick,
+            )
+
+        self.tm.controller.idle()
+        time.sleep(0.1)
+
+        # Change to onboard sensor
+        self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.ONBOARD)
+
+        # Re-calibrate with new sensor setup
+        self.try_calibrate()
+        time.sleep(0.1)
+
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(5):
+            new_pos = random.uniform(-10000, 10000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=2000 * tick,
+            )
+
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/studio/Python/tests/test_as5047.py b/studio/Python/tests/test_as5047.py
new file mode 100644
index 00000000..da6973e1
--- /dev/null
+++ b/studio/Python/tests/test_as5047.py
@@ -0,0 +1,210 @@
+"""
+Tinymovr Board Config Tests
+Copyright Ioannis Chatzikonstantinou 2020-2023
+
+Tests saving and loading Tinymovr configuration to/from NVRAM.
+
+This program is free software: you can redistribute it and/or modify it under
+the terms of the GNU General Public License as published by the Free Software
+Foundation, either version 3 of the License, or (at your option) any later
+version.
+This program is distributed in the hope that it will be useful, but WITHOUT
+ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+You should have received a copy of the GNU General Public License along with
+this program. If not, see <http://www.gnu.org/licenses/>.
+"""
+
+import random
+import time
+
+from avlos.unit_field import get_registry
+
+import unittest
+import pytest
+from tests import TMTestCase
+
+ureg = get_registry()
+A = ureg.ampere
+tick = ureg.ticks
+s = ureg.second
+tsleep = 0.50
+
+
+class TestAS5047(TMTestCase):
+
+    @pytest.mark.sensor_as5047
+    def test_a_position_control_before_after_save_and_load_config(self):
+        """
+        Test position control after saving and loading config.
+        WARNING: This will perform one NVRAM write and two erase cycles.
+        """
+        self.check_state(0)
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+        self.configure_sensors(self.tm.sensors.setup.external_spi.type.AS5047)
+        self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.EXTERNAL_SPI)
+
+        self.try_calibrate()
+
+        self.tm.controller.position.p_gain = 9
+        self.tm.controller.velocity.p_gain = 2e-5
+        self.tm.controller.velocity.i_gain = 0
+
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(10):
+            new_pos = random.uniform(-20000, 20000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=2000 * tick,
+            )
+
+        self.tm.controller.idle()
+
+        time.sleep(0.1)
+
+        self.tm.save_config()
+        
+        time.sleep(0.2)
+
+        self.reset_and_wait()
+        
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(10):
+            new_pos = random.uniform(-20000, 20000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=2000 * tick,
+            )
+
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+    @pytest.mark.sensor_as5047
+    def test_b_position_control_following_sensor_change(self):
+        """
+        Test position control before and after changing sensor connection and type.
+        This test will alter the sensor configuration from an external SPI connection
+        to an onboard connection, and verify if the position control maintains accuracy.
+        """
+        self.check_state(0)
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+        # Initially configure with external SPI sensor
+        self.configure_sensors(self.tm.sensors.setup.external_spi.type.AS5047)
+        self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.EXTERNAL_SPI)
+        self.try_calibrate()
+
+        # Set initial controller gains
+        self.tm.controller.position.p_gain = 9
+        self.tm.controller.velocity.p_gain = 3e-5
+        self.tm.controller.velocity.i_gain = 0
+        self.tm.controller.velocity.limit = 200000
+
+        # Start with external SPI sensor
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(5):
+            new_pos = random.uniform(-20000, 20000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=2000 * tick,
+            )
+
+        self.tm.controller.idle()
+        time.sleep(0.1)
+
+        # Change to onboard sensor
+        self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.ONBOARD)
+
+        # Re-calibrate with new sensor setup
+        self.try_calibrate()
+        time.sleep(0.1)
+
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(5):
+            new_pos = random.uniform(-20000, 20000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=2000 * tick,
+            )
+
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+    @pytest.mark.sensor_as5047
+    def test_c_position_control_change_baud_rates(self):
+        """
+        Test position control with external sensor and
+        different baud rates.
+        """
+        self.check_state(0)
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+        self.configure_sensors(self.tm.sensors.setup.external_spi.type.AS5047)
+        self.select_sensors(self.tm.sensors.select.position_sensor.connection.ONBOARD, self.tm.sensors.select.position_sensor.connection.EXTERNAL_SPI)
+
+        self.try_calibrate()
+
+        self.tm.controller.position.p_gain = 9
+        self.tm.controller.velocity.p_gain = 2e-5
+        self.tm.controller.velocity.i_gain = 0
+
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(10):
+            new_pos = random.uniform(-20000, 20000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=2000 * tick,
+            )
+
+        self.tm.controller.idle()
+
+        self.tm.sensors.setup.external_spi.rate = "1_5Mbps"
+        time.sleep(0.1)
+
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(10):
+            new_pos = random.uniform(-20000, 20000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=2000 * tick,
+            )
+
+        self.tm.controller.idle()
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/studio/Python/tests/test_base_function.py b/studio/Python/tests/test_base_function.py
deleted file mode 100644
index a56c6837..00000000
--- a/studio/Python/tests/test_base_function.py
+++ /dev/null
@@ -1,41 +0,0 @@
-"""
-This unit test suite tests functionality
-of Tinymovr boards.
-"""
-import time
-
-from avlos.unit_field import get_registry
-
-import unittest
-from tests import TMTestCase
-
-ureg = get_registry()
-A = ureg.ampere
-ticks = ureg.ticks
-s = ureg.second
-
-
-class TestBaseFunction(TMTestCase):
-    
-    def test_position_control(self):
-        """
-        Test position control
-        """
-        self.check_state(0)
-        self.tm.motor.I_cal = 5
-        self.tm.controller.current.Iq_limit = 5
-        self.try_calibrate()
-        self.tm.controller.position_mode()
-        self.check_state(2)
-
-        for i in range(5):
-            self.tm.controller.position.setpoint = i * 3000 * ticks
-            time.sleep(0.25)
-            self.assertAlmostEqual(
-                i * 3000 * ticks, self.tm.encoder.position_estimate, delta=1000 * ticks
-            )
-
-
-
-if __name__ == "__main__":
-    unittest.main(failfast=True)
diff --git a/studio/Python/tests/test_board.py b/studio/Python/tests/test_board.py
index 6316d2ca..ae118351 100644
--- a/studio/Python/tests/test_board.py
+++ b/studio/Python/tests/test_board.py
@@ -22,27 +22,33 @@
 from avlos.unit_field import get_registry
 
 import unittest
+import pytest
 from tests import TMTestCase
 
 ureg = get_registry()
 A = ureg.ampere
 ticks = ureg.ticks
 s = ureg.second
-tsleep = 0.19
+tsleep = 0.20
 
 
 class TestBoard(TMTestCase):
-    def test_a_encoder(self):
+
+    @pytest.mark.hitl_default
+    @pytest.mark.hitl_mini
+    def test_a_sensor_readings(self):
         """
-        Test encoder readings
+        Test sensor readings
         """
         pos_estimates = []
         for _ in range(500):
-            pos_estimates.append(self.tm.encoder.position_estimate)
+            pos_estimates.append(self.tm.sensors.user_frame.position_estimate)
             time.sleep(0.001)
         # apparently the statistics lib works with quantities only
         self.assertLess(st.pstdev(pos_estimates) * ticks, 5 * ticks)
 
+    @pytest.mark.hitl_default
+    @pytest.mark.hitl_mini
     def test_b_invalid_values(self):
         """
         Test rejection of invalid values for limits and gains
@@ -72,6 +78,8 @@ def test_b_invalid_values(self):
 
         time.sleep(2)
 
+    @pytest.mark.hitl_default
+    @pytest.mark.hitl_mini
     def test_c_calibrate(self):
         """
         Test board calibration if not calibrated
@@ -79,26 +87,40 @@ def test_c_calibrate(self):
         self.check_state(0)
         self.try_calibrate()
 
+    @pytest.mark.eol
+    @pytest.mark.hitl_default
+    @pytest.mark.hitl_mini
     def test_d_position_control(self):
         """
         Test position control
         """
+        hw_rev = self.tm.hw_revision
         self.check_state(0)
+        self.tm.controller.current.Iq_limit = 5
         self.try_calibrate()
         self.tm.controller.position_mode()
         self.check_state(2)
 
-        for i in range(10):
-            self.tm.controller.position.setpoint = i * 1000 * ticks
-            time.sleep(tsleep)
+        self.tm.controller.position.setpoint = 0
+        time.sleep(0.4)
+
+        for i in range(5):
+            self.tm.controller.position.setpoint = i * 3000 * ticks
+            if hw_rev > 20:
+                time.sleep(0.50)
+            else:
+                time.sleep(0.20)
             self.assertAlmostEqual(
-                i * 1000 * ticks, self.tm.encoder.position_estimate, delta=1000 * ticks
+                i * 3000 * ticks, self.tm.sensors.user_frame.position_estimate, delta=1000 * ticks
             )
 
+    @pytest.mark.hitl_default
+    @pytest.mark.hitl_mini
     def test_e_velocity_control(self):
         """
         Test velocity control
         """
+        hw_rev = self.tm.hw_revision
         self.check_state(0)
         self.try_calibrate()
         self.tm.controller.velocity_mode()
@@ -106,54 +128,73 @@ def test_e_velocity_control(self):
 
         R = 15
 
+        if hw_rev > 20:
+            multiplier = 4000 * ticks / s
+            max_delta = 15000 * ticks / s
+        else:
+            multiplier = 20000 * ticks / s
+            max_delta = 30000 * ticks / s
+
         velocity_pairs = []
 
         for i in range(R):
-            target = i * 20000 * ticks / s
+            target = i * multiplier
             self.tm.controller.velocity.setpoint = target
             time.sleep(tsleep)
-            velocity_pairs.append((target, self.tm.encoder.velocity_estimate))
+            velocity_pairs.append((target, self.tm.sensors.user_frame.velocity_estimate))
 
         for i in range(R):
-            target = (R - i) * 20000 * ticks / s
+            target = (R - i) * multiplier
             self.tm.controller.velocity.setpoint = target
             time.sleep(tsleep)
-            velocity_pairs.append((target, self.tm.encoder.velocity_estimate))
+            velocity_pairs.append((target, self.tm.sensors.user_frame.velocity_estimate))
 
         for i in range(R):
-            target = -i * 20000 * ticks / s
+            target = -i * multiplier
             self.tm.controller.velocity.setpoint = target
             time.sleep(tsleep)
-            velocity_pairs.append((target, self.tm.encoder.velocity_estimate))
+            velocity_pairs.append((target, self.tm.sensors.user_frame.velocity_estimate))
 
         for i in range(R):
-            target = (i - R) * 20000 * ticks / s
+            target = (i - R) * multiplier
             self.tm.controller.velocity.setpoint = target
             time.sleep(tsleep)
-            velocity_pairs.append((target, self.tm.encoder.velocity_estimate))
+            velocity_pairs.append((target, self.tm.sensors.user_frame.velocity_estimate))
 
         for target, estimate in velocity_pairs:
-            self.assertAlmostEqual(target, estimate, delta=30000 * ticks / s)
+            self.assertAlmostEqual(target, estimate, delta=max_delta)
 
+    @pytest.mark.hitl_default
+    @pytest.mark.hitl_mini
     def test_f_random_pos_control(self):
         """
         Test random positions
         """
+        hw_rev = self.tm.hw_revision
         self.check_state(0)
         self.try_calibrate()
         self.tm.controller.position_mode()
         self.check_state(2)
 
-        for _ in range(10):
-            new_pos = random.uniform(-24000, 24000)
+        if hw_rev > 20:
+            self.tm.controller.velocity.limit = 100000 * ticks / s
+            tick_range = 10000
+        else:
+            self.tm.controller.velocity.limit = 200000 * ticks / s
+            tick_range = 24000
+
+        for _ in range(15):
+            new_pos = random.uniform(-tick_range, tick_range)
             self.tm.controller.position.setpoint = new_pos * ticks
+            
             time.sleep(0.35)
             self.assertAlmostEqual(
-                self.tm.encoder.position_estimate,
+                self.tm.sensors.user_frame.position_estimate,
                 self.tm.controller.position.setpoint,
-                delta=1900 * ticks,
+                delta=2000 * ticks,
             )
 
+    @pytest.mark.hitl_default
     def test_g_limits(self):
         """
         Test limits
@@ -176,28 +217,30 @@ def test_g_limits(self):
         time.sleep(0.5)
         self.assertAlmostEqual(
             30000 * ticks / s,
-            self.tm.encoder.velocity_estimate,
+            self.tm.sensors.user_frame.velocity_estimate,
             delta=5000 * ticks / s,
         )
         self.tm.controller.velocity.setpoint = -400000 * ticks / s
         time.sleep(0.5)
         self.assertAlmostEqual(
             -30000 * ticks / s,
-            self.tm.encoder.velocity_estimate,
+            self.tm.sensors.user_frame.velocity_estimate,
             delta=5000 * ticks / s,
         )
 
         self.tm.controller.velocity.setpoint = 0
         time.sleep(0.5)
 
-    # def test_h_timings(self):
-    #     """
-    #     Test DWT busy/total cycle timings
-    #     """
-    #     self.assertGreater(self.tm.scheduler.total, 0)
-    #     self.assertGreater(self.tm.scheduler.busy, 0)
-    #     self.assertLess(self.tm.scheduler.busy, 3000)
+    @pytest.mark.hitl_default
+    def test_h_timings(self):
+        """
+        Test DWT busy/total cycle timings
+        """
+        if (self.tm.scheduler.load == 0 or self.tm.scheduler.load > 7000):
+            self.skipTest("Invalid timing values. Skipping test.")
+        self.assertLess(self.tm.scheduler.load, 4000)
 
+    @pytest.mark.hitl_default
     def test_i_states(self):
         """
         Test state transitions
@@ -225,52 +268,54 @@ def test_i_states(self):
         self.assertEqual(self.tm.controller.state, 2)
         self.tm.controller.idle()
 
+    @pytest.mark.hitl_default
     def test_j_gimbal_mode(self):
         """
         Test gimbal mode
         """
+        if self.tm.hw_revision > 20 and self.tm.hw_revision < 30:
+            self.skipTest("Gimbal test not relevant to Tinymovr M5.x. Skipping test.")
         self.reset_and_wait()
         # Ensure we're idle
         self.check_state(0)
         self.tm.motor.type = 1  # gimbal
-        self.tm.motor.I_cal = 5.0
         self.tm.motor.R = 0.2
         self.tm.motor.L = 20 * 1e-5
         self.try_calibrate()
         self.tm.controller.position_mode()
         self.check_state(2)
 
-        for i in range(10):
-            self.tm.controller.position.setpoint = i * 1000 * ticks
-            time.sleep(0.2)
+        for i in range(5):
+            self.tm.controller.position.setpoint = i * 2000 * ticks
+            time.sleep(0.3)
             self.assertAlmostEqual(
-                i * 1000 * ticks, self.tm.encoder.position_estimate, delta=1000 * ticks
+                i * 2000 * ticks, self.tm.sensors.user_frame.position_estimate, delta=2000 * ticks
             )
-            time.sleep(0.4)
 
-    def test_k_rotor_offset_and_direction(self):
+    @pytest.mark.hitl_default
+    def test_k_rotor_offset_and_multiplier(self):
         """
-        Test rotor offset and direction
+        Test rotor offset and multiplier
         """
         self.check_state(0)
         self.try_calibrate()
         self.tm.controller.position_mode()
         self.check_state(2)
 
-        self.assertEqual(self.tm.motor.offset, 0)
-        self.assertEqual(self.tm.motor.direction, 1)
+        self.assertEqual(self.tm.sensors.user_frame.offset, 0)
+        self.assertEqual(self.tm.sensors.user_frame.multiplier, 1)
 
-        offset = self.tm.encoder.position_estimate
-        self.tm.motor.offset = offset
-        self.assertAlmostEqual(self.tm.encoder.position_estimate, 0, delta=100)
+        offset = self.tm.sensors.user_frame.position_estimate
+        self.tm.sensors.user_frame.offset = offset
+        self.assertAlmostEqual(self.tm.sensors.user_frame.position_estimate, 0, delta=100)
 
-        self.tm.motor.direction = -1
-        self.assertAlmostEqual(self.tm.encoder.position_estimate, 0, delta=100)
+        self.tm.sensors.user_frame.multiplier = -1
+        self.assertAlmostEqual(self.tm.sensors.user_frame.position_estimate, 0, delta=100)
 
         self.tm.controller.position.setpoint = offset
         time.sleep(0.5)
-        self.tm.motor.offset = 0
-        self.tm.motor.direction = -1
+        self.tm.sensors.user_frame.offset = 0
+        self.tm.sensors.user_frame.multiplier = -1
         self.assertAlmostEqual(self.tm.controller.position.setpoint, 0, delta=100)
 
     # def test_l_read_write_endpoints(self):
@@ -296,6 +341,7 @@ def test_k_rotor_offset_and_direction(self):
     #             20000 * k * ticks, values.position, delta=2000 * (k + 1) * ticks
     #         )
 
+    @pytest.mark.hitl_default
     def test_m_recalibrate(self):
         """
         Test re-calibration without reset
@@ -317,11 +363,12 @@ def test_m_recalibrate(self):
         self.tm.controller.position_mode()
         pos_estimates = []
         for k in range(50):
-            pos_estimates.append(self.tm.encoder.position_estimate)
+            pos_estimates.append(self.tm.sensors.user_frame.position_estimate)
             time.sleep(0.05)
         self.tm.controller.idle()
         self.assertLess(st.pstdev(pos_estimates) * ticks, 100 * ticks)
 
+    @pytest.mark.hitl_default
     def test_n_velocity_ramp(self):
         """
         Test velocity ramp
@@ -340,13 +387,14 @@ def test_n_velocity_ramp(self):
         self.tm.controller.velocity_mode()
         self.tm.controller.velocity.setpoint = 200000
         for k in range(100):
-            vel_estimates.append(self.tm.encoder.velocity_estimate.magnitude)
+            vel_estimates.append(self.tm.sensors.user_frame.velocity_estimate.magnitude)
             t_points.append(k * interval)
             time.sleep(interval)
         self.tm.controller.idle()
         a, _ = np.polyfit(t_points, vel_estimates, 1)
         self.assertAlmostEqual(a, ramp_value * frequency, delta=5000)
 
+    @pytest.mark.hitl_default
     def test_o_composite_set_get(self):
         """
         Test composite setter/getter function
@@ -355,14 +403,16 @@ def test_o_composite_set_get(self):
         # Ensure we're idle
         self.check_state(0)
         self.try_calibrate()
-        pos_estimate_ref = self.tm.encoder.position_estimate
+        pos_estimate_ref = self.tm.sensors.user_frame.position_estimate
         pos_estimate_comp = self.tm.controller.set_pos_vel_setpoints(0, 0) * ticks
         self.assertAlmostEqual(pos_estimate_ref, pos_estimate_comp, delta=200*ticks)
 
+    @pytest.mark.hitl_default
     def test_p_flux_braking(self):
         """
         Test flux braking
         """
+        self.skipTest("Temporarily skipping flux braking test.")
         self.reset_and_wait()
         # Ensure we're idle
         self.check_state(0)
diff --git a/studio/Python/tests/test_board_config.py b/studio/Python/tests/test_board_config.py
deleted file mode 100644
index f9f8dd9c..00000000
--- a/studio/Python/tests/test_board_config.py
+++ /dev/null
@@ -1,116 +0,0 @@
-"""
-Tinymovr Board Config Tests
-Copyright Ioannis Chatzikonstantinou 2020-2023
-
-Tests saving and loading Tinymovr configuration to/from NVRAM.
-
-This program is free software: you can redistribute it and/or modify it under
-the terms of the GNU General Public License as published by the Free Software
-Foundation, either version 3 of the License, or (at your option) any later
-version.
-This program is distributed in the hope that it will be useful, but WITHOUT
-ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
-FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
-You should have received a copy of the GNU General Public License along with
-this program. If not, see <http://www.gnu.org/licenses/>.
-"""
-
-import random
-import time
-
-from avlos.unit_field import get_registry
-
-import unittest
-from tests import TMTestCase
-
-ureg = get_registry()
-A = ureg.ampere
-tick = ureg.ticks
-s = ureg.second
-
-
-class TestBoardConfig(TMTestCase):
-    def test_a_state_errors(self):
-        """
-        Test state transitions
-        WARNING: This may perform one NVRAM erase cycle.
-        """
-        if self.tm.motor.calibrated:
-            self.tm.erase_config()
-            time.sleep(0.2)
-
-        self.reset_and_wait()
-        self.check_state(0)
-        self.assertEqual(self.tm.motor.calibrated, False)
-        self.tm.controller.position_mode()
-        self.check_state(0)
-        self.reset_and_wait()
-        self.check_state(0)
-        self.assertEqual(self.tm.motor.calibrated, False)
-        self.try_calibrate()
-        self.assertEqual(self.tm.motor.calibrated, True)
-        self.check_state(0)
-        time.sleep(0.2)
-        self.tm.controller.position_mode()
-        self.check_state(2)
-        time.sleep(0.2)
-        self.tm.controller.calibrate()
-        self.check_state(0)
-
-        self.reset_and_wait()
-
-    def test_b_save_load_config(self):
-        """
-        Test erasing, saving and loading of config.
-        WARNING: This will perform one NVRAM write and two erase cycles.
-        """
-        self.check_state(0)
-        self.tm.erase_config()
-        time.sleep(0.2)
-        self.assertEqual(self.tm.motor.calibrated, False)
-        self.try_calibrate()
-        self.assertEqual(self.tm.motor.calibrated, True)
-        R = self.tm.motor.R
-        L = self.tm.motor.L
-        pole_pairs = self.tm.motor.pole_pairs
-        self.tm.save_config()
-        time.sleep(0.2)
-        self.reset_and_wait()
-        self.assertEqual(self.tm.motor.calibrated, True)
-        self.assertAlmostEqual(R, self.tm.motor.R)
-        self.assertAlmostEqual(L, self.tm.motor.L)
-        self.assertAlmostEqual(pole_pairs, self.tm.motor.pole_pairs)
-        self.tm.erase_config()
-        time.sleep(0.2)
-
-    def test_c_parameter_persistence(self):
-        """
-        Test persisting parameters across config saves.
-        WARNING: This will perform one NVRAM write and two erase cycles.
-        """
-        self.check_state(0)
-        self.tm.erase_config()
-        time.sleep(0.2)
-
-        self.tm.controller.position.p_gain = 30
-        self.tm.controller.velocity.p_gain = 3e-5
-        self.tm.controller.velocity.i_gain = 2e-2
-        self.tm.controller.velocity.deadband = 100
-        self.tm.controller.velocity.limit = 120000
-        self.tm.controller.current.Iq_limit = 18
-        self.tm.save_config()
-        
-        time.sleep(0.2)
-        self.reset_and_wait()
-        self.assertAlmostEqual(self.tm.controller.position.p_gain, 30) #  * 1 / s
-        self.assertAlmostEqual(self.tm.controller.velocity.p_gain, 3e-5) #  * A * s / tick
-        self.assertAlmostEqual(self.tm.controller.velocity.i_gain, 2e-2) #  * A * s / tick
-        self.assertAlmostEqual(self.tm.controller.velocity.deadband, 100 * tick) 
-        self.assertAlmostEqual(self.tm.controller.velocity.limit, 120000 * tick / s)
-        self.assertAlmostEqual(self.tm.controller.current.Iq_limit, 18 * A)
-        self.tm.erase_config()
-        time.sleep(0.2)
-
-
-if __name__ == "__main__":
-    unittest.main()
diff --git a/studio/Python/tests/test_dfu.py b/studio/Python/tests/test_dfu.py
index 031626b7..5a1f75a7 100644
--- a/studio/Python/tests/test_dfu.py
+++ b/studio/Python/tests/test_dfu.py
@@ -15,29 +15,36 @@
 this program. If not, see <http://www.gnu.org/licenses/>.
 """
 
-import os
 import time
 import can
 
-from tinymovr import init_tee, destroy_tee
+try:
+    from rd6006 import RD6006
+    rd = RD6006('/dev/tty.usbserial-2110')
+    psu_connected = True
+except:
+    psu_connected = False
+
+from tinymovr import init_router, destroy_router
 from tinymovr.config import (
     get_bus_config,
-    create_device
+    create_device,
+    configure_logging
 )
 
 import unittest
-
+import pytest
 
 class TMTestCase(unittest.TestCase):
 
     @classmethod
     def setUp(cls):
-        params = get_bus_config(["canine", "slcan_disco"])
-        params["bitrate"] = 1000000
-        cls.can_bus = can.Bus(**params)
+        cls.logger = configure_logging()
+        cls.params = get_bus_config(["canine", "slcan_disco"], bitrate=1000000)
+        init_router(can.Bus, cls.params, logger=cls.logger)
 
+    @pytest.mark.dfu
     def test_dfu(self, node_id=1):
-        init_tee(self.can_bus)
         time.sleep(0.5)
         for i in range(10):
             print("Testing DFU iteration ", i+1)
@@ -48,22 +55,51 @@ def test_dfu(self, node_id=1):
             bl = create_device(node_id=node_id)
             bl_hash = bl.protocol_hash
             bl.reset()
-            time.sleep(0.2)
+            time.sleep(0.1)
             tm = create_device(node_id=node_id)
             tm_hash2 = tm.protocol_hash
             tm.reset()
-            time.sleep(0.2)
+            time.sleep(0.1)
+            tm_hash3 = tm.protocol_hash
+            self.assertNotEqual(tm_hash, 0)
+            self.assertEqual(tm_hash, tm_hash2)
+            self.assertEqual(tm_hash, tm_hash3)
+            self.assertNotEqual(tm_hash, bl_hash)
+            time.sleep(0.1)
+
+    @pytest.mark.dfu
+    def test_dfu_power_off(self, node_id=1):
+        if psu_connected == False:
+            raise unittest.SkipTest("Skipping test because no connected RD series PSU found")
+        time.sleep(0.5)
+        for i in range(3):
+            print("Testing DFU iteration ", i+1)
+            tm = create_device(node_id=node_id)
+            tm_hash = tm.protocol_hash
+            tm.enter_dfu()
+            time.sleep(0.5)
+            bl = create_device(node_id=node_id)
+            bl_hash = bl.protocol_hash
+            rd.enable=False
+            time.sleep(2.0)
+            rd.enable=True
+            time.sleep(0.5)
+            tm = create_device(node_id=node_id)
+            tm_hash2 = tm.protocol_hash
+            rd.enable=False
+            time.sleep(2.0)
+            rd.enable=True
+            time.sleep(0.5)
             tm_hash3 = tm.protocol_hash
             self.assertNotEqual(tm_hash, 0)
             self.assertEqual(tm_hash, tm_hash2)
             self.assertEqual(tm_hash, tm_hash3)
             self.assertNotEqual(tm_hash, bl_hash)
-            time.sleep(0.2)
+            time.sleep(0.1)
 
     @classmethod
     def tearDownClass(cls):
-        destroy_tee()
-        cls.can_bus.shutdown()
+        destroy_router()
 
 
 if __name__ == "__main__":
diff --git a/studio/Python/tests/test_hall.py b/studio/Python/tests/test_hall.py
new file mode 100644
index 00000000..4821fbc4
--- /dev/null
+++ b/studio/Python/tests/test_hall.py
@@ -0,0 +1,105 @@
+"""
+Tinymovr Board Config Tests
+Copyright Ioannis Chatzikonstantinou 2020-2023
+
+Tests saving and loading Tinymovr configuration to/from NVRAM.
+
+This program is free software: you can redistribute it and/or modify it under
+the terms of the GNU General Public License as published by the Free Software
+Foundation, either version 3 of the License, or (at your option) any later
+version.
+This program is distributed in the hope that it will be useful, but WITHOUT
+ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+You should have received a copy of the GNU General Public License along with
+this program. If not, see <http://www.gnu.org/licenses/>.
+"""
+
+import random
+import time
+
+from avlos.unit_field import get_registry
+
+import unittest
+import pytest
+from tests import TMTestCase
+
+ureg = get_registry()
+A = ureg.ampere
+tick = ureg.ticks
+s = ureg.second
+tsleep = 1.00
+
+
+def set_pole_pairs(tm):
+    tm.motor.pole_pairs = 15
+
+class TestHall(TMTestCase):
+
+    @pytest.mark.sensor_hall
+    def test_a_position_control_w_loaded_config(self):
+        """
+        Test position control after saving and loading config.
+        WARNING: This will perform one NVRAM write and two erase cycles.
+        """
+        self.check_state(0)
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+        self.tm.sensors.select.position_sensor.connection = self.tm.sensors.select.position_sensor.connection.HALL
+        self.tm.sensors.select.commutation_sensor.connection = self.tm.sensors.select.commutation_sensor.connection.HALL
+        # Commutation sensor remains internal
+
+        self.tm.sensors.select.position_sensor.bandwidth = 50;
+        self.tm.sensors.select.commutation_sensor.bandwidth = 80;
+        time.sleep(0.2)
+
+        self.try_calibrate(precheck_callback=set_pole_pairs)
+
+        self.tm.controller.position.p_gain = 15
+        self.tm.controller.velocity.limit = 400000
+        self.tm.controller.velocity.p_gain = 1e-5
+        self.tm.controller.velocity.i_gain = 0
+
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(5):
+            new_pos = random.uniform(-200000, 200000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=30000 * tick,
+            )
+
+        self.tm.controller.idle()
+
+        time.sleep(0.1)
+
+        self.tm.save_config()
+        
+        time.sleep(0.2)
+
+        self.reset_and_wait()
+        
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(10):
+            new_pos = random.uniform(-200000, 200000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=30000 * tick,
+            )
+
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/studio/Python/tests/test_nvm.py b/studio/Python/tests/test_nvm.py
new file mode 100644
index 00000000..dccce1f0
--- /dev/null
+++ b/studio/Python/tests/test_nvm.py
@@ -0,0 +1,250 @@
+"""
+Tinymovr Board Config Tests
+Copyright Ioannis Chatzikonstantinou 2020-2023
+
+Tests saving and loading Tinymovr configuration to/from NVRAM.
+
+This program is free software: you can redistribute it and/or modify it under
+the terms of the GNU General Public License as published by the Free Software
+Foundation, either version 3 of the License, or (at your option) any later
+version.
+This program is distributed in the hope that it will be useful, but WITHOUT
+ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+You should have received a copy of the GNU General Public License along with
+this program. If not, see <http://www.gnu.org/licenses/>.
+"""
+
+import random
+import time
+
+from avlos.unit_field import get_registry
+
+import unittest
+from tests import TMTestCase
+
+ureg = get_registry()
+A = ureg.ampere
+tick = ureg.ticks
+s = ureg.second
+tsleep = 0.30
+
+
+class TestNVM(TMTestCase):
+    def test_a_state_errors(self):
+        """
+        Test state transitions
+        WARNING: This may perform one NVRAM erase cycle.
+        """
+        if self.tm.motor.calibrated:
+            self.tm.erase_config()
+            time.sleep(0.2)
+
+        self.reset_and_wait()
+        self.check_state(0)
+        self.assertEqual(self.tm.motor.calibrated, False)
+        self.tm.controller.position_mode()
+        self.check_state(0)
+        self.reset_and_wait()
+        self.check_state(0)
+        self.assertEqual(self.tm.motor.calibrated, False)
+        self.try_calibrate()
+        self.assertEqual(self.tm.motor.calibrated, True)
+        self.check_state(0)
+        time.sleep(0.2)
+        self.tm.controller.position_mode()
+        self.check_state(2)
+        time.sleep(0.2)
+        self.tm.controller.calibrate()
+        self.check_state(0)
+
+        self.reset_and_wait()
+
+    def test_b_save_load_config(self):
+        """
+        Test erasing, saving and loading of config.
+        WARNING: This will perform one NVRAM write and two erase cycles.
+        """
+        self.check_state(0)
+        self.tm.erase_config()
+        time.sleep(0.2)
+        self.assertEqual(self.tm.motor.calibrated, False)
+        self.try_calibrate()
+        self.assertEqual(self.tm.motor.calibrated, True)
+        R = self.tm.motor.R
+        L = self.tm.motor.L
+        pole_pairs = self.tm.motor.pole_pairs
+        self.tm.save_config()
+        time.sleep(0.2)
+        self.reset_and_wait()
+        self.assertEqual(self.tm.motor.calibrated, True)
+        self.assertAlmostEqual(R, self.tm.motor.R)
+        self.assertAlmostEqual(L, self.tm.motor.L)
+        self.assertAlmostEqual(pole_pairs, self.tm.motor.pole_pairs)
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+    def test_c_parameter_persistence(self):
+        """
+        Test persisting parameters across config saves.
+        WARNING: This will perform one NVRAM write and two erase cycles.
+        """
+        self.check_state(0)
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+        self.tm.controller.position.p_gain = 30
+        self.tm.controller.velocity.p_gain = 3e-5
+        self.tm.controller.velocity.i_gain = 2e-2
+        self.tm.controller.velocity.deadband = 100
+        self.tm.controller.velocity.limit = 120000
+        self.tm.controller.current.Iq_limit = 18
+        self.tm.save_config()
+        
+        time.sleep(0.2)
+        self.reset_and_wait()
+        self.assertAlmostEqual(self.tm.controller.position.p_gain, 30) #  * 1 / s
+        self.assertAlmostEqual(self.tm.controller.velocity.p_gain, 3e-5) #  * A * s / tick
+        self.assertAlmostEqual(self.tm.controller.velocity.i_gain, 2e-2) #  * A * s / tick
+        self.assertAlmostEqual(self.tm.controller.velocity.deadband, 100 * tick) 
+        self.assertAlmostEqual(self.tm.controller.velocity.limit, 120000 * tick / s)
+        self.assertAlmostEqual(self.tm.controller.current.Iq_limit, 18 * A)
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+    def test_d_position_control_w_loaded_config(self):
+        """
+        Test position control after saving and loading config.
+        WARNING: This will perform one NVRAM write and two erase cycles.
+        """
+        self.check_state(0)
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+        self.try_calibrate()
+
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(15):
+            new_pos = random.uniform(-24000, 24000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=2000 * tick,
+            )
+
+        self.tm.controller.idle()
+
+        time.sleep(0.1)
+
+        self.tm.save_config()
+        
+        time.sleep(0.2)
+
+        self.reset_and_wait()
+        
+        self.tm.controller.position_mode()
+        self.check_state(2)
+
+        for _ in range(15):
+            new_pos = random.uniform(-24000, 24000)
+            self.tm.controller.position.setpoint = new_pos * tick
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.position_estimate,
+                self.tm.controller.position.setpoint,
+                delta=2000 * tick,
+            )
+
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+    def test_e_save_settings_and_reset(self):
+        """
+        Test case for issue reported by @Phosfor on Discord:
+        1. Configure current limits etc.
+        2. Calibrate the controller
+        3. Make sure everything is working (i.e. enter current/velocity mode)
+        4. Save the configuration
+        5. Reset the tinymovr; all settings are still correct and the tinymovr reports that it is calibrated
+        6. Try to enter current/velocity mode again; the tinymovr says it is in the correct mode and in the control state, but the motor is not turning
+        """
+        # Step 1: Configure current limits etc.
+        self.check_state(0)
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+        self.tm.controller.current.Iq_limit = 15 * A
+        self.tm.controller.velocity.limit = 200000 * tick / s
+        self.tm.controller.velocity.i_gain = 1e-5
+        self.tm.controller.velocity.p_gain = 1.6e-4
+        self.tm.controller.velocity.deadband = 50 * tick
+        self.tm.traj_planner.max_vel = 110000 * tick / s
+        self.tm.traj_planner.max_accel = 500000 * tick / s
+        self.tm.traj_planner.max_decel = 500000 * tick / (s * s)
+
+        # Step 2: Calibrate the controller
+        self.try_calibrate()
+        self.assertEqual(self.tm.motor.calibrated, True)
+
+        # Step 3: Make sure everything is working (i.e. enter current/velocity mode)
+        self.tm.controller.velocity_mode()
+        self.check_state(2)
+        
+        for _ in range(5):
+            new_velocity = random.uniform(-50000, 50000)
+            self.tm.controller.velocity.setpoint = new_velocity * tick / s
+            time.sleep(tsleep)
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.velocity_estimate,
+                self.tm.controller.velocity.setpoint,
+                delta=5000 * tick / s,
+            )
+        
+        self.tm.controller.idle()
+        time.sleep(0.1)
+
+        # Step 4: Save the configuration
+        self.tm.save_config()
+        time.sleep(0.2)
+
+        # Step 5: Reset the tinymovr; all settings are still correct and the tinymovr reports that it is calibrated
+        self.reset_and_wait()
+        self.assertEqual(self.tm.motor.calibrated, True)
+        self.assertAlmostEqual(self.tm.controller.current.Iq_limit, 15 * A)
+        self.assertAlmostEqual(self.tm.controller.velocity.limit, 200000 * tick / s)
+        self.assertAlmostEqual(self.tm.controller.velocity.i_gain, 1e-5)
+        self.assertAlmostEqual(self.tm.controller.velocity.p_gain, 1.6e-4)
+        self.assertAlmostEqual(self.tm.controller.velocity.deadband, 50 * tick)
+        self.assertAlmostEqual(self.tm.traj_planner.max_vel, 110000 * tick / s)
+        self.assertAlmostEqual(self.tm.traj_planner.max_accel, 500000 * tick / s)
+        self.assertAlmostEqual(self.tm.traj_planner.max_decel, 500000 * tick / (s * s))
+
+        # Step 6: Try to enter current/velocity mode again; the tinymovr says it is in the correct mode and in the control state, but the motor is not turning (and I don't hear any switching noise that I usually hear when the tinymovr is in the active state)
+        self.tm.controller.velocity_mode()
+        self.check_state(2)
+        
+        for _ in range(5):
+            new_velocity = random.uniform(-50000, 50000)
+            self.tm.controller.velocity.setpoint = new_velocity * tick / s
+            time.sleep(tsleep)
+            # Here we expect the motor not to turn, so we check for lack of movement
+            self.assertAlmostEqual(
+                self.tm.sensors.user_frame.velocity_estimate,
+                self.tm.controller.velocity.setpoint,
+                delta=5000 * tick / s,
+            )
+        
+        self.tm.controller.idle()
+        time.sleep(0.1)
+
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/studio/Python/tests/test_rates.py b/studio/Python/tests/test_rates.py
index 3634649c..5120d4c1 100644
--- a/studio/Python/tests/test_rates.py
+++ b/studio/Python/tests/test_rates.py
@@ -18,11 +18,14 @@
 import time
 from tests import TMTestCase
 
+import pytest
 
 iterations = 5000
 
 
 class TestRates(TMTestCase):
+
+    @pytest.mark.hitl_default
     def test_round_trip_time(self):
         """
         Test round-trip message time (2 packets)
@@ -33,7 +36,7 @@ def test_round_trip_time(self):
         elapsed_time()
         sum = 0
         for _ in range(iterations):
-            sum += self.tm.encoder.position_estimate
+            sum += self.tm.sensors.user_frame.position_estimate
         res = elapsed_time()
         print("Round-trip time (2 packets): " + str(res / iterations) + " seconds")
 
diff --git a/studio/Python/tests/test_simulation.py b/studio/Python/tests/test_simulation.py
index b9b7a35e..4510b974 100644
--- a/studio/Python/tests/test_simulation.py
+++ b/studio/Python/tests/test_simulation.py
@@ -15,30 +15,38 @@
 this program. If not, see <http://www.gnu.org/licenses/>.
 """
 
-from tinymovr import init_tee, destroy_tee
+import unittest
+from unittest.mock import patch, MagicMock
+from tinymovr import init_router, destroy_router
 from tinymovr.channel import ResponseError
 from tinymovr.config import create_device
-from unittest.mock import patch, MagicMock
-import unittest
-
 
 class TestSimulation(unittest.TestCase):
     
     @patch("can.Bus")
-    def test_response_error(self, can_bus):
+    def test_response_error(self, mock_can_bus_class):
         """
-        Test response error
+        Test that an appropriate error is raised when the device receives an erroneous response.
         """
-        can_bus.send = MagicMock()
-        can_bus.recv = MagicMock()
-        init_tee(can_bus)
+        # Setup mock
+        mock_can_bus_instance = MagicMock()
+        mock_can_bus_class.return_value = mock_can_bus_instance
+        mock_params = MagicMock()
+        mock_logger = MagicMock()
+        
+        # Initialize the router with the mocked class and parameters
+        init_router(mock_can_bus_class, mock_params, logger=mock_logger)
+        
+        # Test that an error is raised during device creation
         with self.assertRaises(ResponseError):
-            tm = create_device(node_id=1)
-        assert can_bus.send.called
-        assert can_bus.recv.called
-        destroy_tee()
-
-
+            create_device(node_id=1)
+        
+        # Check if send and recv were called on the can bus instance
+        assert mock_can_bus_instance.send.called
+        assert mock_can_bus_instance.recv.called
+        
+        # Clean up router
+        destroy_router()
 
 if __name__ == "__main__":
     unittest.main()
diff --git a/studio/Python/tests/test_watchdog.py b/studio/Python/tests/test_watchdog.py
index a22eae0f..33df2bbe 100644
--- a/studio/Python/tests/test_watchdog.py
+++ b/studio/Python/tests/test_watchdog.py
@@ -7,6 +7,7 @@
 from avlos.unit_field import get_registry
 
 import unittest
+import pytest
 from tests import TMTestCase
 
 ureg = get_registry()
@@ -16,6 +17,8 @@
 tsleep = 0.18
 
 class TestWatchdog(TMTestCase):
+
+    @pytest.mark.hitl_default
     def test_watchdog(self):
         """
         Test Watchdog function
diff --git a/studio/Python/tests/tm_test_case.py b/studio/Python/tests/tm_test_case.py
index 8a6d6c94..0b31e8d8 100644
--- a/studio/Python/tests/tm_test_case.py
+++ b/studio/Python/tests/tm_test_case.py
@@ -19,20 +19,32 @@
 import time
 import can
 
-from tinymovr import init_tee, destroy_tee
-from tinymovr.config import get_bus_config, create_device
+from tinymovr import init_router, destroy_router
+from tinymovr.config import get_bus_config, create_device, configure_logging
 
 import unittest
 
 
 class TMTestCase(unittest.TestCase):
+
+    @classmethod
+    def configure_sensors(cls, external_spi_type):
+        cls.tm.sensors.setup.external_spi.type = external_spi_type
+        time.sleep(0.2)
+
+    @classmethod
+    def select_sensors(cls, commutation_sensor_connection, position_sensor_connection):
+        cls.tm.sensors.select.commutation_sensor.connection = commutation_sensor_connection
+        cls.tm.sensors.select.position_sensor.connection = position_sensor_connection
+        time.sleep(0.2)
+
     @classmethod
     def setUpClass(cls):
-        params = get_bus_config(["canine", "slcan_disco"])
-        params["bitrate"] = 1000000
-        cls.can_bus = can.Bus(**params)
-        init_tee(cls.can_bus)
+        params = get_bus_config(["canine", "slcan_disco"], bitrate=1000000)
+        cls.logger = configure_logging()
+        init_router(can.Bus, params, logger=cls.logger)
         cls.tm = create_device(node_id=1)
+        
         cls.reset_and_wait()
 
     def tearDown(self):
@@ -41,18 +53,31 @@ def tearDown(self):
     @classmethod
     def tearDownClass(cls):
         cls.tm.reset()
-        destroy_tee()
-        cls.can_bus.shutdown()
+        destroy_router()
 
     @classmethod
     def reset_and_wait(cls, timeout=0.5):
         cls.tm.reset()
         time.sleep(timeout)
 
-    def try_calibrate(self, force=False, *args, **kwargs):
+    def try_calibrate(self, I_cal=None, force=False, precheck_callback=None,*args, **kwargs):
         if True == force or not self.tm.calibrated:
+            hw_rev = self.tm.hw_revision
+            self.check_state(0)
+            if I_cal and I_cal > 0:
+                self.tm.motor.I_cal = I_cal
+            elif hw_rev > 20:
+                self.tm.motor.I_cal = 0.8
+            else:
+                self.tm.motor.I_cal = 5
+
+            if hw_rev > 20:
+                self.tm.controller.velocity.P_gain = 2e-05
+
             self.tm.controller.calibrate()
             self.wait_for_calibration(*args, **kwargs)
+            if precheck_callback:
+                precheck_callback(self.tm)
             self.assertTrue(self.tm.calibrated)
 
     def wait_for_calibration(self, check_interval=0.05):