Diff: LSM6DSOSensor.cpp

Revision:

5:213250c75a9e

Parent:

4:bcf0cf6e43a7

--- a/LSM6DSOSensor.cpp	Tue Sep 17 08:40:49 2019 +0000
+++ b/LSM6DSOSensor.cpp	Tue Sep 17 12:29:47 2019 +0000
@@ -514,6 +514,389 @@
 }
 
 /**
+ * @brief  Get the LSM6DSO accelerometer sensor high-resolution selection
+ * @param  lpf2_en the pointer where the high-resolution selection is written
+ * @retval 0 in case of success, an error code otherwise
+ */
+int LSM6DSOSensor::get_x_lpf2_en(uint8_t *lpf2_en)
+{
+
+    /* Read actual high-resolution selection from sensor. */
+    if (lsm6dso_xl_filter_lp2_get(&_reg_ctx, lpf2_en) != 0) {
+        return 1;
+    }
+
+    return 0;
+}
+
+/**
+ * @brief  Set the LSM6DSO accelerometer sensor high-resolution selection
+ * @param  lpf2_en the high-resolution selection to be set
+ * @retval 0 in case of success, an error code otherwise
+ */
+int LSM6DSOSensor::set_x_lpf2_en(uint8_t lpf2_en)
+{
+    if (lsm6dso_xl_filter_lp2_set(&_reg_ctx, lpf2_en) != 0) {
+        return 1;
+    }
+
+    return 0;
+}
+
+/**
+ * @brief  Get the LSM6DSO accelerometer sensor filter configuration
+ * @param  hp_slope_xl_en the pointer where the filter selection is written, hpcf_xl the pointer where the filter configuration is written
+ * @retval 0 in case of success, an error code otherwise
+ */
+int LSM6DSOSensor::get_x_filter_config(uint8_t *hp_slope_xl_en, uint8_t *hpcf_xl)
+{
+    int ret = 0;
+    lsm6dso_hp_slope_xl_en_t filter_low_level;
+
+    /* Read actual filter configuration from sensor. */
+    if (lsm6dso_xl_hp_path_on_out_get(&_reg_ctx, &filter_low_level) != 0) {
+        return 1;
+    }
+
+    switch (filter_low_level) {
+        case LSM6DSO_HP_PATH_DISABLE_ON_OUT:
+            *hp_slope_xl_en = 0;
+            *hpcf_xl = 0;
+            break;
+
+        case LSM6DSO_LP_ODR_DIV_10:
+            *hp_slope_xl_en = 0;
+            *hpcf_xl = 1;
+            break;
+        
+        case LSM6DSO_LP_ODR_DIV_20:
+            *hp_slope_xl_en = 0;
+            *hpcf_xl = 2;
+            break;
+
+        case LSM6DSO_LP_ODR_DIV_45:
+            *hp_slope_xl_en = 0;
+            *hpcf_xl = 3;
+            break;
+
+        case LSM6DSO_LP_ODR_DIV_100:
+            *hp_slope_xl_en = 0;
+            *hpcf_xl = 4;
+            break;
+
+        case LSM6DSO_LP_ODR_DIV_200:
+            *hp_slope_xl_en = 0;
+            *hpcf_xl = 5;
+            break;
+
+        case LSM6DSO_LP_ODR_DIV_400:
+            *hp_slope_xl_en = 0;
+            *hpcf_xl = 6;
+            break;
+            
+        case LSM6DSO_LP_ODR_DIV_800:
+            *hp_slope_xl_en = 0;
+            *hpcf_xl = 7;
+            break;
+            
+        case LSM6DSO_SLOPE_ODR_DIV_4:
+            *hp_slope_xl_en = 1;
+            *hpcf_xl = 0;
+            break;
+
+        case LSM6DSO_HP_ODR_DIV_10:
+            *hp_slope_xl_en = 1;
+            *hpcf_xl = 1;
+            break;
+        
+        case LSM6DSO_HP_ODR_DIV_20:
+            *hp_slope_xl_en = 1;
+            *hpcf_xl = 2;
+            break;
+
+        case LSM6DSO_HP_ODR_DIV_45:
+            *hp_slope_xl_en = 1;
+            *hpcf_xl = 3;
+            break;
+
+        case LSM6DSO_HP_ODR_DIV_100:
+            *hp_slope_xl_en = 1;
+            *hpcf_xl = 4;
+            break;
+
+        case LSM6DSO_HP_ODR_DIV_200:
+            *hp_slope_xl_en = 1;
+            *hpcf_xl = 5;
+            break;
+
+        case LSM6DSO_HP_ODR_DIV_400:
+            *hp_slope_xl_en = 1;
+            *hpcf_xl = 6;
+            break;
+            
+        case LSM6DSO_HP_ODR_DIV_800:
+            *hp_slope_xl_en = 1;
+            *hpcf_xl = 7;
+            break;
+
+        default:
+            ret = 1;
+            break;
+    }
+
+    return ret;
+}
+
+/**
+ * @brief  Set the LSM6DSO accelerometer sensor filter configuration
+ * @param  hp_slope_xl_en the filter selection to be set, hpcf_xl the filter configuration to be set
+ * @retval 0 in case of success, an error code otherwise
+ */
+int LSM6DSOSensor::set_x_filter_config(uint8_t hp_slope_xl_en, uint8_t hpcf_xl)
+{
+    uint8_t filter = hpcf_xl + (hp_slope_xl_en << 4);
+    lsm6dso_hp_slope_xl_en_t new_filter;
+
+    new_filter = (filter == 0x00)   ? LSM6DSO_HP_PATH_DISABLE_ON_OUT
+                : (filter == 0x01)  ? LSM6DSO_LP_ODR_DIV_10
+                : (filter == 0x02)  ? LSM6DSO_LP_ODR_DIV_20
+                : (filter == 0x03)  ? LSM6DSO_LP_ODR_DIV_45
+                : (filter == 0x04)  ? LSM6DSO_LP_ODR_DIV_100
+                : (filter == 0x05)  ? LSM6DSO_LP_ODR_DIV_200
+                : (filter == 0x06)  ? LSM6DSO_LP_ODR_DIV_400
+                : (filter == 0x07)  ? LSM6DSO_LP_ODR_DIV_800
+                : (filter == 0x10)  ? LSM6DSO_SLOPE_ODR_DIV_4
+                : (filter == 0x11)  ? LSM6DSO_HP_ODR_DIV_10
+                : (filter == 0x12)  ? LSM6DSO_HP_ODR_DIV_20
+                : (filter == 0x13)  ? LSM6DSO_HP_ODR_DIV_45
+                : (filter == 0x14)  ? LSM6DSO_HP_ODR_DIV_100
+                : (filter == 0x15)  ? LSM6DSO_HP_ODR_DIV_200
+                : (filter == 0x16)  ? LSM6DSO_HP_ODR_DIV_400
+                : (filter == 0x17)  ? LSM6DSO_HP_ODR_DIV_800
+                :                     LSM6DSO_HP_PATH_DISABLE_ON_OUT;
+
+    if (lsm6dso_xl_hp_path_on_out_set(&_reg_ctx, new_filter) != 0) {
+        return 1;
+    }
+
+    return 0;
+}
+
+/**
+ * @brief  Get the LSM6DSO gyroscope sensor power mode
+ * @param  g_hm_mode the pointer where the high-performance mode is written
+ * @retval 0 in case of success, an error code otherwise
+ */
+int LSM6DSOSensor::get_g_power_mode(uint8_t *g_hm_mode)
+{
+    int ret = 0;
+    lsm6dso_g_hm_mode_t power_mode_low_level;
+
+    /* Read actual power mode from sensor. */
+    if (lsm6dso_gy_power_mode_get(&_reg_ctx, &power_mode_low_level) != 0) {
+        return 1;
+    }
+
+    switch (power_mode_low_level) {
+        case LSM6DSO_GY_HIGH_PERFORMANCE:
+            *g_hm_mode = 0;
+            break;
+
+        case LSM6DSO_GY_NORMAL:
+            *g_hm_mode = 1;
+            break;
+
+        default:
+            ret = 1;
+            break;
+    }
+
+    return ret;
+}
+
+/**
+ * @brief  Set the LSM6DSO gyroscope sensor power mode
+ * @param  g_hm_mode the high-performance mode to be set
+ * @retval 0 in case of success, an error code otherwise
+ */
+int LSM6DSOSensor::set_g_power_mode(uint8_t g_hm_mode)
+{
+    lsm6dso_g_hm_mode_t new_power_mode;
+
+    new_power_mode = (g_hm_mode == 0)   ? LSM6DSO_GY_HIGH_PERFORMANCE
+                    :                     LSM6DSO_GY_NORMAL;
+
+    if (lsm6dso_gy_power_mode_set(&_reg_ctx, new_power_mode) != 0) {
+        return 1;
+    }
+
+    return 0;
+}
+
+/**
+ * @brief  Get the LSM6DSO gyroscope sensor low-pass filter configuration
+ * @param  lpf1_sel_g the pointer where the lpf enable is written, ftype the pointer where the filter configuration is written
+ * @retval 0 in case of success, an error code otherwise
+ */
+int LSM6DSOSensor::get_g_lpf_config(uint8_t *lpf1_sel_g, uint8_t *ftype)
+{
+    int ret = 0;
+    lsm6dso_ftype_t ftype_low_level;
+    
+    /* Read actual lpf enable from sensor. */
+    if (lsm6dso_gy_filter_lp1_get(&_reg_ctx, lpf1_sel_g) != 0) {
+        return 1;
+    }
+    
+    /* Read actual filter configuration from sensor. */
+    if (lsm6dso_gy_lp1_bandwidth_get(&_reg_ctx, &ftype_low_level) != 0) {
+        return 1;
+    }
+
+    switch (ftype_low_level) {
+        case LSM6DSO_ULTRA_LIGHT:
+            *ftype = 0;
+            break;
+
+        case LSM6DSO_VERY_LIGHT:
+            *ftype = 1;
+            break;
+        
+        case LSM6DSO_LIGHT:
+            *ftype = 2;
+            break;
+
+        case LSM6DSO_MEDIUM:
+            *ftype = 3;
+            break;
+
+        case LSM6DSO_STRONG:
+            *ftype = 4;
+            break;
+
+        case LSM6DSO_VERY_STRONG:
+            *ftype = 5;
+            break;
+
+        case LSM6DSO_AGGRESSIVE:
+            *ftype = 6;
+            break;
+            
+        case LSM6DSO_XTREME:
+            *ftype = 7;
+            break;
+        
+        default:
+            ret = 1;
+            break;
+    }
+
+    return ret;
+}
+
+/**
+ * @brief  Set the LSM6DSO gyroscope sensor low-pass filter configuration
+ * @param  lpf1_sel_g the lpf enable to be set, ftype the filter configuration to be set
+ * @retval 0 in case of success, an error code otherwise
+ */
+int LSM6DSOSensor::set_g_lpf_config(uint8_t lpf1_sel_g, uint8_t ftype)
+{
+    lsm6dso_ftype_t new_ftype;
+
+    new_ftype = (ftype == 0)    ? LSM6DSO_ULTRA_LIGHT
+                : (ftype == 1)  ? LSM6DSO_VERY_LIGHT
+                : (ftype == 2)  ? LSM6DSO_LIGHT
+                : (ftype == 3)  ? LSM6DSO_MEDIUM
+                : (ftype == 4)  ? LSM6DSO_STRONG
+                : (ftype == 5)  ? LSM6DSO_VERY_STRONG
+                : (ftype == 6)  ? LSM6DSO_AGGRESSIVE
+                :                 LSM6DSO_XTREME;
+
+    if (lsm6dso_gy_filter_lp1_set(&_reg_ctx, lpf1_sel_g) != 0) {
+        return 1;
+    }
+    
+    if (lsm6dso_gy_lp1_bandwidth_set(&_reg_ctx, new_ftype) != 0) {
+        return 1;
+    }
+
+    return 0;
+}
+
+/**
+ * @brief  Get the LSM6DSO gyroscope sensor high-pass filter configuration
+ * @param  hp_en_g the pointer where the hpf enable is written, hpm_g the pointer where the cutoff selection is written
+ * @retval 0 in case of success, an error code otherwise
+ */
+int LSM6DSOSensor::get_g_hpf_config(uint8_t *hp_en_g, uint8_t *hpm_g)
+{
+    int ret = 0;
+    lsm6dso_hpm_g_t hpm_g_low_level;
+    
+    /* Read actual filter configuration from sensor. */
+    if (lsm6dso_gy_hp_path_internal_get(&_reg_ctx, &hpm_g_low_level) != 0) {
+        return 1;
+    }
+
+    switch (hpm_g_low_level) {
+        case LSM6DSO_HP_FILTER_NONE:
+            *hp_en_g = 0;
+            *hpm_g = 0;
+            break;
+        
+        case LSM6DSO_HP_FILTER_16mHz:
+            *hp_en_g = 1;
+            *hpm_g = 0;
+            break;
+        
+        case LSM6DSO_HP_FILTER_65mHz:
+            *hp_en_g = 1;
+            *hpm_g = 1;
+            break;
+        
+        case LSM6DSO_HP_FILTER_260mHz:
+            *hp_en_g = 1;
+            *hpm_g = 2;
+            break;
+        
+        case LSM6DSO_HP_FILTER_1Hz04:
+            *hp_en_g = 1;
+            *hpm_g = 3;
+            break;
+        
+        default:
+            ret = 1;
+            break;
+    }
+
+    return ret;
+}
+
+/**
+ * @brief  Set the LSM6DSO gyroscope sensor high-pass filter configuration
+ * @param  hp_en_g the hpf enable to be set, hpm_g the cutoff selection to be set
+ * @retval 0 in case of success, an error code otherwise
+ */
+int LSM6DSOSensor::set_g_hpf_config(uint8_t hp_en_g, uint8_t hpm_g)
+{
+    uint8_t hpf_config = hpm_g + (hp_en_g << 7);
+    lsm6dso_hpm_g_t new_hpm_g;
+
+    new_hpm_g = (hpf_config == 0x00)    ? LSM6DSO_HP_FILTER_NONE
+                : (hpf_config == 0x80)  ? LSM6DSO_HP_FILTER_16mHz
+                : (hpf_config == 0x81)  ? LSM6DSO_HP_FILTER_65mHz
+                : (hpf_config == 0x82)  ? LSM6DSO_HP_FILTER_260mHz
+                : (hpf_config == 0x83)  ? LSM6DSO_HP_FILTER_1Hz04
+                :                         LSM6DSO_HP_FILTER_NONE;
+    
+    if (lsm6dso_gy_hp_path_internal_set(&_reg_ctx, new_hpm_g) != 0) {
+        return 1;
+    }
+
+    return 0;
+}
+
+/**
  * @brief  Get the LSM6DSO accelerometer sensor raw axes
  * @param  value pointer where the raw values of the axes are written
  * @retval 0 in case of success, an error code otherwise