Repository: tinymovr/Tinymovr (language: python)

Issue:
Fix protocol hash incompatibility in the latest release
This is a workaround rather than a real fix. We are still investigating as to why the protocol hash in the latest release was altered, without altering the spec file.


Discussion / hints:

Fix patch:
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/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/conf.py b/docs/conf.py
index 0496d220..d246bec3 100644
--- a/docs/conf.py
+++ b/docs/conf.py
@@ -20,7 +20,7 @@
 # -- Project information -----------------------------------------------------
 
 project = 'Tinymovr'
-copyright = '2020, Yannis Chatzikonstantinou'
+copyright = '2020-2024, Yannis Chatzikonstantinou'
 author = 'Yannis Chatzikonstantinou'
 
 
diff --git a/docs/hardware/connectors_m5_2.png b/docs/hardware/connectors_m5_2.png
new file mode 100644
index 00000000..c6b577bd
Binary files /dev/null and b/docs/hardware/connectors_m5_2.png differ
diff --git a/docs/hardware/guide.rst b/docs/hardware/guide.rst
index 59aaff11..90b0c0d5 100644
--- a/docs/hardware/guide.rst
+++ b/docs/hardware/guide.rst
@@ -73,9 +73,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 +118,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 +148,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/hardware/overview.rst b/docs/hardware/overview.rst
index c127ca44..8e43f896 100644
--- a/docs/hardware/overview.rst
+++ b/docs/hardware/overview.rst
@@ -54,12 +54,19 @@ Board Dimensions (M5.1, M5.2)
   :width: 800
   :alt: Tinymovr M5 dimensions
 
-Connectivity (M5.1, M5.2)
+Connectivity (M5.1)
 #########################
 
 .. image:: connectors_m5.png
   :width: 800
-  :alt: Tinymovr M5 connectors and pinouts
+  :alt: Tinymovr M5.1 connectors and pinouts
+
+Connectivity (M5.2)
+#########################
+
+.. image:: connectors_m5_2.png
+  :width: 800
+  :alt: Tinymovr M5.2 connectors and pinouts
 
 
 R3.x
diff --git a/docs/protocol/reference.rst b/docs/protocol/reference.rst
index d24de64f..0856aa3e 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,278 @@ 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.load
+-------------------------------------------------------------------
+
+ID: 16
+
+Type: uint32
+
+
+
+Processor load in ticks per PWM cycle.
+
+
 
 scheduler.errors
 -------------------------------------------------------------------
 
-ID: 14
+ID: 17
+
 Type: uint8
 
 
+
 Any scheduler errors, 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
 
+- CURRENT_LIMIT_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 +407,914 @@ 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
+ID: 47
+
+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
 
-User-defined offset of the motor.
 
 
+Any motor/calibration errors, as a bitmask
+
+Flags: 
+
+- PHASE_RESISTANCE_OUT_OF_RANGE
+
+- PHASE_INDUCTANCE_OUT_OF_RANGE
 
+- INVALID_POLE_PAIRS
 
-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 external sensor type. Either MA7XX, AS5047 or AMT22.
+
+Options: 
+
+- MA7XX
+
+- AS5047
+
+- AMT22
+
+sensors.setup.external_spi.calibrated
+-------------------------------------------------------------------
+
+ID: 62
+
+Type: bool
+
+
+
+Whether the sensor has been calibrated.
+
+
+
+sensors.setup.external_spi.errors
+-------------------------------------------------------------------
+
+ID: 63
+
+Type: uint8
+
+
+
+Any sensor errors, as a bitmask
+
+Flags: 
+
+- CALIBRATION_FAILED
+
+- READING_UNSTABLE
+
+sensors.setup.hall.calibrated
+-------------------------------------------------------------------
+
+ID: 64
+
+Type: bool
+
+
+
+Whether the sensor has been calibrated.
+
+
+
+sensors.setup.hall.errors
+-------------------------------------------------------------------
+
+ID: 65
+
+Type: uint8
+
+
+
+Any sensor errors, as a bitmask
+
+Flags: 
+
+- CALIBRATION_FAILED
+
+- READING_UNSTABLE
+
+sensors.select.position_sensor.connection
+-------------------------------------------------------------------
+
+ID: 66
+
+Type: uint8
+
+
+
+The position sensor connection. Either ONBOARD, EXTERNAL_SPI or HALL.
+
+Options: 
+
+- ONBOARD
+
+- EXTERNAL_SPI
+
+- HALL
+
+sensors.select.position_sensor.bandwidth
+-------------------------------------------------------------------
+
+ID: 67
+
 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: 68
+
+Type: int32
+
+
+
+The raw position sensor angle.
+
+
+
+sensors.select.position_sensor.position_estimate
+-------------------------------------------------------------------
+
+ID: 69
+
 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: 70
+
+Type: float
+
+Units: tick / second
+
+The filtered velocity estimate in the position sensor reference frame.
+
+
+
+sensors.select.commutation_sensor.connection
+-------------------------------------------------------------------
+
+ID: 71
+
 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: 72
+
 Type: float
+
 Units: hertz
 
-The encoder observer bandwidth.
+The commutation sensor observer bandwidth.
+
+
+
+sensors.select.commutation_sensor.raw_angle
+-------------------------------------------------------------------
+
+ID: 73
+
+Type: int32
+
 
 
+The raw commutation sensor angle.
 
 
-encoder.calibrated
+
+sensors.select.commutation_sensor.position_estimate
 -------------------------------------------------------------------
 
-ID: 57
-Type: bool
+ID: 74
 
+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: 75
 
+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: 76
+
 Type: float
+
 Units: tick / second
 
 The max allowed acceleration of the generated trajectory.
 
 
 
-
 traj_planner.max_decel
 -------------------------------------------------------------------
 
-ID: 60
+ID: 77
+
 Type: float
+
 Units: tick / second ** 2
 
 The max allowed deceleration of the generated trajectory.
 
 
 
-
 traj_planner.max_vel
 -------------------------------------------------------------------
 
-ID: 61
+ID: 78
+
 Type: float
+
 Units: tick / second
 
 The max allowed cruise velocity of the generated trajectory.
 
 
 
-
 traj_planner.t_accel
 -------------------------------------------------------------------
 
-ID: 62
+ID: 79
+
 Type: float
+
 Units: second
 
 In time mode, the acceleration time of the generated trajectory.
 
 
 
-
 traj_planner.t_decel
 -------------------------------------------------------------------
 
-ID: 63
+ID: 80
+
 Type: float
+
 Units: second
 
 In time mode, the deceleration time of the generated trajectory.
 
 
 
-
 traj_planner.t_total
 -------------------------------------------------------------------
 
-ID: 64
+ID: 81
+
 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: 82
 
-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: 83
 
-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: 84
+
 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: 85
+
 Type: float
+
 Units: tick / second
 
 The velocity at which the motor performs homing.
 
 
 
-
 homing.max_homing_t
 -------------------------------------------------------------------
 
-ID: 69
+ID: 86
+
 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: 87
+
 Type: float
+
 Units: tick
 
 The retraction distance the motor travels after the endstop has been found.
 
 
 
-
 homing.warnings
 -------------------------------------------------------------------
 
-ID: 71
+ID: 88
+
 Type: uint8
 
 
+
 Any homing warnings, as a bitmask
 
 Flags: 
-- HOMING_TIMEOUT
 
+- HOMING_TIMEOUT
 
 homing.stall_detect.velocity
 -------------------------------------------------------------------
 
-ID: 72
+ID: 89
+
 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: 90
+
 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: 91
+
 Type: float
+
 Units: second
 
 The time to remain in stall detection mode before the motor is considered stalled.
 
 
 
-
 home() -> void
--------------------------------------------------------------------
+--------------------------------------------------------------------------------------------
+
+ID: 92
 
-ID: 75
 Return Type: void
 
 
-Perform the homing operation.
 
+Perform the homing operation.
 
 watchdog.enabled
 -------------------------------------------------------------------
 
-ID: 76
+ID: 93
+
 Type: bool
 
 
-Whether the watchdog is enabled or not.
 
+Whether the watchdog is enabled or not.
 
 
 
 watchdog.triggered
 -------------------------------------------------------------------
 
-ID: 77
+ID: 94
+
 Type: bool
 
 
-Whether the watchdog has been triggered or not.
 
+Whether the watchdog has been triggered or not.
 
 
 
 watchdog.timeout
 -------------------------------------------------------------------
 
-ID: 78
+ID: 95
+
 Type: float
+
 Units: second
 
 The watchdog timeout period.
 
 
 
-
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/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..c4afca17 100644
--- a/docs/sensors/sensors.rst
+++ b/docs/sensors/sensors.rst
@@ -1,20 +1,130 @@
 Sensors and Encoders
 ####################
 
+
 Overview
 ********
 
 This document provides information and guides for using the various angle sensor and encoder types supported by Tinymovr.
 
-Observer bandwidth
-******************
 
-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.
+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.
+
+.. 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
+
+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 Tinymovr board. The pinout for the Hall effect sensor connector is shown below.
+
+.. image:: connectors_R52.jpg
+  :width: 800
+  :alt: Tinymovr R5.2 connector diagram
+
+.. image:: connectors_R51.jpg
+  :width: 800
+  :alt: Tinymovr R5.1 connector diagram
+
+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 +137,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()
 
-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.
+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()
+
+
+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
+==================
+
+.. 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). 
 
-Because of this vast change in resolution (almost 2 orders of magnitude), the gains need to be updated:
+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 +306,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 +322,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/upgrade/connectors_m5_2.png b/docs/upgrade/connectors_m5_2.png
new file mode 100644
index 00000000..c6b577bd
Binary files /dev/null and b/docs/upgrade/connectors_m5_2.png differ
diff --git a/docs/upgrade/upgrade.rst b/docs/upgrade/upgrade.rst
index 09d867ce..53884a88 100644
--- a/docs/upgrade/upgrade.rst
+++ b/docs/upgrade/upgrade.rst
@@ -29,12 +29,19 @@ R5.0, R5.1
   :width: 800
   :alt: Tinymovr R5.0 and R5.1 connectors and pinouts
 
-M5.1, M5.2
-==========
+M5.1
+====
 
 .. image:: connectors_m5.png
   :width: 800
-  :alt: Tinymovr M5 connectors and pinouts
+  :alt: Tinymovr M5.1 connectors and pinouts
+
+M5.2
+====
+
+.. image:: connectors_m5_2.png
+  :width: 800
+  :alt: Tinymovr M5.2 connectors and pinouts
 
 R3.x
 ====
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/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..11f38e6f 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;
 
@@ -134,6 +139,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 +163,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 +182,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..e60a7016 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[96])(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_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_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_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();
@@ -170,6 +182,30 @@ uint8_t avlos_enter_dfu(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cm
     return AVLOS_RET_CALL;
 }
 
+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_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
 {
 if (AVLOS_CMD_READ == cmd) {
@@ -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,23 @@ 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_user_offset();
+        v = motor_get_I_cal();
         *buffer_len = sizeof(v);
         memcpy(buffer, &v, sizeof(v));
         return AVLOS_RET_READ;
@@ -695,35 +761,89 @@ if (AVLOS_CMD_READ == cmd) {
 else if (AVLOS_CMD_WRITE == cmd) {
         float v;
         memcpy(&v, buffer, sizeof(v));
-        motor_set_user_offset(v);
+        motor_set_I_cal(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_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) {
-        int8_t v;
-        v = motor_get_user_direction();
+        float v;
+        v = frame_user_to_position_sensor_get_offset();
         *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_offset(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_user_frame_multiplier(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd)
+{
+if (AVLOS_CMD_READ == cmd) {
+        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) {
+        float v;
+        memcpy(&v, buffer, sizeof(v));
+        frame_user_to_position_sensor_set_multiplier(v);
+        return AVLOS_RET_WRITE;
+    }
+    return AVLOS_RET_NOACTION;
+}
+
+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,107 @@ 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_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 +959,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 +977,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 +989,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 +1001,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 +1013,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 +1031,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 +1049,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..08cf8371 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 = 3066507445;
+extern uint8_t (*avlos_endpoints[96])(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,16 +187,44 @@ 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_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.
+*
+* Endpoint ID: 16
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_scheduler_load(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
 /*
 * avlos_scheduler_errors
 *
 * Any scheduler errors, as a bitmask
 *
+* Endpoint ID: 17
+*
 * @param buffer
 * @param buffer_len
 */
@@ -254,6 +235,8 @@ uint8_t avlos_scheduler_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Com
 *
 * 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,308 @@ 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.
+*
+* Endpoint ID: 52
+*
+* @param buffer
+* @param buffer_len
+*/
+uint8_t avlos_motor_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_motor_I_cal
 *
-* User-defined offset of the motor.
+* The calibration current.
+*
+* Endpoint ID: 53
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_motor_offset(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_direction
+* avlos_motor_errors
 *
-* User-defined direction of the motor.
+* Any motor/calibration errors, as a bitmask
+*
+* Endpoint ID: 54
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_motor_direction(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_calibrated
+* avlos_sensors_user_frame_position_estimate
 *
-* Whether the motor has been calibrated.
+* The filtered position estimate in the user reference frame.
+*
+* Endpoint ID: 55
 *
 * @param buffer
 * @param buffer_len
 */
-uint8_t avlos_motor_calibrated(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_I_cal
+* avlos_sensors_user_frame_velocity_estimate
 *
-* The calibration current.
+* The filtered velocity estimate in the user reference frame.
+*
+* Endpoint ID: 56
 *
 * @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_velocity_estimate(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_motor_errors
+* avlos_sensors_user_frame_offset
 *
-* Any motor/calibration errors, as a bitmask
+* The user defined offset.
+*
+* Endpoint ID: 57
 *
 * @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_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 external sensor type. Either MA7XX, AS5047 or AMT22.
+*
+* 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_calibrated
+*
+* Whether the sensor has been calibrated.
 *
-* 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_calibrated(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
 
 /*
-* avlos_encoder_errors
+* avlos_sensors_setup_external_spi_errors
+*
+* Any sensor errors, as a bitmask
 *
-* Any encoder errors, as a bitmask
+* 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_errors(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command cmd);
+
+/*
+* avlos_sensors_setup_hall_calibrated
+*
+* Whether the sensor has been calibrated.
+*
+* Endpoint ID: 64
+*
+* @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: 65
+*
+* @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: 66
+*
+* @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: 67
+*
+* @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: 68
+*
+* @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: 69
+*
+* @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: 70
+*
+* @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: 71
+*
+* @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: 72
+*
+* @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: 73
+*
+* @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: 74
+*
+* @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: 75
+*
+* @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: 76
+*
 * @param buffer
 * @param buffer_len
 */
@@ -704,6 +943,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: 77
+*
 * @param buffer
 * @param buffer_len
 */
@@ -714,6 +955,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: 78
+*
 * @param buffer
 * @param buffer_len
 */
@@ -724,6 +967,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: 79
+*
 * @param buffer
 * @param buffer_len
 */
@@ -734,6 +979,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: 80
+*
 * @param buffer
 * @param buffer_len
 */
@@ -744,6 +991,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: 81
+*
 * @param buffer
 * @param buffer_len
 */
@@ -752,7 +1001,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: 82
 *
 * @param buffer
 * @param buffer_len
@@ -762,7 +1013,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: 83
 *
 * @param buffer
 * @param buffer_len
@@ -774,6 +1027,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: 84
+*
 * @param buffer
 * @param buffer_len
 */
@@ -784,6 +1039,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: 85
+*
 * @param buffer
 * @param buffer_len
 */
@@ -794,6 +1051,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: 86
+*
 * @param buffer
 * @param buffer_len
 */
@@ -804,6 +1063,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: 87
+*
 * @param buffer
 * @param buffer_len
 */
@@ -814,6 +1075,8 @@ uint8_t avlos_homing_retract_dist(uint8_t * buffer, uint8_t * buffer_len, Avlos_
 *
 * Any homing warnings, as a bitmask
 *
+* Endpoint ID: 88
+*
 * @param buffer
 * @param buffer_len
 */
@@ -824,6 +1087,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: 89
+*
 * @param buffer
 * @param buffer_len
 */
@@ -834,6 +1099,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: 90
+*
 * @param buffer
 * @param buffer_len
 */
@@ -844,6 +1111,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: 91
+*
 * @param buffer
 * @param buffer_len
 */
@@ -854,6 +1123,8 @@ uint8_t avlos_homing_stall_detect_t(uint8_t * buffer, uint8_t * buffer_len, Avlo
 *
 * Perform the homing operation.
 *
+* Endpoint ID: 92
+*
 * @param buffer
 * @param buffer_len
 */
@@ -864,6 +1135,8 @@ uint8_t avlos_homing_home(uint8_t * buffer, uint8_t * buffer_len, Avlos_Command
 *
 * Whether the watchdog is enabled or not.
 *
+* Endpoint ID: 93
+*
 * @param buffer
 * @param buffer_len
 */
@@ -874,6 +1147,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: 94
+*
 * @param buffer
 * @param buffer_len
 */
@@ -884,6 +1159,8 @@ uint8_t avlos_watchdog_triggered(uint8_t * buffer, uint8_t * buffer_len, Avlos_C
 *
 * The watchdog timeout period.
 *
+* Endpoint ID: 95
+*
 * @param buffer
 * @param buffer_len
 */
diff --git a/firmware/src/common.h b/firmware/src/common.h
index d64398d9..157451fd 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
@@ -87,14 +89,6 @@
 
 #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
@@ -129,18 +123,28 @@
 #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))
+
 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..c2fda7f9 100644
--- a/firmware/src/config.h
+++ b/firmware/src/config.h
@@ -38,7 +38,6 @@
 #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
 
 // 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..379674c4 100644
--- a/firmware/src/controller/controller.c
+++ b/firmware/src/controller/controller.c
@@ -16,14 +16,13 @@
 //  * along with this program. If not, see <http://www.gnu.org/licenses/>.
 
 #include "src/system/system.h"
-#include <src/encoder/encoder.h>
+#include <src/sensor/sensor.h>
 #include <src/observer/observer.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"
@@ -35,16 +34,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,10 +56,10 @@ 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};
 
@@ -71,12 +70,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 +88,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,37 +107,31 @@ 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())
-            {
-                (void)((CalibrateADCOffset() && CalibrateResistance() && CalibrateInductance()) && calibrate_hall_sequence());
-            }
+            system_reset_calibration();
+            (void)(ADC_calibrate_offset() && motor_calibrate_resistance() && motor_calibrate_inductance());
+            (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
@@ -146,11 +139,7 @@ void Controller_ControlLoop(void)
                 CLControlStep();
             }
         }
-        else
-        {
-            // pass
-        }
-        WaitForControlLoopInterrupt();
+        wait_for_control_loop_interrupt();
     }
 }
 
@@ -158,22 +147,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 +184,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 +236,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 +244,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 +260,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 +286,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 +301,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();
+            state.pos_setpoint = observer_get_pos_estimate(&position_observer);
             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;
+            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 +367,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 +449,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 +585,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 +597,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..ff968412 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();
     CAN_init();
-    Timer_Init();
+    if (!nvm_load_config())
+    {
+        sensors_init_with_defaults();
+        observers_init_with_defaults();
+    }
+    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..73f1abe6 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,90 @@ 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())
+    {
+        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_get_phase_currents(&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);
+            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;
@@ -180,16 +253,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 +268,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..27a5616a 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
@@ -31,22 +34,34 @@
 #define MAX_PHASE_INDUCTANCE (1e-2f)
 #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)
+#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 +71,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 +89,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..c015dd4e 100644
--- a/firmware/src/nvm/flash_func.c
+++ b/firmware/src/nvm/flash_func.c
@@ -157,60 +157,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..a6f35222 100644
--- a/firmware/src/nvm/nvm.c
+++ b/firmware/src/nvm/nvm.c
@@ -23,24 +23,25 @@
 
 static struct NVMStruct s;
 
+const uint32_t config_size = sizeof(struct NVMStruct);
+
 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();
+	frames_get_config(&(s.frames_config));
 	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();
+	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();
 	strlcpy(s.version, GIT_VERSION, sizeof(s.version));
 	memcpy(data, &s, sizeof(struct NVMStruct));
-	if (STATE_IDLE == controller_get_state())
+	if (CONTROLLER_STATE_IDLE == controller_get_state())
 	{
 		uint8_t *dataBuffer = data;
 		__disable_irq();
@@ -61,12 +62,11 @@ bool nvm_load_config(void)
 	strlcpy(static_version, GIT_VERSION, sizeof(static_version));
 	if (strcmp(s.version, static_version) == 0)
 	{
+		frames_restore_config(&s.frames_config);
 		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);
+		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);
@@ -77,9 +77,12 @@ bool nvm_load_config(void)
 
 void nvm_erase(void)
 {
-	if (STATE_IDLE == controller_get_state())
+	if (CONTROLLER_STATE_IDLE == controller_get_state())
 	{
-		flash_erase_page(SETTINGS_PAGE);
+		for (uint8_t i=0; i<SETTINGS_PAGE_COUNT; i++)
+		{
+			flash_erase_page(SETTINGS_PAGE + i);
+		}
 		system_reset();
 	}
 }
diff --git a/firmware/src/nvm/nvm.h b/firmware/src/nvm/nvm.h
index eee680cb..c04ac7e7 100644
--- a/firmware/src/nvm/nvm.h
+++ b/firmware/src/nvm/nvm.h
@@ -17,34 +17,36 @@
 
 #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];
 };
 
+#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);
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 e5e57758..cfb7c9fd 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,56 +31,61 @@
 
 volatile uint32_t msTicks = 0;
 
-static volatile 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.
 }
 
@@ -91,22 +96,22 @@ void ADC_IRQHandler(void)
 	// 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))
+	if (gate_driver_is_enabled() && scheduler_state.busy)
 	{
-		state.errors |= SCHEDULER_ERRORS_CONTROL_BLOCK_REENTERED;
+		scheduler_state.errors |= SCHEDULER_ERRORS_CONTROL_BLOCK_REENTERED;
 		// We do not change the control state here, to
 		// avoid any concurrency issues
 	}
 	else
 	{
-		state.adc_interrupt = true;
+		scheduler_state.adc_interrupt = true;
 	}
 }
 
 void CAN_IRQHandler(void)
 {
 	pac5xxx_can_int_clear_RI();
-	state.can_interrupt = true;
+	scheduler_state.can_interrupt = true;
 }
 
 void SysTick_Handler(void)
@@ -118,18 +123,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..26e0f987 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;
 } 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_errors(void)
+{
+	return scheduler_state.errors;
+}
+
+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..a62c8a2d
--- /dev/null
+++ b/firmware/src/sensor/amt22.c
@@ -0,0 +1,75 @@
+
+//  * 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)
+{
+    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->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(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct) {
+    AMT22SensorConfig c = {0};
+    c.ssp_port = port;
+    c.ssp_struct = ssp_struct;
+    return amt22_init_with_config(s, &c);
+}
+
+bool amt22_init_with_config(Sensor *s, const AMT22SensorConfig *c) {
+    AMT22Sensor *as = (AMT22Sensor *)s;
+    as->config = *c;
+    ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, 0, 0);
+    delay_us(10000); 
+
+    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..080a7bc6
--- /dev/null
+++ b/firmware/src/sensor/amt22.h
@@ -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/>.
+
+#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;
+} AMT22SensorConfig;
+
+typedef struct
+{
+    Sensor base;
+    AMT22SensorConfig config;
+    uint8_t errors;
+    int32_t angle;
+} AMT22Sensor;
+
+void amt22_make_blank_sensor(Sensor *s);
+bool amt22_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct);
+bool amt22_init_with_config(Sensor *s, const AMT22SensorConfig *c);
+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)
+{
+    ssp_write_one(((const AMT22Sensor *)s)->config.ssp_struct, AMT22_CMD_READ_ANGLE);
+}
+
+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;
+    volatile uint16_t read_value = ssp_read_one(as->config.ssp_struct);
+    // TODO: Add parity check
+    const int32_t angle = read_value & 0x3FFF; 
+    if (check_error)
+    {
+        // Implement error checking specific to the AMT22, if necessary
+    }
+    as->angle = angle;
+}
diff --git a/firmware/src/sensor/as5047.c b/firmware/src/sensor/as5047.c
new file mode 100644
index 00000000..d5309792
--- /dev/null
+++ b/firmware/src/sensor/as5047.c
@@ -0,0 +1,75 @@
+
+//  * 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)
+{
+    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->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(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct) {
+    AS5047PSensorConfig c = {0};
+    c.ssp_port = port;
+    c.ssp_struct = ssp_struct;
+    return as5047p_init_with_config(s, &c);
+}
+
+bool as5047p_init_with_config(Sensor *s, const AS5047PSensorConfig *c) {
+    AS5047PSensor *as = (AS5047PSensor *)s;
+    as->config = *c;
+    ssp_init(as->config.ssp_port, SSP_MS_MASTER, 16, 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..afd86c18
--- /dev/null
+++ b/firmware/src/sensor/as5047.h
@@ -0,0 +1,95 @@
+
+//  * 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 // Adjust based on the AS5047P datasheet
+} AS5047PCommand;
+
+typedef struct
+{
+    SSP_TYPE ssp_port;
+    PAC55XX_SSP_TYPEDEF *ssp_struct;
+    // Add any additional configuration parameters needed for AS5047P
+} 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(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct);
+bool as5047p_init_with_config(Sensor *s, const AS5047PSensorConfig *c);
+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..84308a6d
--- /dev/null
+++ b/firmware/src/sensor/hall.c
@@ -0,0 +1,153 @@
+
+//  * 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->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;
+    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..d1cb3078
--- /dev/null
+++ b/firmware/src/sensor/hall.h
@@ -0,0 +1,80 @@
+
+//  * 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);
+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..0cc4a9f7
--- /dev/null
+++ b/firmware/src/sensor/ma7xx.c
@@ -0,0 +1,76 @@
+
+//  * 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)
+{
+    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->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(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct)
+{
+    MA7xxSensorConfig c = {0};
+    c.ssp_port = port;
+    c.ssp_struct = ssp_struct;
+    return ma7xx_init_with_config(s, &c);
+}
+
+ bool ma7xx_init_with_config(Sensor *s, const MA7xxSensorConfig *c)
+{
+    MA7xxSensor *ms = (MA7xxSensor *)s;
+    ms->config = *c;
+    ssp_init(ms->config.ssp_port, SSP_MS_MASTER, 4, 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..21cf8beb
--- /dev/null
+++ b/firmware/src/sensor/ma7xx.h
@@ -0,0 +1,140 @@
+
+//  * 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;
+} 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(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct);
+bool ma7xx_init_with_config(Sensor *s, const MA7xxSensorConfig *c);
+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..724d043a
--- /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 * 24];
+    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..46e230b8
--- /dev/null
+++ b/firmware/src/sensor/sensor.h
@@ -0,0 +1,147 @@
+
+//  * 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_R3
+#define ONBOARD_SENSOR_SSP_PORT SSPD
+#define ONBOARD_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
+#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 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_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..e71f7c65
--- /dev/null
+++ b/firmware/src/sensor/sensors.c
@@ -0,0 +1,311 @@
+
+//  * 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(s, ONBOARD_SENSOR_SSP_PORT, ONBOARD_SENSOR_SSP_STRUCT);
+            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(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT);
+                    break;
+                case SENSOR_TYPE_AS5047:
+                    return as5047p_init_with_port(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT);
+                    break;
+                case SENSOR_TYPE_AMT22:
+                    return amt22_init_with_port(s, EXTERNAL_SENSOR_SSP_PORT, EXTERNAL_SENSOR_SSP_STRUCT);
+                    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)
+{
+    sensor_type_t internal_type;
+    bool valid = false;
+    switch (type)
+    {
+        case SENSORS_SETUP_EXTERNAL_SPI_TYPE_MA7XX:
+            internal_type = SENSOR_TYPE_MA7XX;
+            valid = true;
+            break;
+        case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AS5047:
+            internal_type = SENSOR_TYPE_AS5047;
+            valid = true;
+            break;
+        case SENSORS_SETUP_EXTERNAL_SPI_TYPE_AMT22:
+            internal_type = SENSOR_TYPE_AMT22;
+            valid = true;
+            break;
+        default:
+            break;
+    }
+    if (valid 
+        && (controller_get_state() == CONTROLLER_STATE_IDLE)
+        && (internal_type != sensors[SENSOR_CONNECTION_EXTERNAL_SPI].sensor.config.type))
+    {
+        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_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)
+    {
+        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))
+        {
+            uint8_t *error_ptr = motor_get_error_ptr();
+            *error_ptr |= MOTOR_ERRORS_INVALID_POLE_PAIRS;
+            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)
+    {
+        frame_set_position_sensor_to_motor(frames.commutation_sensor_to_motor);
+    }
+    else if (sensor_get_type(commutation_sensor_p) != SENSOR_TYPE_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..7846fb8c
--- /dev/null
+++ b/firmware/src/sensor/sensors.h
@@ -0,0 +1,192 @@
+
+//  * 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;
+    }
+}
+
+void sensor_external_spi_set_type_avlos(sensors_setup_external_spi_type_options type);
+
+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..72d0ed11 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,9 +661,9 @@ 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_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;
@@ -698,7 +718,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, uint8_t cph, uint8_t cpol)
 {
     PAC55XX_SSP_TYPEDEF *ssp_ptr;
 
@@ -736,13 +797,13 @@ 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.CPH = cph;                                  // Clock Out Phase
+	ssp_ptr->CON.CPO = cpol;                                 // Clock Out Polarity
     ssp_ptr->CON.DSS = SSP_DATA_SIZE_16;                     // Data Size Select, 16 bit data
     ssp_ptr->CON.SOD = SSP_OUTPUT_NOT_DRIVE;                 // Slave Output Disable
 
@@ -760,6 +821,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..5cc80f40 100644
--- a/firmware/src/ssp/ssp_func.h
+++ b/firmware/src/ssp/ssp_func.h
@@ -16,6 +16,13 @@
 
 #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
 
 // Interrupt Enable Enumeration Type
@@ -31,7 +38,8 @@ typedef enum
 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, 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..94da8a23 100644
--- a/firmware/src/system/system.c
+++ b/firmware/src/system/system.c
@@ -19,9 +19,10 @@
 #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;
@@ -85,6 +86,11 @@ void system_init(void)
     // Ensure ADC GP0 register is zero, to bypass DFU mode on next boot
     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,19 +111,6 @@ 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)
@@ -132,6 +125,15 @@ void system_enter_dfu(void)
     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 +141,9 @@ TM_RAMFUNC float system_get_Vbus(void)
 
 TM_RAMFUNC bool system_get_calibrated(void)
 {
-    return motor_get_calibrated() & encoder_get_calibrated();
+    return (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 +151,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..fb009616
--- /dev/null
+++ b/firmware/src/tm_enums.h
@@ -0,0 +1,145 @@
+/*
+* 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_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
+{
+    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_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_options;
+
+typedef enum
+{
+    MOTOR_TYPE_HIGH_CURRENT = 0, 
+    MOTOR_TYPE_GIMBAL = 1
+} 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_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_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_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..22db35fb 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;
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..ba1f4939
--- /dev/null
+++ b/firmware/src/xfs.c
@@ -0,0 +1,14 @@
+
+
+#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
+};
diff --git a/firmware/src/xfs.h b/firmware/src/xfs.h
new file mode 100644
index 00000000..0c236443
--- /dev/null
+++ b/firmware/src/xfs.h
@@ -0,0 +1,125 @@
+//  * 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/xf1.h>
+
+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;
+} FramesConfig;
+
+extern FramesConfig frames;
+
+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());
+}
+
+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());
+    }
+}
+
+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());
+}
+
+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());
+}
diff --git a/studio/Python/Manifest.in b/studio/Python/Manifest.in
index 832ffb1e..39735dd7 100644
--- a/studio/Python/Manifest.in
+++ b/studio/Python/Manifest.in
@@ -11,4 +11,5 @@ 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.yaml
+include tinymovr/specs/dfu_1_0_x.yaml
+include tinymovr/specs/dfu_1_1_x.yaml
diff --git a/studio/Python/tinymovr/config/config.py b/studio/Python/tinymovr/config/config.py
index 6f72b1a9..edb2eedc 100644
--- a/studio/Python/tinymovr/config/config.py
+++ b/studio/Python/tinymovr/config/config.py
@@ -25,11 +25,33 @@
 from tinymovr.codec import DataType
 from tinymovr.channel import CANChannel
 
-specs = {"hash_uint32": {}}
+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]}
+
+def get_device_spec(hash, logger=None):
+    if logger is None:
+        logger = logging.getLogger("tinymovr")
+
+    try:
+        return specs["hash_uint32"][hash]
+    except KeyError:
+        for hash_alias in hash_aliases[hash]:
+            try:
+                spec = specs["hash_uint32"][hash_alias]
+                logger.debug("Found compatible spec via hash alias {} for device hash {}", hash_alias, hash)
+                return spec
+            except KeyError:
+                pass
+    return None
 
 def init_specs_dict():
     global specs
+    specs = {"hash_uint32": {}}
     for yaml_file in Path(files("tinymovr").joinpath("specs/")).glob("*.yaml"):
         with open(str(yaml_file)) as def_raw:
             spec = yaml.safe_load(def_raw)
@@ -64,11 +86,13 @@ def get_bus_config(suggested_types=None):
         raise can.CanInitializationError("No active interface found") from exc
 
 
-def create_device(node_id):
+def create_device(node_id, logger=None):
     """
     Create a device with the defined ID.
     The hash value will be retrieved from the remote.
     """
+    if logger is None:
+        logger = logging.getLogger("tinymovr")
 
     # Temporarily using a default spec to get the protocol_hash
     # This assumes that `protocol_hash` is standard across different specs
@@ -81,11 +105,11 @@ def create_device(node_id):
 
     # Check for the correct spec using the remote hash
     protocol_hash = node.protocol_hash
-    device_spec = specs["hash_uint32"].get(protocol_hash)
+    device_spec = get_device_spec(protocol_hash)
     chan.compare_hash = protocol_hash & 0xFF
 
     if not device_spec:
-        raise ValueError(f"No device spec found for hash {protocol_hash}.")
+        raise ValueError(f"Device found, but incompatible (no device spec found for hash {protocol_hash}).")
 
     node = deserialize(device_spec)
     node._channel = chan
@@ -93,20 +117,27 @@ def create_device(node_id):
     return node
 
 
-def create_device_with_hash_msg(heartbeat_msg):
+def create_device_with_hash_msg(heartbeat_msg, logger=None):
     """
     Create a device, the heartbeat msg will be used
     to decode the actual hash value and id.
     """
+    if logger is None:
+        logger = logging.getLogger("tinymovr")
+
     node_id = heartbeat_msg.arbitration_id & 0x3F
     chan = CANChannel(node_id)
 
     hash, *_ = chan.serializer.deserialize(heartbeat_msg.data[:4], DataType.UINT32)
-    device_spec = specs["hash_uint32"].get(hash)
+    device_spec = get_device_spec(hash)
     chan.compare_hash = hash & 0xFF
 
     if not device_spec:
-        raise ValueError(f"No device spec found for hash {hash}.")
+        # 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}).")
 
     node = deserialize(device_spec)
 
diff --git a/studio/Python/tinymovr/dfu.py b/studio/Python/tinymovr/dfu.py
index 299bbaf6..a56a744f 100644
--- a/studio/Python/tinymovr/dfu.py
+++ b/studio/Python/tinymovr/dfu.py
@@ -89,6 +89,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 +101,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:
diff --git a/studio/Python/tinymovr/gui/widgets.py b/studio/Python/tinymovr/gui/widgets.py
index 21f5d225..07a5feda 100644
--- a/studio/Python/tinymovr/gui/widgets.py
+++ b/studio/Python/tinymovr/gui/widgets.py
@@ -239,6 +239,9 @@ 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)
diff --git a/studio/Python/tinymovr/gui/window.py b/studio/Python/tinymovr/gui/window.py
index bee0cf0e..1aa40fce 100644
--- a/studio/Python/tinymovr/gui/window.py
+++ b/studio/Python/tinymovr/gui/window.py
@@ -33,6 +33,7 @@
     QHeaderView,
     QLabel,
     QMessageBox,
+    QTreeWidgetItem,
 )
 from PySide6.QtGui import QAction
 import pyqtgraph as pg
@@ -58,6 +59,7 @@
 
 class MainWindow(QMainWindow):
     TreeItemCheckedSignal = Signal(dict)
+    updateVisibleAttrsSignal = Signal(set)
 
     def __init__(self, app, arguments, logger):
         super(MainWindow, self).__init__()
@@ -99,6 +101,8 @@ def __init__(self, app, arguments, logger):
         # 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()
@@ -158,15 +162,21 @@ 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)
 
     @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 +218,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):
         """
@@ -326,3 +349,28 @@ def show_about_box(self):
                 app_str
             ),
         )
+
+    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)
diff --git a/studio/Python/tinymovr/gui/worker.py b/studio/Python/tinymovr/gui/worker.py
index 3932d9a6..4d658bdf 100644
--- a/studio/Python/tinymovr/gui/worker.py
+++ b/studio/Python/tinymovr/gui/worker.py
@@ -32,7 +32,7 @@ class Worker(QObject):
     regenSignal = QtCore.Signal(dict)
     handleErrorSignal = QtCore.Signal(object)
 
-    def __init__(self, busparams, logger):
+    def __init__(self, busparams, logger, refresh_period=0.3):
         super().__init__()
         self.logger = logger
         self.mutx = QtCore.QMutex()
@@ -43,10 +43,14 @@ def __init__(self, busparams, 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()
@@ -78,18 +82,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 +102,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()
diff --git a/studio/Python/tinymovr/specs/tinymovr_1_7_x.yaml b/studio/Python/tinymovr/specs/tinymovr_1_7_x.yaml
index 30578409..ab02618a 100644
--- a/studio/Python/tinymovr/specs/tinymovr_1_7_x.yaml
+++ b/studio/Python/tinymovr/specs/tinymovr_1_7_x.yaml
@@ -39,7 +39,7 @@ remote_attributes:
     dtype: float
     unit: degC
     meta: {dynamic: True}
-    getter_name: adc_get_mcu_temp
+    getter_name: ADC_get_mcu_temp
     summary: The internal temperature of the PAC55xx MCU.
   - name: calibrated
     dtype: bool
@@ -47,10 +47,15 @@ remote_attributes:
     getter_name: system_get_calibrated
     summary: Whether the system has been calibrated.
   - name: errors
-    flags: [UNDERVOLTAGE, DRIVER_FAULT, CHARGE_PUMP_FAULT_STAT, CHARGE_PUMP_FAULT, DRV10_DISABLE, DRV32_DISABLE, DRV54_DISABLE]
+    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
@@ -74,8 +79,17 @@ remote_attributes:
     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: errors
         flags: [CONTROL_BLOCK_REENTERED]
         meta: {dynamic: True}
@@ -84,13 +98,13 @@ remote_attributes:
   - name: controller
     remote_attributes:
       - name: state
-        dtype: uint8
+        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
-        dtype: uint8
+        options: [CURRENT, VELOCITY, POSITION, TRAJECTORY, HOMING]
         meta: {dynamic: True}
         getter_name: controller_get_mode
         setter_name: controller_set_mode
@@ -113,7 +127,7 @@ remote_attributes:
             meta: {jog_step: 100}
             getter_name: controller_get_pos_setpoint_user_frame
             setter_name: controller_set_pos_setpoint_user_frame
-            summary: The position setpoint.
+            summary: The position setpoint in the user reference frame.
           - name: p_gain
             dtype: float
             meta: {export: True}
@@ -128,7 +142,7 @@ remote_attributes:
             meta: {jog_step: 200}
             getter_name: controller_get_vel_setpoint_user_frame
             setter_name: controller_set_vel_setpoint_user_frame
-            summary: The velocity setpoint.
+            summary: The velocity setpoint in the user reference frame.
           - name: limit
             dtype: float
             unit: tick/sec
@@ -145,15 +159,15 @@ remote_attributes:
           - name: i_gain
             dtype: float
             meta: {export: True}
-            getter_name: controller_get_vel_integrator_gain
-            setter_name: controller_set_vel_integrator_gain
+            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_integrator_deadband
-            setter_name: controller_set_vel_integrator_deadband
+            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
@@ -170,13 +184,13 @@ remote_attributes:
             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.
+            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.
+            summary: The Id setpoint in the user reference frame.
           - name: Iq_limit
             dtype: float
             unit: ampere
@@ -188,7 +202,7 @@ remote_attributes:
             unit: ampere
             meta: {dynamic: True}
             getter_name: controller_get_Iq_estimate_user_frame
-            summary: The Iq estimate.
+            summary: The Iq estimate in the user reference frame.
           - name: bandwidth
             dtype: float
             unit: Hz
@@ -247,8 +261,8 @@ remote_attributes:
         dtype: void
         arguments: []
       - name: set_pos_vel_setpoints
-        summary: Set the position and velocity setpoints in one go, and retrieve the position estimate
-        caller_name: controller_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
@@ -274,6 +288,11 @@ remote_attributes:
         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
@@ -302,18 +321,6 @@ remote_attributes:
         getter_name: motor_get_is_gimbal
         setter_name: motor_set_is_gimbal
         summary: The type of the motor. Either high current or gimbal.
-      - name: offset
-        dtype: float
-        meta: {export: True}
-        getter_name: motor_get_user_offset
-        setter_name: motor_set_user_offset
-        summary: User-defined offset of the motor.
-      - name: direction
-        dtype: int8
-        meta: {export: True}
-        getter_name: motor_get_user_direction
-        setter_name: motor_set_user_direction
-        summary: User-defined direction of the motor.
       - name: calibrated
         dtype: bool
         meta: {dynamic: True}
@@ -331,60 +338,143 @@ remote_attributes:
         meta: {dynamic: True}
         getter_name: motor_get_errors
         summary: Any motor/calibration errors, as a bitmask
-      # - name: phase_currents
-      #   remote_attributes:
-      #     - name: U
-      #       dtype: float
-      #       unit: ampere
-      #       getter_name: motor_get_IU
-      #       summary: Measured current in phase U.
-      #     - name: V
-      #       dtype: float
-      #       unit: ampere
-      #       getter_name: motor_get_IV
-      #       summary: Measured current in phase V.
-      #     - name: W
-      #       dtype: float
-      #       unit: ampere
-      #       getter_name: motor_get_IW
-      #       summary: Measured current in phase W.
-  - name: encoder
+  - name: sensors
     remote_attributes:
-      - name: position_estimate
-        dtype: float
-        unit: ticks
-        meta: {dynamic: True}
-        getter_name: observer_get_pos_estimate_user_frame
-        summary: The filtered encoder position estimate.
-      - name: velocity_estimate
-        dtype: float
-        unit: ticks/second
-        meta: {dynamic: True}
-        getter_name: observer_get_vel_estimate_user_frame
-        summary: The filtered encoder velocity estimate.
-      - name: type
-        options: [INTERNAL, HALL]
-        meta: {export: True}
-        getter_name: encoder_get_type
-        setter_name: encoder_set_type
-        summary: The encoder type. Either INTERNAL or HALL.
-      - name: bandwidth
-        dtype: float
-        unit: Hz
-        meta: {export: True}
-        getter_name: observer_get_bw
-        setter_name: observer_set_bw
-        summary: The encoder observer bandwidth.
-      - name: calibrated
-        dtype: bool
-        meta: {dynamic: True}
-        getter_name: encoder_get_calibrated
-        summary: Whether the encoder has been calibrated.
-      - name: errors
-        flags: [CALIBRATION_FAILED, READING_UNSTABLE]
-        meta: {dynamic: True}
-        getter_name: encoder_get_errors
-        summary: Any encoder errors, as a bitmask
+      - 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 external sensor type. Either MA7XX, AS5047 or AMT22.
+              - 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
@@ -430,7 +520,7 @@ remote_attributes:
         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 respecting velocity and acceleration limits.
+        summary: Move to target position in the user reference frame respecting velocity and acceleration limits.
         caller_name: planner_move_to_vlimit
         dtype: void
         arguments:
@@ -438,7 +528,7 @@ remote_attributes:
             dtype: float
             unit: tick
       - name: move_to_tlimit
-        summary: Move to target position respecting time limits for each sector.
+        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:


Test patch:
diff --git a/studio/Python/tests/test_as5047.py b/studio/Python/tests/test_as5047.py
new file mode 100644
index 00000000..8fd21e56
--- /dev/null
+++ b/studio/Python/tests/test_as5047.py
@@ -0,0 +1,115 @@
+"""
+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.50
+
+
+class TestAS5047(TMTestCase):
+
+    @classmethod
+    def configure_sensors(cls):
+        cls.tm.sensors.setup.external_spi.type = cls.tm.sensors.setup.external_spi.type.AS5047
+        cls.tm.sensors.select.position_sensor.connection = cls.tm.sensors.select.position_sensor.connection.EXTERNAL_SPI
+        # Commutation sensor remains internal
+        time.sleep(0.2)
+
+    @classmethod
+    def setUpClass(cls):
+        super(TestAS5047, cls).setUpClass()
+        
+        cls.configure_sensors()
+        
+        # We look at the position estimate, which is highly unlikely to be exactly zero.
+        if cls.tm.sensors.select.position_sensor.raw_angle == 16383:
+            raise unittest.SkipTest("AS5047 sensor not present. Skipping test.")
+
+        cls.reset_and_wait()
+
+    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.
+        """
+        pass
+        self.check_state(0)
+        self.tm.erase_config()
+        time.sleep(0.2)
+
+        self.configure_sensors()
+
+        self.tm.motor.I_cal = 0.8
+
+        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)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/studio/Python/tests/test_base_function.py b/studio/Python/tests/test_base_function.py
index a56c6837..49848f1f 100644
--- a/studio/Python/tests/test_base_function.py
+++ b/studio/Python/tests/test_base_function.py
@@ -32,7 +32,7 @@ def test_position_control(self):
             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
+                i * 3000 * ticks, self.tm.sensors.user_frame.position_estimate, delta=1000 * ticks
             )
 
 
diff --git a/studio/Python/tests/test_board.py b/studio/Python/tests/test_board.py
index 6316d2ca..f460eabd 100644
--- a/studio/Python/tests/test_board.py
+++ b/studio/Python/tests/test_board.py
@@ -28,17 +28,17 @@
 A = ureg.ampere
 ticks = ureg.ticks
 s = ureg.second
-tsleep = 0.19
+tsleep = 0.20
 
 
 class TestBoard(TMTestCase):
-    def test_a_encoder(self):
+    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)
@@ -92,7 +92,7 @@ def test_d_position_control(self):
             self.tm.controller.position.setpoint = i * 1000 * ticks
             time.sleep(tsleep)
             self.assertAlmostEqual(
-                i * 1000 * ticks, self.tm.encoder.position_estimate, delta=1000 * ticks
+                i * 1000 * ticks, self.tm.sensors.user_frame.position_estimate, delta=1000 * ticks
             )
 
     def test_e_velocity_control(self):
@@ -112,25 +112,25 @@ def test_e_velocity_control(self):
             target = i * 20000 * ticks / s
             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
             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
             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
             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)
@@ -144,14 +144,14 @@ def test_f_random_pos_control(self):
         self.tm.controller.position_mode()
         self.check_state(2)
 
-        for _ in range(10):
+        for _ in range(15):
             new_pos = random.uniform(-24000, 24000)
             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,
             )
 
     def test_g_limits(self):
@@ -176,27 +176,27 @@ 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)
+    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)
 
     def test_i_states(self):
         """
@@ -229,6 +229,8 @@ 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)
@@ -242,35 +244,34 @@ def test_j_gimbal_mode(self):
 
         for i in range(10):
             self.tm.controller.position.setpoint = i * 1000 * ticks
-            time.sleep(0.2)
+            time.sleep(0.3)
             self.assertAlmostEqual(
-                i * 1000 * ticks, self.tm.encoder.position_estimate, delta=1000 * ticks
+                i * 1000 * ticks, self.tm.sensors.user_frame.position_estimate, delta=1000 * ticks
             )
-            time.sleep(0.4)
 
-    def test_k_rotor_offset_and_direction(self):
+    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):
@@ -317,7 +318,7 @@ 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)
@@ -340,7 +341,7 @@ 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()
@@ -355,7 +356,7 @@ 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)
 
@@ -363,6 +364,7 @@ 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
index f9f8dd9c..b5be5e1f 100644
--- a/studio/Python/tests/test_board_config.py
+++ b/studio/Python/tests/test_board_config.py
@@ -27,6 +27,7 @@
 A = ureg.ampere
 tick = ureg.ticks
 s = ureg.second
+tsleep = 0.20
 
 
 class TestBoardConfig(TMTestCase):
@@ -111,6 +112,56 @@ def test_c_parameter_persistence(self):
         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)
+
 
 if __name__ == "__main__":
     unittest.main()
diff --git a/studio/Python/tests/test_hall.py b/studio/Python/tests/test_hall.py
new file mode 100644
index 00000000..3ce7098a
--- /dev/null
+++ b/studio/Python/tests/test_hall.py
@@ -0,0 +1,104 @@
+"""
+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 = 1.00
+
+
+def set_pole_pairs(tm):
+    tm.motor.pole_pairs = 15
+
+class TestHall(TMTestCase):
+
+    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.
+        """
+        pass
+        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_rates.py b/studio/Python/tests/test_rates.py
index 3634649c..47dcbb2b 100644
--- a/studio/Python/tests/test_rates.py
+++ b/studio/Python/tests/test_rates.py
@@ -33,7 +33,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/tm_test_case.py b/studio/Python/tests/tm_test_case.py
index 8a6d6c94..ecdd0146 100644
--- a/studio/Python/tests/tm_test_case.py
+++ b/studio/Python/tests/tm_test_case.py
@@ -49,10 +49,12 @@ 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, force=False, precheck_callback=None, *args, **kwargs):
         if True == force or not self.tm.calibrated:
             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):