diff --git a/firmware/Makefile b/firmware/Makefile
index b25a5e6..1a9f769 100644
--- a/firmware/Makefile
+++ b/firmware/Makefile
@@ -10,6 +10,8 @@ OBJDUMP=avr-objdump
 OBJCOPY=avr-objcopy
 SIZE=avr-size
 CFLAGS=-g -Wall -Werror -O3 -std=c99
+# for this project we need float capable printf
+LDFLAGS=-lm -Wl,-u,vfprintf -lprintf_flt
 # the target (arduino nano)
 DEVICE=atmega328p
 F_CPU=16000000UL
@@ -66,7 +68,7 @@ compile: $(TARGET).elf
 
 # link compiled files
 %.elf: $(OBJ)
-	$(CC) $(CFLAGS) -I. -mmcu=$(DEVICE) -DF_CPU=$(F_CPU) -Wl,-Map=$(TARGET).map,--cref -o $@ $(OBJ)
+	$(CC) $(LDFLAGS) -I. -mmcu=$(DEVICE) -DF_CPU=$(F_CPU) -Wl,-Map=$(TARGET).map,--cref -o $@ $(OBJ)
 
 # create extended listing for additional information
 %.lst: %.elf
diff --git a/firmware/main.c b/firmware/main.c
index b36759b..e31842a 100644
--- a/firmware/main.c
+++ b/firmware/main.c
@@ -25,16 +25,24 @@
 #include <avr/pgmspace.h> // Program Space Utilities
 #include <avr/sleep.h> // Power Management and Sleep Modes
 
-
 #include "uart.h" // basic UART functions
 #include "main.h" // main definitions
 
+/* contants */
+// max measurement jitter for high and low value
+#define JITTER_HIGH 8
+#define JITTER_LOW 3
+// how many value have to be stable before showing output
+#define MEASUREMENT_STABLE 3
+// weight coefficient (timer 1 ticks/kg)
+// proportionally decreasing for high, increasing for low
+#define COEF_HIGH 41.78
+#define COEF_LOW 39.87
+
 /* variables */
 volatile uint8_t state_portD; // the state of port D
 volatile bool scale_on = false; // is the scale on (based on the SCALE_ON PIN)
-#ifdef DEBUG
 volatile bool scale_old = false; // the previous state (to detect changes)
-#endif
 volatile bool pwm_high = false; // is the PWM for the weight measurement high
 volatile bool measurement_flag = false; // is a PWM  measurement value ready
 volatile uint16_t measurement_value = 0; // the PWM measurement value from the timer
@@ -144,37 +152,76 @@ void io_init(void)
 
 int main(void)
 {
+	uint16_t measurement_high = 0, measurement_low = 0; // the average measurement value
+	uint16_t zero_high = 0, zero_low = 0; // the intial zero value
+	uint8_t count_high = 0, count_low = 0; // the number of stable counts
+	
 	io_init(); // initialize IOs
 
 	printf(PSTR("welcome to the body scale weight reader\n")); // print welcome message
 	while (true) { // endless loop for micro-controller
-#ifdef DEBUG
 		/* display if scale switches on */
 		if (scale_old!=scale_on && scale_on) {
+#ifdef DEBUG
 			printf(PSTR("scale start\n"));
-			scale_old=scale_on;
-		}
 #endif
+			measurement_high = measurement_low = zero_high = zero_low = count_high = count_low = 0; // re-initialize values
+			scale_old = scale_on; // save new state
+		}
 		/* display value if available */
 		if (measurement_flag) {
 			if (pwm_high) {
 #ifdef DEBUG
-				printf("high: %u\n",measurement_value);
+				printf("high: %u\n", measurement_value);
 #endif
+				if (measurement_value<=measurement_high+JITTER_HIGH && measurement_value>=measurement_high-JITTER_HIGH) { // stable value
+					measurement_high = (measurement_high+measurement_value)/2;
+					count_high += 1;
+				} else {
+					measurement_high = measurement_value;
+					count_high = 0;
+				}
 			} else {
 #ifdef DEBUG
-				printf("low: %u\n",measurement_value);
+				printf("low: %u\n", measurement_value);
 #endif
+				if (measurement_value<=measurement_low+JITTER_LOW && measurement_value>=measurement_low-JITTER_LOW) { // stable value
+					measurement_low = (measurement_low+measurement_value)/2;
+					count_low += 1;
+				} else {
+					measurement_low = measurement_value;
+					count_low = 0;
+				}
 			}
+#ifdef DEBUG
+			printf("values high/low (stable): %u (%u)/%u (%u)\n", measurement_high, count_high, measurement_low, count_low);
+#endif
 			measurement_flag = false;
 		}
+		/* display weight if values are stable */
+		if (count_high>=MEASUREMENT_STABLE && count_low>=MEASUREMENT_STABLE) {
+			if (zero_high == 0 && zero_low == 0) { // initialize zero values
+				zero_high = measurement_high;
+				zero_low = measurement_low;
 #ifdef DEBUG
+				printf("zero values high/low: %u/%u\n", zero_high, zero_low);
+#endif
+			} else { // prinf measurement weight
+				double weight_high = (int16_t)(zero_high-measurement_high)/COEF_HIGH;
+				double weight_low = (int16_t)(measurement_low-zero_low)/COEF_LOW;
+				double weight = (weight_high+weight_low)/2;
+				printf("%d kg\n", zero_high-measurement_high);
+				printf("%.2f kg\n", weight);
+			}
+			count_high = count_low = 0;
+		}
 		/* display if scale switches off */
 		if (scale_old!=scale_on && !scale_on) {
+#ifdef DEBUG
 			printf(PSTR("scale stop\n"));
-			scale_old=scale_on;
-		}
 #endif
+			scale_old = scale_on; // save new state
+		}
 		/* go to sleep and wait for next interrupt */
 		set_sleep_mode(SLEEP_MODE_IDLE);
 		sleep_mode();