diff --git a/stm-firmware/digio.c b/stm-firmware/digio.c
index 7ec5b9e..4c8f86b 100644
--- a/stm-firmware/digio.c
+++ b/stm-firmware/digio.c
@@ -27,6 +27,7 @@
 static const uint8_t digio_pins[] = {DIGIO_PINS};
 static const uint8_t digio_default_io[] = {DIGIO_INOUT_DEFAULT};
 static const uint8_t digio_default_altfunc[] = {DIGIO_ALTFUNC_DEFAULT};
+static uint16_t loudspeaker_val;
 
 static void digio_setup_pin_int(uint8_t bit_no, uint8_t in_out, uint8_t alt_func)
 {
@@ -118,6 +119,19 @@ int led_get(uint8_t num)
 	return ((LED_PORT->ODR & (1<<led_pins[num])) ? 1 : 0);
 }
 
+
+static void loudspeaker_freq_timer_init(void)
+{
+#if LOUDSPEAKER_MULTIFREQ
+	rcc_manager_enable_clock(&RCC->APB1ENR, BITMASK_TO_BITNO(RCC_APB1ENR_TIM7EN));
+	TIM7->CR1 = 0UL;
+	TIM7->CR2 = 0UL;
+	TIM7->PSC = 1000;
+	TIM7->DIER = TIM_DIER_UIE;
+	NVIC_EnableIRQ(TIM7_IRQn);
+#endif
+}
+
 void loudspeaker_setup()
 {
 	rcc_manager_enable_clock(&RCC->AHB1ENR, BITMASK_TO_BITNO(LOUDSPEAKER_RCC_MASK));
@@ -125,16 +139,56 @@ void loudspeaker_setup()
 	LOUDSPEAKER_PORT->MODER &= MODER_DELETE(LOUDSPEAKER_PIN);
 	LOUDSPEAKER_PORT->MODER |= OUTPUT(LOUDSPEAKER_PIN);
 
-	loudspeaker_set(0);
+	loudspeaker_freq_timer_init();
+	loudspeaker_set(0U);
 }
-void loudspeaker_set(int val)
+
+static void loudspeaker_start_beep(uint16_t val)
 {
-	if (val)
-		LOUDSPEAKER_PORT->ODR |= (1<<LOUDSPEAKER_PIN);
-	else
-		LOUDSPEAKER_PORT->ODR &= ~(1<<LOUDSPEAKER_PIN);
+#if LOUDSPEAKER_MULTIFREQ
+	TIM7->ARR = val;
+	TIM7->CNT = 0UL;
+	TIM7->CR1 |= TIM_CR1_CEN;
+#else
+	(void)val;
+	LOUDSPEAKER_PORT->ODR |= (1<<LOUDSPEAKER_PIN);
+#endif
 }
-int loudspeaker_get()
+
+static void loudspeaker_stop_beep(void)
 {
-	return ((LOUDSPEAKER_PORT->ODR & (1<<LOUDSPEAKER_PIN)) ? 1 : 0);
+#if LOUDSPEAKER_MULTIFREQ
+	TIM7->CR1 &= ~TIM_CR1_CEN;
+	__DSB();
+	TIM7->SR = 0UL;
+	__DSB();
+	LOUDSPEAKER_PORT->ODR &= ~(1<<LOUDSPEAKER_PIN);
+#else
+	LOUDSPEAKER_PORT->ODR &= ~(1<<LOUDSPEAKER_PIN);
+#endif
 }
+
+void loudspeaker_set(uint16_t val)
+{
+	loudspeaker_val = val;
+
+	if (!val) {
+		loudspeaker_stop_beep();
+	} else {
+		loudspeaker_start_beep(val);
+	}
+}
+
+uint16_t loudspeaker_get()
+{
+	return loudspeaker_val;
+}
+
+#if LOUDSPEAKER_MULTIFREQ
+void TIM7_IRQHandler(void)
+{
+	TIM7->SR = 0UL;
+	__DSB();
+	LOUDSPEAKER_PORT->ODR ^= (1<<LOUDSPEAKER_PIN);
+}
+#endif
diff --git a/stm-firmware/include/reflow-controller/digio.h b/stm-firmware/include/reflow-controller/digio.h
index 8f665da..3fb7671 100644
--- a/stm-firmware/include/reflow-controller/digio.h
+++ b/stm-firmware/include/reflow-controller/digio.h
@@ -58,10 +58,11 @@ int led_get(uint8_t num);
 #define LOUDSPEAKER_PORT GPIOB
 #define LOUDSPEAKER_RCC_MASK RCC_AHB1ENR_GPIOBEN
 #define LOUDSPEAKER_PIN 1
+#define LOUDSPEAKER_MULTIFREQ 1
 
 void loudspeaker_setup();
-void loudspeaker_set(int val);
-int loudspeaker_get();
+void loudspeaker_set(uint16_t val);
+uint16_t loudspeaker_get();
 
 
 #endif /* __DIGIO_H__ */
diff --git a/stm-firmware/include/reflow-controller/periph-config/safety-adc-hwcfg.h b/stm-firmware/include/reflow-controller/periph-config/safety-adc-hwcfg.h
index bd9e6c7..26a23c8 100644
--- a/stm-firmware/include/reflow-controller/periph-config/safety-adc-hwcfg.h
+++ b/stm-firmware/include/reflow-controller/periph-config/safety-adc-hwcfg.h
@@ -28,4 +28,11 @@
 #define TEMP_CHANNEL_NUM (16)
 #define INT_REF_CHANNEL_NUM (17)
 
+#define SAFETY_ADC_INT_REF_MV 1210.0f
+
+#define SAFETY_ADC_TEMP_NOM 25.0f
+#define SAFETY_ADC_TEMP_NOM_MV 760.0f
+#define SAFETY_ADC_TEMP_MV_SLOPE 2.5f
+
+
 #endif /* __SAFETY_ADC_HWCFG_H__ */
diff --git a/stm-firmware/include/reflow-controller/pid-controller.h b/stm-firmware/include/reflow-controller/pid-controller.h
index 1e64741..c2914cc 100644
--- a/stm-firmware/include/reflow-controller/pid-controller.h
+++ b/stm-firmware/include/reflow-controller/pid-controller.h
@@ -25,15 +25,19 @@ struct pid_controller {
 	float k_deriv;
 	float k_int;
 	float k_p;
+	float k_int_t;
+	float k_deriv_t;
 	float output_sat_max;
 	float output_sat_min;
 	float integral_max;
+	float sample_period;
 	volatile float control_output;
 	volatile float last_in;
 	volatile float integral;
+	volatile float derivate;
 };
 
-void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min, float output_sat_max, float integral_max);
+void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min, float output_sat_max, float integral_max, float sample_period);
 
 void pid_zero(struct pid_controller *pid);
 
diff --git a/stm-firmware/include/reflow-controller/systick.h b/stm-firmware/include/reflow-controller/systick.h
index 5060096..fa1568c 100644
--- a/stm-firmware/include/reflow-controller/systick.h
+++ b/stm-firmware/include/reflow-controller/systick.h
@@ -45,7 +45,7 @@ extern volatile uint32_t wait_tick_ms;
  * @brief Systemclock in milliseconds.
  *
  * This value must not be reset during the whole runtime.
- *
+ * @warning In order to use this, you must assure that the read access is atomic.
  */
 extern volatile uint64_t global_tick_ms;
 
diff --git a/stm-firmware/main.c b/stm-firmware/main.c
index 9965338..623558e 100644
--- a/stm-firmware/main.c
+++ b/stm-firmware/main.c
@@ -221,7 +221,7 @@ int main()
 	shell_handle = shell_init(write_shell_callback);
 	shell_print_motd(shell_handle);
 
-	pid_init(&pid, 0.1, 0.1, 4.0, 0.0, 100.0, 40.0);
+	pid_init(&pid, 0.1, 0.1, 4.0, 0.0, 100.0, 40.0, 0.25);
 	pid_zero(&pid);
 
 	while (1) {
@@ -259,7 +259,7 @@ int main()
 			oven_driver_set_power(0U);
 
 			/* Activate loundspeaker permanently */
-			loudspeaker_set(1);
+			loudspeaker_set(100);
 		} else if (pid_controller_active) {
 			/* In case temperature measurement is okay and controlelr is working, write output power */
 			oven_driver_set_power(pid_controller_output < 0 ? 0U : (uint8_t)pid_controller_output);
diff --git a/stm-firmware/pid-controller.c b/stm-firmware/pid-controller.c
index ef97235..c73d591 100644
--- a/stm-firmware/pid-controller.c
+++ b/stm-firmware/pid-controller.c
@@ -20,18 +20,22 @@
 
 #include <reflow-controller/pid-controller.h>
 
-void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min, float output_sat_max, float integral_max)
+void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min, float output_sat_max, float integral_max, float sample_period)
 {
 	if (!pid)
 		return;
 
+	pid->sample_period = sample_period;
 	pid->k_p = k_p;
 	pid->k_int = k_int;
 	pid->k_deriv = k_deriv;
+	pid->k_int_t = pid->k_int * pid->sample_period / 2.0f;
+	pid->k_deriv_t = pid->k_deriv * 2.0f / pid->sample_period;
 	pid->output_sat_max = output_sat_max;
 	pid->output_sat_min = output_sat_min;
-	pid->control_output = 0;
 	pid->integral_max = integral_max;
+
+	pid_zero(pid);
 }
 
 void pid_zero(struct pid_controller *pid)
@@ -43,12 +47,13 @@ void pid_zero(struct pid_controller *pid)
 
 static void calculate_integral(struct pid_controller *pid, float deviation)
 {
-	pid->integral += deviation * pid->k_int;
+	pid->integral = pid->integral + pid->k_int_t * (deviation + pid->last_in);
 
+	/* Saturate integral term to spoecified maximum */
 	if (pid->integral > pid->integral_max) {
 		pid->integral = pid->integral_max;
-	} else if (pid->integral < -pid->integral_max) {
-		pid->integral = -pid->integral_max;
+	} else if (pid->integral < -pid->integral_max){
+		pid->integral = - pid->integral_max;
 	}
 }
 
@@ -66,17 +71,20 @@ float pid_sample(struct pid_controller *pid, float deviation)
 		calculate_integral(pid, deviation);
 	}
 
+	/* Calculate derivative part */
+	pid->derivate = pid->k_deriv_t * (deviation - pid->last_in) - pid->derivate;
+
+	output += pid->derivate;
 	output += pid->integral;
-	output -= (deviation - pid->last_in) * pid->k_deriv;
 
-	pid->last_in = deviation;
-
-	if (output > pid->output_sat_max)
-		output = pid->output_sat_max;
+	/* Saturate output */
 	if (output < pid->output_sat_min)
 		output = pid->output_sat_min;
+	else if (output > pid->output_sat_max)
+		output = pid->output_sat_max;
 
 	pid->control_output = output;
+	pid->last_in = deviation;
 
 	return output;
 }
diff --git a/stm-firmware/safety-adc.c b/stm-firmware/safety-adc.c
index fb91b5f..945fc93 100644
--- a/stm-firmware/safety-adc.c
+++ b/stm-firmware/safety-adc.c
@@ -53,11 +53,12 @@ enum safety_adc_check_result safety_adc_check_results(uint16_t vref_result, uint
 
 
 	if (vref_calculated) {
-		*vref_calculated = (1210.0 * 4095) / (float)vref_result;
+		*vref_calculated = (SAFETY_ADC_INT_REF_MV * 4095.0f) / (float)vref_result;
 	}
 
 	if (temp_calculated) {
-		*temp_calculated = (((float)temp_result / 4095.0f * 2500.0f - 760.0f) / 2.5f) + 25.0f;
+		*temp_calculated = (((float)temp_result / 4095.0f * 2500.0f -
+				     SAFETY_ADC_TEMP_NOM_MV) / SAFETY_ADC_TEMP_MV_SLOPE) + SAFETY_ADC_TEMP_NOM;
 	}
 
 	return res;
diff --git a/stm-firmware/systick.c b/stm-firmware/systick.c
index b96c245..c998f6e 100644
--- a/stm-firmware/systick.c
+++ b/stm-firmware/systick.c
@@ -43,7 +43,13 @@ void systick_wait_ms(uint32_t ms)
 
 uint64_t systick_get_global_tick()
 {
-	return global_tick_ms;
+	uint64_t temp;
+
+	__disable_irq();
+	temp = global_tick_ms;
+	__enable_irq();
+
+	return temp;
 }
 
 bool __attribute__((optimize("O3"))) systick_ticks_have_passed(uint64_t start_timestamp, uint64_t ticks)