diff --git a/firmware/src/observer/observer.c b/firmware/src/observer/observer.c
index c79c46a0..d51af118 100644
--- a/firmware/src/observer/observer.c
+++ b/firmware/src/observer/observer.c
@@ -21,12 +21,6 @@
 #include <src/system/system.h>
 #include <src/observer/observer.h>
 
-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};
@@ -51,8 +45,8 @@ void observer_set_bandwidth(Observer *o, float bw)
     if (bw > 0.0f)
     {
         o->config.track_bw = bw;
-		o->config.kp = 2.0f * config.track_bw;
-    	o->config.ki = 0.25f * (config.kp * config.kp);
+		o->config.kp = 2.0f * o->config.track_bw;
+    	o->config.ki = 0.25f * (o->config.kp * o->config.kp);
     }
 }
 
diff --git a/firmware/src/observer/observer.h b/firmware/src/observer/observer.h
index 50440ff9..400e4405 100644
--- a/firmware/src/observer/observer.h
+++ b/firmware/src/observer/observer.h
@@ -66,7 +66,7 @@ static inline void observer_update(Observer *o)
 	{
 		const float sensor_ticks = sensor_get_ticks(o->sensor_ptr);
 		const float sensor_half_ticks = sensor_ticks * 0.5f;
-		const int16_t angle_meas = sensor_get_angle_rectified(o->sensor_ptr);
+		const int32_t angle_meas = sensor_get_angle_rectified(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_half_ticks)
diff --git a/firmware/src/sensor/as5047.h b/firmware/src/sensor/as5047.h
index c18910cb..0b94686f 100644
--- a/firmware/src/sensor/as5047.h
+++ b/firmware/src/sensor/as5047.h
@@ -40,7 +40,7 @@ typedef struct
     Sensor base;
     AS5047PSensorConfig config;
     uint8_t errors;
-    uint16_t angle;
+    int32_t angle;
 } AS5047PSensor;
 
 bool as5047p_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct);
@@ -64,7 +64,7 @@ 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 uint16_t as5047p_get_raw_angle(const Sensor *s)
+static inline int32_t as5047p_get_raw_angle(const Sensor *s)
 {
     return ((const AS5047PSensor *)s)->angle;
 }
@@ -72,10 +72,10 @@ static inline uint16_t as5047p_get_raw_angle(const Sensor *s)
 static inline void as5047p_update(Sensor *s, bool check_error)
 {
     AS5047PSensor *as = (AS5047PSensor *)s;
-    const int16_t angle = ssp_read_one(as->config.ssp_struct) & 0x3FFF; // Mask to get the angle value
+    const int32_t angle = ssp_read_one(as->config.ssp_struct) & 0x3FFF; // Mask to get the angle value
     if (check_error)
     {
-    	const int16_t delta = as->angle - angle;
+    	const int32_t delta = as->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)) )
diff --git a/firmware/src/sensor/hall.h b/firmware/src/sensor/hall.h
index 10aaa552..cccfdd9c 100644
--- a/firmware/src/sensor/hall.h
+++ b/firmware/src/sensor/hall.h
@@ -32,7 +32,7 @@ typedef struct
     Sensor base;
     HallSensorConfig config;
     uint8_t errors;
-	uint16_t angle;
+	int32_t angle;
     uint8_t sector;
     uint8_t hw_defaults[3];
 } HallSensor;
@@ -48,7 +48,7 @@ static inline uint8_t hall_get_errors(const Sensor *s)
     return ((HallSensor *)s)->errors;
 }
 
-static inline int16_t hall_get_angle(const Sensor *s)
+static inline int32_t hall_get_angle(const Sensor *s)
 {
     return ((HallSensor *)s)->angle;
 }
diff --git a/firmware/src/sensor/ma7xx.h b/firmware/src/sensor/ma7xx.h
index 8ffbe72a..72d213ac 100644
--- a/firmware/src/sensor/ma7xx.h
+++ b/firmware/src/sensor/ma7xx.h
@@ -41,7 +41,7 @@ typedef struct
     Sensor base;
     MA7xxSensorConfig config;
     uint8_t errors;
-	int16_t angle;
+	int32_t angle;
 } MA7xxSensor;
 
 bool ma7xx_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *ssp_struct);
@@ -65,7 +65,7 @@ static inline void ma7xx_send_angle_cmd(const Sensor *s)
 	ssp_write_one(((const MA7xxSensor *)s)->config.ssp_struct, MA_CMD_ANGLE);
 }
 
-static inline int16_t ma7xx_get_raw_angle(const Sensor *s)
+static inline int32_t ma7xx_get_raw_angle(const Sensor *s)
 {
     return ((const MA7xxSensor *)s)->angle;
 }
@@ -73,11 +73,11 @@ static inline int16_t ma7xx_get_raw_angle(const Sensor *s)
 static inline void ma7xx_update(Sensor *s, bool check_error)
 {
     MA7xxSensor *ms = (MA7xxSensor *)s;
-    const int16_t angle = ssp_read_one(ms->config.ssp_struct);
+    const int32_t angle = ssp_read_one(ms->config.ssp_struct);
 
     if (check_error)
     {
-    	const int16_t delta = ms->angle - angle;
+    	const int32_t delta = ms->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)) )
diff --git a/firmware/src/sensor/sensor.h b/firmware/src/sensor/sensor.h
index 7c88dd8c..181364a4 100644
--- a/firmware/src/sensor/sensor.h
+++ b/firmware/src/sensor/sensor.h
@@ -43,7 +43,7 @@ typedef struct Observer Observer;
 
 typedef bool (*sensor_is_calibrated_func_t)(const Sensor *);
 typedef bool (*sensor_calibrate_func_t)(Sensor *, Observer *);
-typedef int16_t (*sensor_get_raw_angle_func_t)(const Sensor *);
+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 *);
@@ -93,13 +93,13 @@ void sensor_reset(Sensor *s);
 bool sensor_calibrate_offset_and_rectification(Sensor *s, Observer *o);
 bool sensor_calibrate_direction_and_pole_pair_count(Sensor *s, Observer *o);
 
-static inline int16_t sensor_get_angle_rectified(const Sensor *s)
+static inline int32_t sensor_get_angle_rectified(const Sensor *s)
 {
     const uint8_t offset_bits = (ENCODER_BITS - ECN_BITS);
-    const int16_t angle = s->get_raw_angle_func(s);
-    const int16_t off_1 = s->config.rec_table[angle>>offset_bits];
-	const int16_t off_2 = s->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);
+    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;
 }
 
@@ -117,7 +117,7 @@ static inline void sensor_invalidate(Sensor *s)
     s->current = false;
 }
 
-static inline uint16_t sensor_get_ticks(Sensor *s)
+static inline uint32_t sensor_get_ticks(Sensor *s)
 {
     return s->ticks;
 }