From a0d68c6afb2229e0f8a93da5f6ce0f89df0ef4c9 Mon Sep 17 00:00:00 2001
From: Alessio Morale <alessiomorale@gmail.com>
Date: Sun, 8 Jun 2014 21:33:18 +0200
Subject: [PATCH] OP-1227 Fix high CPU usage: Reduce Accel rate to 1600Hz,
 optimize sample reading, explicitly set bus speed Note: Counter 0xA3450001 is
 the time it takes to transfer the requested number of samples

---
 flight/pios/common/pios_adxl345.c | 63 ++++++++++++++++++++++++++++---
 flight/pios/inc/pios_adxl345.h    |  1 +
 2 files changed, 58 insertions(+), 6 deletions(-)

diff --git a/flight/pios/common/pios_adxl345.c b/flight/pios/common/pios_adxl345.c
index da2d1677c..8a789ae34 100644
--- a/flight/pios/common/pios_adxl345.c
+++ b/flight/pios/common/pios_adxl345.c
@@ -41,6 +41,12 @@ struct adxl345_dev {
     uint32_t slave_num;
     enum pios_adxl345_dev_magic magic;
 };
+#undef PIOS_INCLUDE_INSTRUMENTATION
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+#include <pios_instrumentation.h>
+static int8_t counterUpd;
+// Counter 0xA3450001 time it takes to transfer the requested amount of samples using PIOS_ADXL345_ReadAndAccumulateSamples
+#endif
 
 // ! Global structure for this device device
 static struct adxl345_dev *dev;
@@ -59,6 +65,9 @@ static struct adxl345_dev *PIOS_ADXL345_alloc(void)
 {
     struct adxl345_dev *adxl345_dev;
 
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    counterUpd  = PIOS_Instrumentation_CreateCounter(0xA3450001);
+#endif
     adxl345_dev = (struct adxl345_dev *)pvPortMalloc(sizeof(*adxl345_dev));
     if (!adxl345_dev) {
         return NULL;
@@ -99,7 +108,7 @@ static int32_t PIOS_ADXL345_ClaimBus()
     if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
         return -2;
     }
-
+    PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_8);
     PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
 
     return 0;
@@ -237,7 +246,7 @@ int32_t PIOS_ADXL345_Init(uint32_t spi_id, uint32_t slave_num)
     dev->slave_num = slave_num;
 
     PIOS_ADXL345_ReleaseBus();
-    PIOS_ADXL345_SelectRate(ADXL_RATE_3200);
+    PIOS_ADXL345_SelectRate(ADXL_RATE_1600);
     PIOS_ADXL345_SetRange(ADXL_RANGE_8G);
     PIOS_ADXL345_FifoDepth(16);
     PIOS_ADXL345_SetMeasure(1);
@@ -299,6 +308,7 @@ int32_t PIOS_ADXL345_FifoElements()
     return rec[1] & 0x3f;
 }
 
+
 /**
  * @brief Read a single set of values from the x y z channels
  * @returns The number of samples remaining in the fifo
@@ -312,13 +322,11 @@ uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data)
     if (PIOS_ADXL345_ClaimBus() != 0) {
         return -2;
     }
-
     // To save memory use same buffer for in and out but offset by
     // a byte
-    uint8_t buf[9] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-    uint8_t rec[9] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+    uint8_t buf[10] = { 0 };
+    uint8_t *rec    = buf + 1;
     buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT; // Multibyte read starting at X0
-
     if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], 9, NULL) < 0) {
         PIOS_ADXL345_ReleaseBus();
         return -3;
@@ -333,4 +341,47 @@ uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data)
     return rec[8] & 0x7F; // return number of remaining entries
 }
 
+/**
+ * @brief Read a single set of values from the x y z channels
+ * @returns 0 if successful
+ */
+
+int8_t PIOS_ADXL345_ReadAndAccumulateSamples(struct pios_adxl345_data *data, uint8_t samples)
+{
+    if (PIOS_ADXL345_Validate(dev) != 0) {
+        return -1;
+    }
+
+    if (PIOS_ADXL345_ClaimBus() != 0) {
+        return -2;
+    }
+    data->x = 0;
+    data->y = 0;
+    data->z = 0;
+    // To save memory use same buffer for in and out but offset by
+    // a byte
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    PIOS_Instrumentation_TimeStart(counterUpd);
+#endif
+    for (uint8_t i = 0; i < samples; i++) {
+        uint8_t buf[7] = { 0 };
+        uint8_t *rec   = &buf[1];
+        PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
+        buf[0] = ADXL_X0_ADDR | ADXL_MULTI_BIT | ADXL_READ_BIT; // Multibyte read starting at X0
+        if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], 7, NULL) < 0) {
+            PIOS_ADXL345_ReleaseBus();
+            return -3;
+        }
+        PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
+        data->x += rec[1] + (rec[2] << 8);
+        data->y += rec[3] + (rec[4] << 8);
+        data->z += rec[5] + (rec[6] << 8);
+    }
+    uint8_t r = PIOS_SPI_TransferByte(dev->spi_id, ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT);
+#ifdef PIOS_INCLUDE_INSTRUMENTATION
+    PIOS_Instrumentation_TimeEnd(counterUpd);
+#endif
+    PIOS_ADXL345_ReleaseBus();
+    return r & 0x7F; // return number of remaining entries
+}
 #endif /* PIOS_INCLUDE_ADXL345 */
diff --git a/flight/pios/inc/pios_adxl345.h b/flight/pios/inc/pios_adxl345.h
index 9118784f0..4385a05b3 100644
--- a/flight/pios/inc/pios_adxl345.h
+++ b/flight/pios/inc/pios_adxl345.h
@@ -72,6 +72,7 @@ int32_t PIOS_ADXL345_SelectRate(uint8_t rate);
 int32_t PIOS_ADXL345_SetRange(uint8_t range);
 int32_t PIOS_ADXL345_Init(uint32_t spi_id, uint32_t slave_num);
 uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data *data);
+int8_t PIOS_ADXL345_ReadAndAccumulateSamples(struct pios_adxl345_data *data, uint8_t samples);
 int32_t PIOS_ADXL345_FifoElements();
 int32_t PIOS_ADXL345_Test();