A4MCAR - Sonar sensor task added

Signed-off-by: Mustafa Ozcelikors <mozcelikors@gmail.com>
diff --git a/a4mcar/low_level_applications/src/sonar_sensor.h b/a4mcar/low_level_applications/src/sonar_sensor.h
new file mode 100644
index 0000000..f048b1c
--- /dev/null
+++ b/a4mcar/low_level_applications/src/sonar_sensor.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2017 FH Dortmund.
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * which accompanies this distribution, and is available at
+ * http://www.eclipse.org/legal/epl-v10.html
+ *
+ * Description:
+ *    A4MCAR Project - Low-level Module SRF02 Sonar Sensor driver functions and driving task - Header file
+ *
+ * Authors:
+ *    M. Ozcelikors <mozcelikors@gmail.com>
+ *
+ * Update History:
+ *
+ */
+
+#ifndef SONAR_SENSOR_H_
+#define SONAR_SENSOR_H_
+
+#include "defines.h"
+
+// I2C Related Defines
+#define I2C_SPEED_KBITPERSEC    ((unsigned) 10)  // I2C speed
+#define Task_MaintainI2CConnection i2c_master    // Renaming library function i2c_master
+
+// Distance sensor related Defines
+#define SENSOR_READ_PERIOD              (200 * MILLISECOND)
+
+#define WRDATA_CENTIMETERS              ((uint8_t)0x51)
+
+#define LEFT_DISTANCE_SENSOR_ID         0
+#define RIGHT_DISTANCE_SENSOR_ID        1
+#define FRONT_DISTANCE_SENSOR_ID        2
+#define REAR_DISTANCE_SENSOR_ID         3
+
+#define LEFT_DISTANCE_SENSOR_DEVICEADDR     ((uint8_t) 0x7A)
+#define RIGHT_DISTANCE_SENSOR_DEVICEADDR    ((uint8_t) 0x7B)
+#define FRONT_DISTANCE_SENSOR_DEVICEADDR    ((uint8_t) 0x7C)
+#define REAR_DISTANCE_SENSOR_DEVICEADDR     ((uint8_t) 0x79)
+
+// Prototypes
+uint8_t getDistanceSensorAddr (int sensor_id);
+void Task_ReadSonarSensors(client i2c_master_if i2c_interface, client distancesensor_if sensors_interface);
+int InitializeMessaging (client i2c_master_if i2c_interface);
+
+#endif /* SONAR_SENSOR_H_ */
diff --git a/a4mcar/low_level_applications/src/sonar_sensor.xc b/a4mcar/low_level_applications/src/sonar_sensor.xc
new file mode 100644
index 0000000..207b993
--- /dev/null
+++ b/a4mcar/low_level_applications/src/sonar_sensor.xc
@@ -0,0 +1,201 @@
+/*
+ * Copyright (c) 2017 FH Dortmund.
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * which accompanies this distribution, and is available at
+ * http://www.eclipse.org/legal/epl-v10.html
+ *
+ * Description:
+ *    A4MCAR Project - Low-level Module SRF02 Sonar Sensor driver functions and driving task
+ *
+ * Authors:
+ *    M. Ozcelikors <mozcelikors@gmail.com>
+ *
+ * Update History:
+ *
+ */
+
+#include "sonar_sensor.h"
+#include "core_debug.h"
+
+/***
+ *  Function Name:              getDistanceSensorAddr
+ *  Function Description :      Returns the sonar sensor I2C device address
+ *
+ *  Argument                Type                       Description
+ *  sensor_id               int                        Identity of the sensor described in the header file
+ */
+uint8_t getDistanceSensorAddr (int sensor_id)
+{
+    switch (sensor_id)
+    {
+        case LEFT_DISTANCE_SENSOR_ID:   return LEFT_DISTANCE_SENSOR_DEVICEADDR;    break;
+        case RIGHT_DISTANCE_SENSOR_ID:  return RIGHT_DISTANCE_SENSOR_DEVICEADDR;   break;
+        case FRONT_DISTANCE_SENSOR_ID:  return FRONT_DISTANCE_SENSOR_DEVICEADDR;   break;
+        case REAR_DISTANCE_SENSOR_ID:   return REAR_DISTANCE_SENSOR_DEVICEADDR;    break;
+        default: return FRONT_DISTANCE_SENSOR_DEVICEADDR;   break;
+    }
+    return FRONT_DISTANCE_SENSOR_DEVICEADDR;
+}
+
+/***
+ *  Function Name:              InitializeMessaging
+ *  Function Description :      Initializes the I2C communication with four SRF-02 sonar sensors
+ *
+ *  Argument                Type                                        Description
+ *  i2c_interface           client i2c_master_if                        Master Mode I2C Comm Interface
+ */
+int InitializeMessaging (client i2c_master_if i2c_interface)
+{
+    int error_flag = 0;
+    i2c_regop_res_t result;
+
+    // Start messaging
+    result = i2c_interface.write_reg(getDistanceSensorAddr(LEFT_DISTANCE_SENSOR_ID), 0x00, WRDATA_CENTIMETERS); //Left Sensor //Args: addr, reg, data
+    if (result != I2C_REGOP_SUCCESS) {
+        //printf("I2C write reg failed\n");
+        error_flag = -1;
+    }
+
+    result = i2c_interface.write_reg(getDistanceSensorAddr(RIGHT_DISTANCE_SENSOR_ID), 0x00, WRDATA_CENTIMETERS); //Right Sensor //Args: addr, reg, data
+    if (result != I2C_REGOP_SUCCESS) {
+        //printf("I2C write reg failed\n");
+        error_flag = -1;
+    }
+
+    result = i2c_interface.write_reg(getDistanceSensorAddr(FRONT_DISTANCE_SENSOR_ID), 0x00, WRDATA_CENTIMETERS); //Front Sensor //Args: addr, reg, data
+    if (result != I2C_REGOP_SUCCESS) {
+        //printf("I2C write reg failed\n");
+        error_flag = -1;
+    }
+
+    result = i2c_interface.write_reg(getDistanceSensorAddr(REAR_DISTANCE_SENSOR_ID), 0x00, WRDATA_CENTIMETERS); //Rear Sensor //Args: addr, reg, data
+    if (result != I2C_REGOP_SUCCESS) {
+        //printf("I2C write reg failed\n");
+        error_flag = -1;
+    }
+
+    return error_flag;
+}
+
+
+/***
+ *  Function Name:              Task_ReadSonarSensors
+ *  Function Description :      Reads 4 SRF02 Ultrasonic sensor values and publishes them to interface
+ *                              sensors_interface
+ *
+ *  Argument                Type                                        Description
+ *  i2c_interface           client i2c_master_if                        Master Mode I2C Comm Interface
+ *  sensors_interface       client distancesensor_if                    Used to share distance sensor values
+ */
+void Task_ReadSonarSensors(client i2c_master_if i2c_interface, client distancesensor_if sensors_interface)
+{
+    // Declare some variables to use later
+    i2c_regop_res_t result;
+    uint8_t high_byte;
+    uint8_t low_byte;
+    unsigned acc; //accumulator for distance measurement
+    uint8_t left, right, front, rear;
+
+    timer tmr3;
+    uint32_t time3, delay3 = SENSOR_READ_PERIOD;
+
+    PrintCoreAndTileInformation("Task_ReadSonarSensors");
+
+    // Timing measurement/debugging related definitions
+    timer debug_timer;
+    uint32_t start_time, end_time;
+
+    tmr3 :> time3;
+
+    while (1) {
+        select
+        {
+            case tmr3 when timerafter(time3) :> void :
+               //Measure start time
+               ////debug_timer :> start_time;
+
+               //Initialize messaging
+               InitializeMessaging(i2c_interface);
+
+               // For Front Sensor
+               // Read from high and low byte respectively
+               high_byte = i2c_interface.read_reg(getDistanceSensorAddr(FRONT_DISTANCE_SENSOR_ID), 0x02, result);
+               low_byte = i2c_interface.read_reg(getDistanceSensorAddr(FRONT_DISTANCE_SENSOR_ID),  0x03, result);
+               // Construct the distance information in centimeters
+               acc = (high_byte * 256) + low_byte;
+               if ((acc < 600)  && (acc > 0)) // Distance should be in between 600cm and 0cm
+               {
+                   front = acc;
+               }
+               else
+               {
+                   front = 0;
+               }
+
+
+               // For Rear Sensor
+               // Read from high and low byte respectively
+               high_byte = i2c_interface.read_reg(getDistanceSensorAddr(REAR_DISTANCE_SENSOR_ID), 0x02, result);
+               low_byte = i2c_interface.read_reg(getDistanceSensorAddr(REAR_DISTANCE_SENSOR_ID),  0x03, result);
+               // Construct the distance information in centimeters
+               acc = (high_byte * 256) + low_byte;
+               if ((acc < 600)  && (acc > 0)) // Distance should be in between 600cm and 0cm
+               {
+                   rear = acc;
+               }
+               else
+               {
+                   rear = 0;
+               }
+
+
+                /*// For Left Sensor
+                // Read from high and low byte respectively
+                high_byte = i2c_interface.read_reg(getDistanceSensorAddr(LEFT_DISTANCE_SENSOR_ID), 0x02, result);
+                low_byte = i2c_interface.read_reg(getDistanceSensorAddr(LEFT_DISTANCE_SENSOR_ID),  0x03, result);
+                // Construct the distance information in centimeters
+                acc = (high_byte * 256) + low_byte;
+                if ((acc < 600)  && (acc > 0)) // Distance should be in between 600cm and 0cm
+                {
+                    left = acc;
+                }
+                else
+                {
+                    left = 0;
+                }
+                //printf("y\n");*/
+
+
+
+                /*// For Right Sensor
+                // Read from high and low byte respectively
+                high_byte = i2c_interface.read_reg(getDistanceSensorAddr(RIGHT_DISTANCE_SENSOR_ID), 0x02, result);
+                low_byte = i2c_interface.read_reg(getDistanceSensorAddr(RIGHT_DISTANCE_SENSOR_ID),  0x03, result);
+                // Construct the distance information in centimeters
+                acc = (high_byte * 256) + low_byte;
+                if ((acc < 600)  && (acc > 0)) // Distance should be in between 600cm and 0cm
+                {
+                    right = acc;
+                }
+                else
+                {
+                    right = 0;
+                }*/
+
+                //Uncomment to see the debug output
+                //debug_printf("%d %d %d %d\n", left, right, front, rear);
+
+                //Send sensor values all together
+                sensors_interface.ShareDistanceSensorValues (left, right, front, rear);
+
+                //Timer Delay
+                time3 += delay3;
+
+                //Measure end time
+                ////debug_timer :> end_time;
+                ////printf("SONAR t: %u", end_time - start_time);
+                break;
+        }
+    }
+}