[ASM330LHH] Implemented "onSimpleUpdate"

Compiled, but not tested, implementation of the "onSimpleUpdate" method for ASM330LHH driver.
- Driver should now be able to correctly read data coming from gyroscope, accelerometer and temperature sensor
- Still has to be tested on a properly working IMU

src/entrypoints/asm330lhh_test.cpp

@@ -33,7 +33,15 @@ int main(){
 
     cs.high();
 
-    ASM330LHH sensor(bus, cs);
+    asm330lhh_params params;
+
+    params.accel_odr = ASM330LHH::ODR::_26HZ;
+    params.gyro_odr = ASM330LHH::ODR::_26HZ;
+    params.bdu = ASM330LHH::BDU::UPDATE_AFTER_READ;
+    params.accel_fs = ASM330LHH::ACCEL_FS::_8G;
+    params.gyro_fs = ASM330LHH::GYRO_FS::_250DPS;
+
+    ASM330LHH sensor(bus, cs, params);
     bool success = sensor.init();
 
     if(success){

src/shared/sensors/ASM330LHH/ASM330LHH.h

@@ -25,46 +25,138 @@
 #include "sensors/Sensor.h"
 #include "Common.h"
 #include "drivers/spi/SPIDriver.h"
+#include "ASM330LHHData.h"
 
 using miosix::getTick;
 using miosix::TICK_FREQ;
  
+
+        
+/**
+* @brief IMU parameters:
+*           gyroscope odr (from power down up to 6667Hz)
+*           gyroscope full scale (from +-125dps up to +-4000dps)
+*           accelerometer odr (from power down up do 6667hz)
+*           accelerometer full scale (from +-2g up to +-16g)
+*           bdu (continuous update or update after read)
+*/
+struct asm330lhh_params
+{
+    uint8_t gyro_odr = 0;
+    uint8_t gyro_fs = 0;
+    uint8_t accel_odr = 0;
+    uint8_t accel_fs = 0;
+    uint8_t bdu = 0;
+};
+
 class ASM330LHH : public virtual Sensor
 {
     public:
+
+        /**
+        * @brief Constructor
+        * 
+        * @param bus            the SPI bus
+        * @param chip_select    CS pin
+        * @param params         Sensor params (bdu, accelerometer and gyroscope odr and sensitivity)
+        * 
+        **/
         ASM330LHH(SPIBusInterface& bus,
-                    GpioPin chip_select) : 
-                spi_slave(bus, chip_select, {}) 
+                    GpioPin chip_select, asm330lhh_params params) : 
+                spi_slave(bus, chip_select, {}), params(params) 
         {
             spi_slave.config.clock_div = SPIClockDivider::DIV64;  // used to set the spi baud rate (maximum is 10 Mhz)
         }
 
+        /**
+        * @brief Constructor
+        * 
+        * @param bus            the SPI bus
+        * @param chip_select    CS pin
+        * @param config         SPI bus configuration
+        * @param params         Sensor parameters (bdu, accelerometer and gyroscope odr and sensitivity)
+        * 
+        **/
         ASM330LHH(SPIBusInterface& bus,
                     GpioPin chip_select,
-                    SPIBusConfig config) :
-                spi_slave(bus, chip_select, config) {}
+                    SPIBusConfig config,
+                    asm330lhh_params params) :
+                spi_slave(bus, chip_select, config), params(params) {}
 
         ~ASM330LHH() {}
 
+        /**
+         * @brief Initialize the sensor.
+         * 
+         * @return boolean value indicating whether the operation succeded or not
+         */
         bool init() override
         { 
 
-            miosix::Thread::sleep(10);
+            miosix::Thread::sleep(100);
 
             setup_device();
 
             if(!check_whoami()){
                 TRACE("Whoami check failed!\n");
+                last_error = ERR_NOT_ME;
                 return false;
             }
+
+            setup_accel(params.accel_odr, params.accel_odr);
+            setup_gyro(params.gyro_odr, params.gyro_fs);
+            set_bdu(params.bdu);
+
             return true; 
         }
 
+         /**
+         * @brief Read new data from accelerometer, gyroscope and temperature sensor.
+         *        Accelerometer unit of measure: mg
+         *        Gyroscope unit of measure: mdps
+         *        Temperature sensor unit of measure: °C
+         *        
+         * 
+         * @return boolean value indicating whether the operation succeded or not
+         */
         bool onSimpleUpdate() override
         {
+            uint16_t buffer[14];
+            asm330lhh_data new_data;
+
+            SPITransaction spi(spi_slave);
+            spi.read(OUT_TEMP_L_REG, (uint8_t*) buffer, 14);
+
+            int16_t val = buffer[0] | buffer[1]<<8;
+            new_data.temperature = ((float) val)/TSen + Toff;
+
+            val = buffer[2] | buffer[3]<<8;
+            new_data.gyro_x = ((float) val)*G_So[params.gyro_fs];
+
+            val = buffer[4] | buffer[5]<<8;
+            new_data.gyro_y = ((float) val)*G_So[params.gyro_fs];
+
+            val = buffer[6] | buffer[7]<<8;
+            new_data.gyro_z = ((float) val)*G_So[params.gyro_fs];
+
+            val = buffer[8] | buffer[9]<<8;
+            new_data.accel_x = ((float) val)*LA_So[params.accel_fs];
+
+            val = buffer[10] | buffer[11]<<8;
+            new_data.accel_y = ((float) val)*LA_So[params.accel_fs];
+
+            val = buffer[12] | buffer[13]<<8;
+            new_data.accel_x = ((float) val)*LA_So[params.accel_fs];
+
+            data = new_data;
             return true;
         }
 
+        /**
+        * @brief Check if the sensor is working.
+        * 
+        * @return boolean indicating whether the sensor is correctly working or not
+        */
         bool selfTest() override 
         {  
             // check that the sensor is properly working
@@ -75,9 +167,9 @@ class ASM330LHH : public virtual Sensor
         
         // not a Sensor class method: you should 
         // define methods to get the sampled data
-        float getData() 
+        asm330lhh_data getData() 
         { 
-            return 0; 
+            return data; 
         }
     
     private:
@@ -110,7 +202,6 @@ class ASM330LHH : public virtual Sensor
 
             // Read and update CTRL4_C
             uint8_t ctrl4_c_val = spi.read(CTRL4_C_REG);
-            TRACE("ctrl4_c_val: %u\n", ctrl4_c_val);
             uint8_t i2c_disable_bit = 4;                    // 0b00000100
             ctrl4_c_val |= i2c_disable_bit;
             spi.write(CTRL4_C_REG, ctrl4_c_val);
@@ -118,16 +209,132 @@ class ASM330LHH : public virtual Sensor
 
             // Read and update CTRL9_XL
             uint8_t ctrl9_xl_val = spi.read(CTRL9_XL_REG);
-            TRACE("ctrl9_xl_val: %u\n", ctrl9_xl_val);
             uint8_t device_conf = 2;                        // 0b00000010
             ctrl9_xl_val |= device_conf;
             spi.write(CTRL9_XL_REG, ctrl9_xl_val);
 
         }
 
+        void setup_accel(uint8_t odr, uint8_t fs) {
+            SPITransaction spi(spi_slave);
+
+            assert(odr>=0 && odr<=10);
+            assert(fs>=0 && fs <=3);
+
+            odr = odr & 15;     // 0b1111
+            odr = odr << 4;
+
+            fs = fs & 3;        // 0b11
+            fs = fs << 2;
+
+            uint8_t ctrl1_xl_value = spi.read(CTRL1_XL_REG);
+            ctrl1_xl_value &= 3;                        // 0b00000011
+            ctrl1_xl_value |= odr;
+            ctrl1_xl_value |= fs;
+            spi.write(CTRL1_XL_REG, ctrl1_xl_value);
+        }
+
+        void setup_gyro(uint8_t odr, uint8_t fs) {
+            SPITransaction spi(spi_slave);
+
+            assert(odr>=0 && odr<=10);      //0b1010
+            assert(fs>=0 && fs<=5);
+
+            odr = odr & 15;                 // 0b1111
+            odr = odr << 4;
+
+            // if fs is 125 or 4000 ctrl2_g register values [1:0]
+            // must be set accordingly, otherwise those must be 00
+            if(fs==0)
+                // FS_125 bit = 1
+                // FS_4000 bit = 0
+                fs = 2;    // 0b10
+            else if(fs==5)
+                // FS_125 bit = 0
+                // FS_4000 bit = 1
+                fs = 1;     // 0b01
+            else {
+                // Set ctrl2_g[3:2]
+                fs = fs - 1;
+                fs = fs << 2;
+            }
+
+            uint8_t ctrl2_g_value = spi.read(CTRL2_G_REG);
+            ctrl2_g_value &= 0;
+            ctrl2_g_value |= odr;
+            ctrl2_g_value |= fs;
+            spi.write(CTRL2_G_REG, ctrl2_g_value);
+        }
+
+        void set_bdu(uint8_t bdu){
+            assert(bdu==1 || bdu==0);
+
+            SPITransaction spi(spi_slave);
+
+            uint8_t ctrl3_c_value = spi.read(CTRL3_C_REG);
+            ctrl3_c_value &=0xbf;                       // 0b10111111
+            bdu &= 1;
+            bdu = bdu << 6;
+            ctrl3_c_value |= bdu;
+            spi.write(CTRL3_C_REG, ctrl3_c_value);
+        }
+
+    public:
+
+        /**
+         * @brief Accelerometer possible values for full scale
+        */
+        enum ACCEL_FS {
+            _2G = 0,
+            _16G = 1,
+            _4G = 2,
+            _8G = 3
+        };
+
+        /**
+         * @brief Gyroscope possible values for full scale
+        */
+        enum GYRO_FS {
+            _125DPS = 0,
+            _250DPS = 1,
+            _500DPS = 2,
+            _1000DPS = 3,
+            _2000DPS = 4,
+            _4000DPS = 5,
+        };
+        
+        /**
+         * @brief ODR possible values for both Accelerometer and Gyroscope
+        */
+        enum ODR {
+            _POWER_DOWN = 0,
+            _12_5HZ = 1,
+            _26HZ = 2,
+            _52HZ = 3,
+            _104HZ = 4,
+            _208HZ = 5,
+            _417HZ = 6,
+            _833HZ = 7,
+            _1667HZ = 8,
+            _3333HZ = 9,
+            _6667HZ = 10
+        };
+
+        /**
+         * BDU options: continuous update or update after read
+        */
+        enum BDU {
+            CONTINUOUS_UPDATE = 0,
+            UPDATE_AFTER_READ = 1,
+        };
+    
     // Constant values
     private:
         const uint8_t WHO_AM_I_DEFAULT_VALUE = 0x6b;
+        const float LA_So[4] = {0.061, 0.122, 0.244, 0.488};
+        const float G_So[6] = {4.37, 8.75, 17.5, 35.0, 70.0, 140.0};
+        const float TSen = 256;
+        const float Toff = 25;
 
     private:
 
@@ -136,11 +343,30 @@ class ASM330LHH : public virtual Sensor
          */
         enum REG {
             WHO_AM_I_REG = 0x0f,
+            CTRL1_XL_REG = 0x10,
+            CTRL2_G_REG = 0x11,
+            CTRL3_C_REG = 0x12,
             CTRL4_C_REG = 0x13,
             CTRL9_XL_REG = 0x18,
+            OUT_TEMP_L_REG = 0x20,
+            OUT_TEMP_H_REG = 0X21,
+            OUTX_L_G_REG = 0X22,
+            OUTX_H_G_REG = 0X23,
+            OUTY_L_G_REG = 0X24,
+            OUTY_H_G_REG = 0X25,
+            OUTZ_L_G_REG = 0X26,
+            OUTZ_H_G_REG = 0X27,
+            OUTX_L_A_REG = 0X28,
+            OUTX_H_A_REG = 0X29,
+            OUTY_L_A_REG = 0X2a,
+            OUTY_H_A_REG = 0X2b,
+            OUTZ_L_A_REG = 0X2c,
+            OUTZ_H_A_REG = 0X2d,
         };
 
     private:
+        const asm330lhh_params params;
         SPISlave spi_slave;
+        asm330lhh_data data;
 
 };
\ No newline at end of file

src/shared/sensors/ASM330LHH/ASM330LHHData.h

+#ifndef ASM330LHH_DATA
+#define ASM330LHH_DATA
+
+struct asm330lhh_data {
+    float gyro_x;           // Gyroscope x axis
+    float gyro_y;           // Gyroscope y axis
+    float gyro_z;           // Gyroscope z axis
+    float accel_x;          // Accelerometer x axis
+    float accel_y;          // Accelerometer y axis
+    float accel_z;          // Accelerometer z axis
+    float temperature;       // Temperature
+};
+
+#endif
\ No newline at end of file