X-Git-Url: http://rtime.felk.cvut.cz/gitweb/pes-rpp/rpp-lib.git/blobdiff_plain/3e0b650fe9e8a76af16b119a0730c9a5df02c40c..5e5f1df1fad4674a87019fa6c72a2f2d2e0d8db0:/rpp/src/rpp/hbr.c

diff --git a/rpp/src/rpp/hbr.c b/rpp/src/rpp/hbr.c
index d55cd9f..d628092 100644
--- a/rpp/src/rpp/hbr.c
+++ b/rpp/src/rpp/hbr.c
@@ -1,4 +1,4 @@
-/* Copyright (C) 2013, 2014 Czech Technical University in Prague
+/* Copyright (C) 2013, 2014, 2015 Czech Technical University in Prague
  *
  * Authors:
  *     - Carlos Jenkins <carlos@jenkins.co.cr>
@@ -19,25 +19,30 @@
 
 
 #include "rpp/rpp.h"
+#include "rpp/mutex.h"
 
 #ifndef FREERTOS_POSIX
 #include "drv/hbridge.h"
+#include "drv/spi_def.h"
 #endif
 
+RPP_MUTEX_DEFINE(mutex_hbr);
+
 static boolean_t initialized = FALSE;
 
 int8_t rpp_hbr_init()
 {
-    if(initialized) {
-        return FAILURE;
-    }
-    initialized = TRUE;
+	if (initialized)
+		return FAILURE;
+	if (!RPP_MUTEX_INIT(mutex_hbr))
+		return FAILURE;
+	initialized = TRUE;
 #ifndef FREERTOS_POSIX
-    dmmInit();
-    hetInit();
-    spi_tms570_init();
+	dmmInit();
+	hetInit();
+	spi_tms570_init(spi_ifcs, ARRAY_SIZE(spi_ifcs));
 #endif
-    return SUCCESS;
+	return SUCCESS;
 }
 
 
@@ -45,10 +50,10 @@ int8_t rpp_hbr_init()
 static void rpp_hdr_reset()
 {
 #ifndef FREERTOS_POSIX
-    drv_hbr_set_en(LOW);
-    drv_hbr_pwm_set_duty(0);
-    drv_hbr_pwm_stop();
-    drv_hbr_set_dir(LOW);
+	drv_hbr_set_en(LOW);
+	drv_hbr_pwm_set_duty(0);
+	drv_hbr_pwm_stop();
+	drv_hbr_set_dir(LOW);
 #endif
 }
 
@@ -57,82 +62,84 @@ static boolean_t enabled = FALSE;
 
 int8_t rpp_hbr_enable(int32_t period)
 {
-    if(enabled) {
-        return FAILURE;
-    }
-
-    if(period < 1) {
-        period = 55; // ~18kHz (18181.818181818 Hz to be precise)
-    }
+	if (enabled)
+		return FAILURE;
 
-    rpp_hdr_reset();
+	if (period < 1)
+		period = 55;  // ~18kHz (18181.818181818 Hz to be precise)
 
 #ifndef FREERTOS_POSIX
-
-    // Configure N2HET
-    if (drv_hbr_pwm_set_signal(period, 0) != SUCCESS) {
-        return FAILURE;
-    }
-    drv_hbr_pwm_start();
-
-    drv_hbr_set_en(HIGH);
-
-    // Start watchdog
-    int ret = drv_hbr_wdg_start();
-    if (ret != SUCCESS && ret != -RPP_EBUSY) { // Don't fail if already started
-        return FAILURE;
-    }
+	RPP_MUTEX_LOCK(mutex_hbr);
+	rpp_hdr_reset();
+
+	// Configure N2HET
+	if (drv_hbr_pwm_set_signal(period, 0) != SUCCESS) {
+		RPP_MUTEX_UNLOCK(mutex_hbr);
+		return FAILURE;
+	}
+	drv_hbr_pwm_start();
+
+	drv_hbr_set_en(HIGH);
+
+	// Start watchdog
+	int ret = drv_hbr_wdg_start();
+	if (ret != SUCCESS && ret != -RPP_EBUSY) {   // Don't fail if already started
+		RPP_MUTEX_UNLOCK(mutex_hbr);
+		return FAILURE;
+	}
+	RPP_MUTEX_UNLOCK(mutex_hbr);
 #endif
 
-    enabled = TRUE;
+	enabled = TRUE;
 
-    return SUCCESS;
+	return SUCCESS;
 }
 
 
 int8_t rpp_hbr_control(double cmd)
 {
-    if(!enabled) {
-        return -1;
-    }
+	if (!enabled)
+		return -1;
 
-    // Check range of the command
-    if((cmd < -1.0) || (cmd > 1.0)) {
-        return -2;
-    }
+	// Check range of the command
+	if ((cmd < -1.0) || (cmd > 1.0))
+		return -2;
 
 #ifndef FREERTOS_POSIX
-    // Great, now scale and return to sanity world of ints :D
-    int32_t scaled = (int32_t)(cmd * 100.0);
+	// Great, now scale and return to sanity world of ints :D
+	int32_t scaled = (int32_t)(cmd * 100.0);
 
 
-    // Set direction
-    drv_hbr_set_dir(scaled > 0);
+	// Set direction
+	RPP_MUTEX_LOCK(mutex_hbr);
+	drv_hbr_set_dir(scaled > 0);
 
-    // Set PWM duty cycle
-    drv_hbr_pwm_set_duty(abs(scaled));
+	// Set PWM duty cycle
+	drv_hbr_pwm_set_duty(abs(scaled));
+	RPP_MUTEX_UNLOCK(mutex_hbr);
 #endif
 
-    return SUCCESS;
+	return SUCCESS;
 }
 
 
 int8_t rpp_hbr_disable()
 {
-    if(!enabled) {
-        return FAILURE;
-    }
+	if (!enabled)
+		return FAILURE;
 
 #ifndef FREERTOS_POSIX
-    rpp_hdr_reset();
-
-    // We ignore is watchdog could not be stopped, because is harmless.
-    // It would be worse if we just could not stop the H-Bridge just because
-    // the watchdog could not be stopped.
-    drv_hbr_wdg_stop();
+	RPP_MUTEX_LOCK(mutex_hbr);
+	rpp_hdr_reset();
+
+	// We ignore is watchdog could not be stopped, because is harmless.
+	// It would be worse if we just could not stop the H-Bridge just because
+	// the watchdog could not be stopped.
+	drv_hbr_wdg_stop();
+	RPP_MUTEX_UNLOCK(mutex_hbr);
 #endif
 
-    enabled = FALSE;
+	enabled = FALSE;
 
-    return SUCCESS;
+	return SUCCESS;
 }