--- a/kstmApp/src/pi_2_dht_read.c	Fri Aug 14 11:30:43 2015 +0200
+++ b/kstmApp/src/pi_2_dht_read.c	Wed Sep 09 18:06:59 2015 +0200
@@ -21,6 +21,10 @@
 #include <stdbool.h>
 #include <stdlib.h>
 
+#include "epicsExit.h"
+#include "epicsThread.h"
+#include "iocsh.h"
+
 #include "pi_2_dht_read.h"
 #include "pi_2_mmio.h"
 
@@ -36,6 +40,11 @@
 #define DHT_PULSES 41
 
 int pi_2_dht_read(int type, int pin, float* humidity, float* temperature) {
+int i;
+volatile int j;
+unsigned int defPrio;
+epicsThreadId threadIdSelf;
+
   // Validate humidity and temperature arguments and set them to zero.
   if (humidity == NULL || temperature == NULL) {
     return DHT_ERROR_ARGUMENT;
@@ -56,7 +65,9 @@
   pi_2_mmio_set_output(pin);
 
   // Bump up process priority and change scheduler to try to try to make process more 'real time'.
-  set_max_priority();
+  defPrio = epicsThreadGetPrioritySelf();
+  threadIdSelf = epicsThreadGetIdSelf();
+  epicsThreadSetPriority( threadIdSelf , epicsThreadPriorityHigh);
 
   // Set pin high for ~500 milliseconds.
   pi_2_mmio_set_high(pin);
@@ -72,28 +83,26 @@
   // Set pin at input.
   pi_2_mmio_set_input(pin);
   // Need a very short delay before reading pins or else value is sometimes still low.
-volatile int i;
-  for (i = 0; i < 50; ++i) {
-  }
+  for (j = 0; j < 50; ++j) { }
 
   // Wait for DHT to pull pin low.
   uint32_t count = 0;
   while (pi_2_mmio_input(pin)) {
     if (++count >= DHT_MAXCOUNT) {
       // Timeout waiting for response.
-      set_default_priority();
+      epicsThreadSetPriority( threadIdSelf , defPrio);
       return DHT_ERROR_TIMEOUT;
     }
   }
 
-  // Record pulse widths for the expected result bits.
-  int j;
-  for ( j=0; j < DHT_PULSES*2; j+=2) {
+
+ // Record pulse widths for the expected result bits.
+  for ( i=0; i < DHT_PULSES*2; i+=2) {
     // Count how long pin is low and store in pulseCounts[i]
     while (!pi_2_mmio_input(pin)) {
       if (++pulseCounts[i] >= DHT_MAXCOUNT) {
         // Timeout waiting for response.
-        set_default_priority();
+        epicsThreadSetPriority( threadIdSelf , defPrio);
         return DHT_ERROR_TIMEOUT;
       }
     }
@@ -101,23 +110,22 @@
     while (pi_2_mmio_input(pin)) {
       if (++pulseCounts[i+1] >= DHT_MAXCOUNT) {
         // Timeout waiting for response.
-        set_default_priority();
+        epicsThreadSetPriority( threadIdSelf , defPrio);
         return DHT_ERROR_TIMEOUT;
       }
     }
   }
 
   // Done with timing critical code, now interpret the results.
-
   // Drop back to normal priority.
-  set_default_priority();
+  // set_default_priority();
+  epicsThreadSetPriority( threadIdSelf , defPrio);
 
   // Compute the average low pulse width to use as a 50 microsecond reference threshold.
   // Ignore the first two readings because they are a constant 80 microsecond pulse.
   uint32_t threshold = 0;
-  int k;
-  for (k=2; i < DHT_PULSES*2; k+=2) {
-    threshold += pulseCounts[k];
+  for (i=2; i < DHT_PULSES*2; i+=2) {
+    threshold += pulseCounts[i];
   }
   threshold /= DHT_PULSES-1;
 
@@ -125,11 +133,10 @@
   // If the count is less than 50us it must be a ~28us 0 pulse, and if it's higher
   // then it must be a ~70us 1 pulse.
   uint8_t data[5] = {0};
-  int l;
-  for ( l=3; l < DHT_PULSES*2; l+=2) {
-    int index = (l-3)/16;
+  for ( i=3; i < DHT_PULSES*2; i+=2) {
+    int index = (i-3)/16;
     data[index] <<= 1;
-    if (pulseCounts[l] >= threshold) {
+    if (pulseCounts[i] >= threshold) {
       // One bit for long pulse.
       data[index] |= 1;
     }