diff --git a/Firmware/adc.h b/Firmware/adc.h
index 1d286917..9ff137df 100644
--- a/Firmware/adc.h
+++ b/Firmware/adc.h
@@ -10,6 +10,17 @@
 extern "C" {
 #endif //defined(__cplusplus)
 
+/*
+http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html
+*/
+#define BITCOUNT(x) (((BX_(x)+(BX_(x)>>4)) & 0x0F0F0F0F) % 255)
+#define BX_(x) ((x) - (((x)>>1)&0x77777777) - (((x)>>2)&0x33333333) - (((x)>>3)&0x11111111))
+
+#define ADC_PIN_IDX(pin) BITCOUNT(ADC_CHAN_MSK & ((1 << (pin)) - 1))
+
+#if BITCOUNT(ADC_CHAN_MSK) != ADC_CHAN_CNT
+# error "ADC_CHAN_MSK oes not match ADC_CHAN_CNT"
+#endif
 
 extern uint8_t adc_state;
 extern uint8_t adc_count;
diff --git a/Firmware/temperature.cpp b/Firmware/temperature.cpp
index e281303f..6a21b825 100644
--- a/Firmware/temperature.cpp
+++ b/Firmware/temperature.cpp
@@ -1552,17 +1552,17 @@ extern "C" {
 
 void adc_ready(void) //callback from adc when sampling finished
 {
-	current_temperature_raw[0] = adc_values[TEMP_0_PIN]; //heater
-	current_temperature_raw_pinda = adc_values[TEMP_PINDA_PIN];
-	current_temperature_bed_raw = adc_values[TEMP_BED_PIN];
+	current_temperature_raw[0] = adc_values[ADC_PIN_IDX(TEMP_0_PIN)]; //heater
+	current_temperature_raw_pinda = adc_values[ADC_PIN_IDX(TEMP_PINDA_PIN)];
+	current_temperature_bed_raw = adc_values[ADC_PIN_IDX(TEMP_BED_PIN)];
 #ifdef VOLT_PWR_PIN
-	current_voltage_raw_pwr = adc_values[VOLT_PWR_PIN];
+	current_voltage_raw_pwr = adc_values[ADC_PIN_IDX(VOLT_PWR_PIN)];
 #endif
 #ifdef AMBIENT_THERMISTOR
-	current_temperature_raw_ambient = adc_values[TEMP_AMBIENT_PIN];
+	current_temperature_raw_ambient = adc_values[ADC_PIN_IDX(TEMP_AMBIENT_PIN)];
 #endif //AMBIENT_THERMISTOR
 #ifdef VOLT_BED_PIN
-	current_voltage_raw_bed = adc_values[VOLT_BED_PIN]; // 6->9
+	current_voltage_raw_bed = adc_values[ADC_PIN_IDX(VOLT_BED_PIN)]; // 6->9
 #endif
 	temp_meas_ready = true;
 }