6 files changed, 142 insertions, 19 deletions
diff --git a/testhal/STM32F1xx/I2C/Makefile b/testhal/STM32F1xx/I2C/Makefile
index 37db54251..08494928a 100644
--- a/testhal/STM32F1xx/I2C/Makefile
+++ b/testhal/STM32F1xx/I2C/Makefile
@@ -82,7 +82,9 @@ CSRC = $(PORTSRC) \
        $(CHIBIOS)/os/various/syscalls.c \
        main.c \
        i2c_pns.c \
-  	   lis3.c\
+  	   lis3.c \
+  	   tmp75.c \
+  	   fake.c
 
 	   
        
diff --git a/testhal/STM32F1xx/I2C/fake.c b/testhal/STM32F1xx/I2C/fake.c
new file mode 100644
index 000000000..75f242207
--- /dev/null
+++ b/testhal/STM32F1xx/I2C/fake.c
@@ -0,0 +1,38 @@
+/**
+ * Not responding slave test
+ */
+
+#include <stdlib.h>
+
+#include "ch.h"
+#include "hal.h"
+
+#include "fake.h"
+
+
+/* input buffer */
+static uint8_t rx_data[2];
+
+/* temperature value */
+static int16_t temperature = 0;
+
+
+#define addr 0b1001100
+
+/* This is main function. */
+void request_fake(void){
+  i2cflags_t errors = 0;
+
+  i2cAcquireBus(&I2CD1);
+  errors = i2cMasterReceive(&I2CD1, addr, rx_data, 2);
+  i2cReleaseBus(&I2CD1);
+
+  if (errors == I2CD_ACK_FAILURE){
+    __NOP();
+  }
+  else{
+    temperature = (rx_data[0] << 8) + rx_data[1];
+  }
+}
+
+
diff --git a/testhal/STM32F1xx/I2C/fake.h b/testhal/STM32F1xx/I2C/fake.h
new file mode 100644
index 000000000..a83652e29
--- /dev/null
+++ b/testhal/STM32F1xx/I2C/fake.h
@@ -0,0 +1,6 @@
+#ifndef FAKE_H_
+#define FAKE_H_
+
+void request_fake(void);
+
+#endif /* FAKE_H_ */
diff --git a/testhal/STM32F1xx/I2C/main.c b/testhal/STM32F1xx/I2C/main.c
index ae1bea5af..0ca040e88 100644
--- a/testhal/STM32F1xx/I2C/main.c
+++ b/testhal/STM32F1xx/I2C/main.c
@@ -17,12 +17,15 @@
  * amount of time.
  */
 
+#include <stdlib.h>
+
 #include "ch.h"
 #include "hal.h"
 
 #include "i2c_pns.h"
 #include "lis3.h"
-
+#include "tmp75.h"
+#include "fake.h"
 
 
 /*
@@ -45,35 +48,45 @@ static msg_t Blink(void *arg) {
  */
 static WORKING_AREA(PollAccelThreadWA, 128);
 static msg_t PollAccelThread(void *arg) {
+  chRegSetThreadName("PollAccel");
   (void)arg;
-  systime_t time = chTimeNow();
-
   while (TRUE) {
-    time += MS2ST(20);
+//    chThdSleepMilliseconds(rand() & 31);
+    chThdSleepMilliseconds(32);
     request_acceleration_data();
-    chThdSleepUntil(time);
   }
   return 0;
 }
 
 
-/*
- * Accelerometer thread
- */
-static WORKING_AREA(PollAccelThreadWA, 128);
-static msg_t PollAccelThread(void *arg) {
+/* Temperature polling thread */
+static WORKING_AREA(PollTmp75ThreadWA, 128);
+static msg_t PollTmp75Thread(void *arg) {
+  chRegSetThreadName("PollTmp75");
   (void)arg;
-  systime_t time = chTimeNow();
-
   while (TRUE) {
-    time += MS2ST(20);
-    request_acceleration_data();
-    chThdSleepUntil(time);
+//    chThdSleepMilliseconds(rand() & 31);
+    chThdSleepMilliseconds(15);
+    /* Call reading function */
+    request_temperature();
   }
   return 0;
 }
 
 
+/* Temperature polling thread */
+static WORKING_AREA(PollFakeThreadWA, 128);
+static msg_t PollFakeThread(void *arg) {
+  chRegSetThreadName("PollFake");
+  (void)arg;
+  while (TRUE) {
+    chThdSleepMilliseconds(16);
+    /* Call reading function */
+    request_fake();
+  }
+  return 0;
+}
+
 
 /*
  * Entry point, note, the main() function is already a thread in the system
@@ -84,18 +97,32 @@ int main(void) {
   halInit();
   chSysInit();
 
-  chThdSleepMilliseconds(1000);
+  chThdSleepMilliseconds(200);
   I2CInit_pns();
 
   /* Create accelerometer thread */
   chThdCreateStatic(PollAccelThreadWA,
           sizeof(PollAccelThreadWA),
-          HIGHPRIO,
+          NORMALPRIO,
           PollAccelThread,
           NULL);
 
+  /* Create temperature thread */
+  chThdCreateStatic(PollTmp75ThreadWA,
+          sizeof(PollTmp75ThreadWA),
+          NORMALPRIO,
+          PollTmp75Thread,
+          NULL);
+
+  /* Create not responding thread */
+  chThdCreateStatic(PollFakeThreadWA,
+          sizeof(PollFakeThreadWA),
+          NORMALPRIO,
+          PollFakeThread,
+          NULL);
+
   /* Creates the blinker thread. */
-  chThdCreateStatic(BlinkWA, sizeof(BlinkWA), LOWPRIO, Blink, NULL);
+  chThdCreateStatic(BlinkWA, sizeof(BlinkWA), HIGHPRIO, Blink, NULL);
 
   /* main loop that do nothing */
   while (TRUE) {
diff --git a/testhal/STM32F1xx/I2C/tmp75.c b/testhal/STM32F1xx/I2C/tmp75.c
new file mode 100644
index 000000000..7acc8c668
--- /dev/null
+++ b/testhal/STM32F1xx/I2C/tmp75.c
@@ -0,0 +1,37 @@
+/**
+ * TMP75 is most simple I2C device in our case. It is already useful with
+ * default settings after powerup.
+ * You only must read 2 sequential bytes from it.
+ */
+
+#include <stdlib.h>
+
+#include "ch.h"
+#include "hal.h"
+
+#include "tmp75.h"
+
+
+/* input buffer */
+static uint8_t tmp75_rx_data[TMP75_RX_DEPTH];
+
+/* temperature value */
+static int16_t temperature = 0;
+
+
+#define tmp75_addr 0b1001000
+
+/* This is main function. */
+void request_temperature(void){
+  int16_t t_int = 0, t_frac = 0;
+
+  i2cAcquireBus(&I2CD1);
+  i2cMasterReceive(&I2CD1, tmp75_addr, tmp75_rx_data, 2);
+  i2cReleaseBus(&I2CD1);
+
+  t_int = tmp75_rx_data[0] * 100;
+  t_frac = (tmp75_rx_data[1] * 100) >> 8;
+  temperature = t_int + t_frac;
+}
+
+
diff --git a/testhal/STM32F1xx/I2C/tmp75.h b/testhal/STM32F1xx/I2C/tmp75.h
new file mode 100644
index 000000000..ab4b5fa9b
--- /dev/null
+++ b/testhal/STM32F1xx/I2C/tmp75.h
@@ -0,0 +1,13 @@
+#ifndef TMP75_H_
+#define TMP75_H_
+
+
+
+/* buffers depth */
+#define TMP75_RX_DEPTH 2
+#define TMP75_TX_DEPTH 2
+
+void init_tmp75(void);
+void request_temperature(void);
+
+#endif /* TMP75_H_ */