diff --git a/src/entrypoints/asm330lhh_test.cpp b/src/entrypoints/asm330lhh_test.cpp
index 87d8f415f60048953e9972184e59e57cda09cbe4..4863a5a125c3bb0ec5fea58e35348367121bfa18 100644
--- a/src/entrypoints/asm330lhh_test.cpp
+++ b/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){
diff --git a/src/shared/sensors/ASM330LHH/ASM330LHH.h b/src/shared/sensors/ASM330LHH/ASM330LHH.h
index 6411cb9a13c57d74d07c5cd2aeddf590e2e7fd13..ebca244c93e40394ca09e1f276e17b5ac694c458 100644
--- a/src/shared/sensors/ASM330LHH/ASM330LHH.h
+++ b/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)
}
- ASM330LHH(SPIBusInterface& bus,
+ /**
+ * @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
diff --git a/src/shared/sensors/ASM330LHH/ASM330LHHData.h b/src/shared/sensors/ASM330LHH/ASM330LHHData.h
new file mode 100644
index 0000000000000000000000000000000000000000..03dae64a1220b0e6fef0fd75c589805ad397a631
--- /dev/null
+++ b/src/shared/sensors/ASM330LHH/ASM330LHHData.h
@@ -0,0 +1,14 @@
+#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
diff --git a/src/shared/sensors/ASM330LHH/ASM330LHH_data.h b/src/shared/sensors/ASM330LHH/ASM330LHH_data.h
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000