SAADC keeps printing same value (0 and previous value)

Hi Devzone

I tried to use saadc with 4channels. when I use this without fpu_fft, It prints correct value well.

But, when I insert the fpu_fft and run it, after a while, saadc prints 0 and fixed previous value.

I think the problem is busy CPU.

Is there any solution?

The whole code is below

/**
 * Copyright (c) 2014 - 2019, Nordic Semiconductor ASA
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form, except as embedded into a Nordic
 *    Semiconductor ASA integrated circuit in a product or a software update for
 *    such product, must reproduce the above copyright notice, this list of
 *    conditions and the following disclaimer in the documentation and/or other
 *    materials provided with the distribution.
 *
 * 3. Neither the name of Nordic Semiconductor ASA nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 * 4. This software, with or without modification, must only be used with a
 *    Nordic Semiconductor ASA integrated circuit.
 *
 * 5. Any software provided in binary form under this license must not be reverse
 *    engineered, decompiled, modified and/or disassembled.
 *
 * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */
/**
 * @brief BLE LED Button Service central and client application main file.
 *
 * This example can be a central for up to 8 peripherals.
 * The peripheral is called ble_app_blinky and can be found in the ble_peripheral
 * folder.
 */
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "nordic_common.h"
#include "compiler_abstraction.h"
#include "nrf.h"
#include "app_error.h"
#include "app_uart.h"
#include "nrf_sdh.h"
#include "nrf_sdh_ble.h"
#include "app_timer.h"
#include "bsp_btn_ble.h"
#include "ble.h"
#include "ble_hci.h"
#include "ble_advertising.h"
#include "ble_conn_params.h"
#include "ble_db_discovery.h"
#include "app_util.h"
#include "ble_nus_c.h"
#include "ble_conn_state.h"
#include "nrf_ble_gatt.h"
#include "nrf_pwr_mgmt.h"
#include "nrf_ble_scan.h"
#include "nrf_drv_ppi.h"
#include "nrf_drv_saadc.h"
#include "nrf_drv_timer.h"
#include "nrf_delay.h"
#include "nrf_twi.h"
#include "nrf_drv_twi.h"
#include "nrf_twi_mngr.h"
#include "nrf_twi_sensor.h"
#include "iis2dlpc.h"
#include "boards.h"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"
#include "app_util_platform.h"
#include "bsp.h"
/*lint -save -e689 */ /* Apparent end of comment ignored */
#include "arm_const_structs.h"
/*lint -restore */

#define APP_BLE_CONN_CFG_TAG      1                                             /**< Tag that refers to the BLE stack configuration that is set with @ref sd_ble_cfg_set. The default tag is @ref APP_BLE_CONN_CFG_TAG. */
#define APP_BLE_OBSERVER_PRIO     3                                             /**< BLE observer priority of the application. There is no need to modify this value. */

#define UART_TX_BUF_SIZE        256                                             /**< UART TX buffer size. */
#define UART_RX_BUF_SIZE        256                                             /**< UART RX buffer size. */

#define CENTRAL_SCANNING_LED      BSP_BOARD_LED_0
#define CENTRAL_CONNECTED_LED     BSP_BOARD_LED_1
#define LEDBUTTON_LED             BSP_BOARD_LED_2                               /**< LED to indicate a change of state of the Button characteristic on the peer. */

#define LEDBUTTON_BUTTON          BSP_BUTTON_0                                  /**< Button that writes to the LED characteristic of the peer. */
#define BUTTON_DETECTION_DELAY    APP_TIMER_TICKS(50)                           /**< Delay from a GPIOTE event until a button is reported as pushed (in number of timer ticks). */
#define NUS_SERVICE_UUID_TYPE   BLE_UUID_TYPE_VENDOR_BEGIN                      /**< UUID type for the Nordic UART Service (vendor specific). */
#define ECHOBACK_BLE_UART_DATA  0                                               /**< The flag whether central sends peripheral the received message again */  

#define DIFF_SIZE 10                                                            /**< difference 0~2 arrays size. */
#define II_ADDR (0x33U >>1)                                                     /**< IIS2DLPC Sensor Adress. */
#define MAX_PENDING_TRANSACTIONS    4                                           /**< Maximum numbers of pending transactions. */

#define SAMPLES_IN_BUFFER 4                                                     /**< The number of analog sensors */
#define SAADC_SAMPLE_RATE 250                                                   /**< The sampling rate of saadc */

#define FLAME_PIN NRF_SAADC_INPUT_AIN0                                          /**< The hadware pin for flame detect sensor */
#define SOUND_PIN NRF_SAADC_INPUT_AIN1                                          /**< The hadware pin for mic sensor */
#define HUMID_PIN NRF_SAADC_INPUT_AIN2                                          /**< The hadware pin for soil-moisture sensor */
#define BAT_PIN NRF_SAADC_INPUT_VDD                                             /**< The hadware pin for battery(vdd) */

#define FLAME_CHANNEL 0                                                         /**< The saadc module channel of flame detect sensor */
#define SOUND_CHANNEL 1                                                         /**< The saadc module channel of mic sensor */
#define HUMID_CHANNEL 2                                                         /**< The saadc module channel of soil-moisture sensor */
#define BAT_CHANNEL 3                                                           /**< The saadc module channel of battery(vdd) */

#define ADC_REF_VOLTAGE_IN_MILLIVOLTS   600                                     /**< Reference voltage (in milli volts) used by ADC while doing conversion. */
#define ADC_PRE_SCALING_COMPENSATION    6                                       /**< The ADC is configured to use VDD with 1/3 prescaling as input. And hence the result of conversion is to be multiplied by 3 to get the actual value of the battery voltage.*/
#define DIODE_FWD_VOLT_DROP_MILLIVOLTS  270                                     /**< Typical forward voltage drop of the diode . */
#define ADC_RES_10BIT                   1024                                    /**< Maximum digital value for 10-bit ADC conversion. */

#define ADC_RESULT_IN_MILLI_VOLTS(ADC_VALUE)\
        ((((ADC_VALUE) * ADC_REF_VOLTAGE_IN_MILLIVOLTS) / ADC_RES_10BIT) * ADC_PRE_SCALING_COMPENSATION)    /**<  The mathematical expression for converting the value of saadc(vdd) as milli volts unit. */

#define GRAPH_WINDOW_HEIGHT              53                                     /**< Graph window height used in draw function. */
#define FPU_EXCEPTION_MASK               0x0000009F                             /**< FPU exception mask used to clear exceptions in FPSCR register. */
#define FPU_FPSCR_REG_STACK_OFF          0x40                                   /**< Offset of FPSCR register stacked during interrupt handling in FPU part stack. */

// We want to use 44100 Hz sampling rate to reach 22050Hz band. 128 (64 pairs) samples are used
// in FFT calculation with result contains 64 bins (22050Hz/64bins -> ~344,5Hz per bin).
#define FFT_TEST_SAMPLE_FREQ_HZ          44100.0f                               /**< Frequency of complex input samples. */
//#define FFT_TEST_SAMPLE_FREQ_HZ          42000.0f
#define FFT_TEST_COMP_SAMPLES_LEN        512                                    /**< Complex numbers input data array size. Correspond to FFT calculation this number must be power of two starting from 2^5 (2^4 pairs) with maximum value 2^13 (2^12 pairs). */
#define FFT_TEST_OUT_SAMPLES_LEN         (FFT_TEST_COMP_SAMPLES_LEN / 2)        /**< Output array size. */

#define SIGNALS_RESOLUTION               100.0f                                 /**< Sine wave frequency and noise amplitude resolution. To count resolution as decimal places in number use this formula: resolution = 1/SIGNALS_RESOLUTION. */
#define SINE_WAVE_FREQ_MAX               20000                                  /**< Maximum frequency of generated sine wave. */
#define NOISE_AMPLITUDE                  1                                      /**< Amplitude of generated noise added to signal. */

//#define ACCEL_FFT_PRINT
#define SOUND_FFT_PRINT

#ifdef ACCEL_FFT_PRINT
#define Y_PRINT
//#define TIMECHECK                                                               /**< The flag of checking execution time. */
#ifdef X_PRINT
#define OUTPUT_PORT x_output
#endif
#ifdef Y_PRINT
#define OUTPUT_PORT y_output
#endif
#ifdef Z_PRINT
#define OUTPUT_PORT z_output
#endif
#endif

#ifdef SOUND_FFT_PRINT
#define OUTPUT_PORT sound_output
#endif

NRF_BLE_GATT_DEF(m_gatt);                                                       /**< GATT module instance. */
BLE_NUS_C_ARRAY_DEF(m_ble_nus_c, NRF_SDH_BLE_TOTAL_LINK_COUNT);
BLE_DB_DISCOVERY_ARRAY_DEF(m_db_disc, NRF_SDH_BLE_TOTAL_LINK_COUNT);            /**< Database discovery module instances. */
NRF_BLE_SCAN_DEF(m_scan);                                                       /**< Scanning Module instance. */
NRF_TWI_MNGR_DEF(m_nrf_twi_mngr, MAX_PENDING_TRANSACTIONS, 0);                  /**< TWI transaction manager instance. */
NRF_TWI_SENSOR_DEF(m_nrf_twi_sensor, &m_nrf_twi_mngr, NRF_TWI_SENSOR_SEND_BUF_SIZE);                        /**< TWI sensor instance. */


static nrf_saadc_channel_config_t ch_config_flame = NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(FLAME_PIN);     /**< The saadc cnannel configuration of flame detect sensor. */
static nrf_saadc_channel_config_t ch_config_sound = NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(SOUND_PIN);     /**< The saadc cnannel configuration of mic sensor. */
static nrf_saadc_channel_config_t ch_config_humid = NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(HUMID_PIN);     /**< The saadc cnannel configuration of soil-moisture sensor. */
static nrf_saadc_channel_config_t ch_config_bat = NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(BAT_PIN);         /**< The saadc cnannel configuration of battery(vdd). */
static const nrf_drv_timer_t m_timer = NRF_DRV_TIMER_INSTANCE(3);               /**< The channel of used timer triver. */
static char const m_target_periph_name[] = "Nordic_Blinky";                     /**< Name of the device to try to connect to. This name is searched for in the scanning report data. */
static axis3bit16_t          data_raw_acceleration;                             /**< Load raw data from iis2dlpc sensor into this variable. */ 
static iis2dlpc_ctx_t        dev_ctx;                                           /**< Handle iis2dlpc sensor's read/write instance with this variable. */
static uint8_t               whoamI, rst;                                       /**< Validate iis2dlpc sensor address and status to use. */ 
static float acceleration_mg[3];                                                /**< Convert raw data to float(usable) data into this variable by using iis2dlpc sdk. */
static int current_value[3];                                                    /**< Current data to use dto62 private function. */
static int previous_value[3] = {0};                                             /**< Previous data to use dto62 private function. */
static char difference0[DIFF_SIZE];                                             /**< X-axis Difference between Previous data and Current data converted by dto62 private function. */
static char difference1[DIFF_SIZE];                                             /**< Y-axis Difference between Previous data and Current data converted by dto62 private function. */
static char difference2[DIFF_SIZE];                                             /**< Z-axis Difference between Previous data and Current data converted by dto62 private function. */
static char m_buffer[DIFF_SIZE*3];                                              /**< Buffer to print all the difference data. */

static float calib_offset[3];

static nrf_saadc_value_t m_buffer_pool[2][SAMPLES_IN_BUFFER];                                      /**< The storage of 4 values which are results of saadc. */
static nrf_ppi_channel_t     m_ppi_channel;                                     /**< The ppi instance for interconnecting timer and saadc. */
static uint16_t m_ble_nus_max_data_len = BLE_GATT_ATT_MTU_DEFAULT - OPCODE_LENGTH - HANDLE_LENGTH;          /**< The maximum length of ble data. */
static uint8_t m_ble_nus_c_count;                                               /**< The instance of present connected count of ble devices. */

static uint16_t accel_counter = 0;                                       /**< The counter used for counting the excution counts of iss2dlpc. */
static uint16_t sound_counter = 0;                                       /**< The counter used for checking excution time. */

static uint32_t  m_ifft_flag             = 0;                            /**< Flag that selects forward (0) or inverse (1) transform. */
static uint32_t  m_do_bit_reverse        = 1;                            /**< Flag that enables (1) or disables (0) bit reversal of output. */
//static float32_t m_fft_input_f32[FFT_TEST_COMP_SAMPLES_LEN];             /**< FFT input array. Time domain. */
//static float32_t m_fft_output_f32[FFT_TEST_OUT_SAMPLES_LEN];             /**< FFT output data. Frequency domain. */

#ifdef TIMECHECK
static uint16_t counter = 0;                                             /**< The counter used for checking excution time. */
static uint32_t start;                                                          /**< The time recorded when program starts. */
static uint32_t stop;                                                           /**< The time recorded when program terminates. */
static uint32_t elapsed;                                                        /**< The execution time. */
#endif

typedef struct
{
    float32_t   x_input[FFT_TEST_COMP_SAMPLES_LEN];
    float32_t   y_input[FFT_TEST_COMP_SAMPLES_LEN];
    float32_t   z_input[FFT_TEST_COMP_SAMPLES_LEN];
    float32_t   sound_input[FFT_TEST_COMP_SAMPLES_LEN];
} m_fft_input_f32;

typedef struct
{
    float32_t   x_output[FFT_TEST_OUT_SAMPLES_LEN];
    float32_t   y_output[FFT_TEST_OUT_SAMPLES_LEN];
    float32_t   z_output[FFT_TEST_OUT_SAMPLES_LEN];
    float32_t   sound_output[FFT_TEST_OUT_SAMPLES_LEN];
} m_fft_output_f32;

static m_fft_input_f32 fft_input;
static m_fft_output_f32 fft_output;

static ble_uuid_t const m_nus_uuid =                                            /**< Universally unique service identifiers. */
{
    .uuid = BLE_UUID_DEVICE_INFORMATION_SERVICE,
    .type = BLE_UUID_TYPE_BLE
};


/**@brief Function for handling asserts in the SoftDevice.
 *
 * @details This function is called in case of an assert in the SoftDevice.
 *
 * @warning This handler is only an example and is not meant for the final product. You need to analyze
 *          how your product is supposed to react in case of an assert.
 * @warning On assert from the SoftDevice, the system can only recover on reset.
 *
 * @param[in] line_num     Line number of the failing assert call.
 * @param[in] p_file_name  File name of the failing assert call.
 */
void assert_nrf_callback(uint16_t line_num, const uint8_t * p_file_name)
{
    app_error_handler(0xDEADBEEF, line_num, p_file_name);
}


/**@brief Function for initializing the LEDs.
 *
 * @details Initializes all LEDs used by the application.
 */
static void leds_init(void)
{
    bsp_board_init(BSP_INIT_LEDS);
}

/**@brief Function for handling Queued Write Module errors.
 *
 * @details A pointer to this function will be passed to each service which may need to inform the
 *          application about an error.
 *
 * @param[in]   nrf_error   Error code containing information about what went wrong.
 */
static void scan_evt_handler(scan_evt_t const * p_scan_evt)
{
    ret_code_t err_code;

    switch(p_scan_evt->scan_evt_id)
    {
        case NRF_BLE_SCAN_EVT_CONNECTING_ERROR:
        {
            err_code = p_scan_evt->params.connecting_err.err_code;
            APP_ERROR_CHECK(err_code);
        } break;

        default:
            break;
    }
}


/**@brief Function for initializing the scanning and setting the filters. */
static void scan_init(void)
{
    ret_code_t          err_code;
    nrf_ble_scan_init_t init_scan;

    memset(&init_scan, 0, sizeof(init_scan));

    init_scan.connect_if_match = true;
    init_scan.conn_cfg_tag     = APP_BLE_CONN_CFG_TAG;

    err_code = nrf_ble_scan_init(&m_scan, &init_scan, scan_evt_handler);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_scan_filter_set(&m_scan, SCAN_UUID_FILTER, &m_nus_uuid);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_scan_filters_enable(&m_scan, NRF_BLE_SCAN_UUID_FILTER, false);
    APP_ERROR_CHECK(err_code);
}

/**@brief Function for starting scanning. */
static void scan_start(void)
{
    ret_code_t ret;
    
    sd_ble_gap_scan_stop();
    NRF_LOG_INFO("Start scanning for device name %s.", (uint32_t)m_target_periph_name);
    ret = nrf_ble_scan_start(&m_scan);
    APP_ERROR_CHECK(ret);
    // Turn on the LED to signal scanning.
    bsp_board_led_on(CENTRAL_SCANNING_LED);
}

static void ble_nus_chars_received_uart_print(uint8_t * p_data, uint16_t data_len)
{
    ret_code_t ret_val;

    NRF_LOG_DEBUG("Receiving data.");
    NRF_LOG_HEXDUMP_DEBUG(p_data, data_len);

    for (uint32_t i = 0; i < data_len; i++)
    {
        do
        {
            ret_val = app_uart_put(p_data[i]);
            if ((ret_val != NRF_SUCCESS) && (ret_val != NRF_ERROR_BUSY))
            {
                NRF_LOG_ERROR("app_uart_put failed for index 0x%04x.", i);
                APP_ERROR_CHECK(ret_val);
            }
        } while (ret_val == NRF_ERROR_BUSY);
    }
    if (p_data[data_len-1] == '\r')
    {
        while (app_uart_put('\n') == NRF_ERROR_BUSY);
    }
    /*
    *It can be used later, so it wasn't removed.

    if (ECHOBACK_BLE_UART_DATA)
    {
        // Send data back to the peripheral.
        do
        {
            for (uint32_t i = 0; i < NRF_SDH_BLE_CENTRAL_LINK_COUNT; i++)
            {
                ret_val = ble_nus_c_string_send(&m_ble_nus_c, p_data, data_len);
                if ((ret_val != NRF_SUCCESS) && (ret_val != NRF_ERROR_BUSY))
                {
                    NRF_LOG_ERROR("Failed sending NUS message. Error 0x%x. ", ret_val);
                    APP_ERROR_CHECK(ret_val);
                }
            }
        } while (ret_val == NRF_ERROR_BUSY);
    }
    */
    
}


/**@brief   Function for handling app_uart events.
 *
 * @details This function receives a single character from the app_uart module and appends it to
 *          a string. The string is sent over BLE when the last character received is a
 *          'new line' '\n' (hex 0x0A) or if the string reaches the maximum data length.
 */
void uart_event_handle(app_uart_evt_t * p_event)
{
    static uint8_t data_array[BLE_NUS_MAX_DATA_LEN];
    static uint16_t index = 0;
    uint32_t ret_val;

    switch (p_event->evt_type)
    {
        /**@snippet [Handling data from UART] */
        case APP_UART_DATA_READY:
            UNUSED_VARIABLE(app_uart_get(&data_array[index]));
            index++;

            if ((data_array[index - 1] == '\n') || (index >= (m_ble_nus_max_data_len)))
            {
                NRF_LOG_DEBUG("Ready to send data over BLE NUS");
                NRF_LOG_HEXDUMP_DEBUG(data_array, index);

                do
                {
                    ret_val = ble_nus_c_string_send(&m_ble_nus_c, data_array, index);
                    if ( (ret_val != NRF_ERROR_INVALID_STATE) && (ret_val != NRF_ERROR_RESOURCES) )
                    {
                        APP_ERROR_CHECK(ret_val);
                    }
                } while (ret_val == NRF_ERROR_RESOURCES);

                index = 0;
            }
            break;

        /**@snippet [Handling data from UART] */
        case APP_UART_COMMUNICATION_ERROR:
            NRF_LOG_ERROR("Communication error occurred while handling UART.");
            APP_ERROR_HANDLER(p_event->data.error_communication);
            break;

        case APP_UART_FIFO_ERROR:
            NRF_LOG_ERROR("Error occurred in FIFO module used by UART.");
            APP_ERROR_HANDLER(p_event->data.error_code);
            break;

        default:
            break;
    }
}


/**@brief Callback handling Nordic UART Service (NUS) client events.
 *
 * @details This function is called to notify the application of NUS client events.
 *
 * @param[in]   p_ble_nus_c   NUS client handle. This identifies the NUS client.
 * @param[in]   p_ble_nus_evt Pointer to the NUS client event.
 */

/**@snippet [Handling events from the ble_nus_c module] */
static void ble_nus_c_evt_handler(ble_nus_c_t * p_ble_nus_c, ble_nus_c_evt_t const * p_ble_nus_evt)
{
    ret_code_t err_code;

    switch (p_ble_nus_evt->evt_type)
    {
        case BLE_NUS_C_EVT_DISCOVERY_COMPLETE:
            NRF_LOG_INFO("Discovery complete.");
            err_code = ble_nus_c_tx_notif_enable(p_ble_nus_c);
            APP_ERROR_CHECK(err_code);
            NRF_LOG_INFO("Connected to device with Nordic UART Service.");

            if(err_code != NRF_ERROR_BUSY)
            {
                APP_ERROR_CHECK(err_code);
            }
            break;

        case BLE_NUS_C_EVT_NUS_TX_EVT:
            printf(p_ble_nus_evt->p_data);
            break;

        case BLE_NUS_C_EVT_DISCONNECTED:
            NRF_LOG_INFO("Disconnected.");
            scan_start();
            break;
    }
}

/**
 * @brief Function for handling shutdown events.
 *
 * @param[in]   event       Shutdown type.
 */
static bool shutdown_handler(nrf_pwr_mgmt_evt_t event)
{
    ret_code_t err_code;

    err_code = bsp_indication_set(BSP_INDICATE_IDLE);
    APP_ERROR_CHECK(err_code);

    switch (event)
    {
        case NRF_PWR_MGMT_EVT_PREPARE_WAKEUP:
            // Prepare wakeup buttons.
            err_code = bsp_btn_ble_sleep_mode_prepare();
            APP_ERROR_CHECK(err_code);
            break;

        default:
            break;
    }

    return true;
}

NRF_PWR_MGMT_HANDLER_REGISTER(shutdown_handler, APP_SHUTDOWN_HANDLER_PRIORITY);

/**@brief Function for initializing nRF uart service that will be used by ble.
 */
static void nus_c_init(void)
{
    ret_code_t       err_code;
    ble_nus_c_init_t init;

    init.evt_handler = ble_nus_c_evt_handler;

    for (m_ble_nus_c_count = 0; m_ble_nus_c_count < NRF_SDH_BLE_TOTAL_LINK_COUNT; m_ble_nus_c_count++)
    {
        err_code = ble_nus_c_init(&m_ble_nus_c[m_ble_nus_c_count], &init);
        APP_ERROR_CHECK(err_code);
    }
    m_ble_nus_c_count = 0;
}

/**@brief Function for handling BLE events.
 *
 * @param[in]   p_ble_evt   Bluetooth stack event.
 * @param[in]   p_context   Unused.
 */
static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
{
    ret_code_t err_code;

    // For readability.
    ble_gap_evt_t const * p_gap_evt = &p_ble_evt->evt.gap_evt;

    switch (p_ble_evt->header.evt_id)
    {
        // Upon connection, check which peripheral is connected, initiate DB
        // discovery, update LEDs status, and resume scanning, if necessary.
        case BLE_GAP_EVT_CONNECTED:
        {
            NRF_LOG_INFO("Connection 0x%x established, starting DB discovery.",
                         p_gap_evt->conn_handle);

            APP_ERROR_CHECK_BOOL(p_gap_evt->conn_handle < NRF_SDH_BLE_CENTRAL_LINK_COUNT);
            err_code = ble_nus_c_handles_assign(&m_ble_nus_c[p_gap_evt->conn_handle], p_gap_evt->conn_handle, NULL);
            APP_ERROR_CHECK(err_code);
            err_code = ble_db_discovery_start(&m_db_disc[p_gap_evt->conn_handle],
                                              p_gap_evt->conn_handle);
            if (err_code != NRF_ERROR_BUSY)
            {
                APP_ERROR_CHECK(err_code);
            }

            // Update LEDs status and check whether it is needed to look for more
            // peripherals to connect to.
            bsp_board_led_on(CENTRAL_CONNECTED_LED);
            if (ble_conn_state_central_conn_count() == NRF_SDH_BLE_CENTRAL_LINK_COUNT)
            {
                bsp_board_led_off(CENTRAL_SCANNING_LED);
            }
            else
            {
                // Resume scanning.
                bsp_board_led_on(CENTRAL_SCANNING_LED);
                scan_start();
            }
        } break; // BLE_GAP_EVT_CONNECTED

        // Upon disconnection, reset the connection handle of the peer that disconnected, update
        // the LEDs status and start scanning again.
        case BLE_GAP_EVT_DISCONNECTED:
        {
            NRF_LOG_INFO("LBS central link 0x%x disconnected (reason: 0x%x)",
                         p_gap_evt->conn_handle,
                         p_gap_evt->params.disconnected.reason);

            if (ble_conn_state_central_conn_count() == 0)
            {
                // Turn off the LED that indicates the connection.
                bsp_board_led_off(CENTRAL_CONNECTED_LED);
            }

            // Start scanning.
            scan_start();

            // Turn on the LED for indicating scanning.
            bsp_board_led_on(CENTRAL_SCANNING_LED);

        } break;
        
        case BLE_GAP_EVT_TIMEOUT:
        {
            // Timeout for scanning is not specified, so only the connection requests can time out.
            if (p_gap_evt->params.timeout.src == BLE_GAP_TIMEOUT_SRC_CONN)
            {
                scan_start();
                NRF_LOG_DEBUG("Connection request timed out.");
            }
        } break;

        case BLE_GAP_EVT_CONN_PARAM_UPDATE_REQUEST:
        {
            NRF_LOG_DEBUG("BLE_GAP_EVT_CONN_PARAM_UPDATE_REQUEST.");
            // Accept parameters requested by peer.
            err_code = sd_ble_gap_conn_param_update(p_gap_evt->conn_handle,
                                        &p_gap_evt->params.conn_param_update_request.conn_params);
            APP_ERROR_CHECK(err_code);
        } break;

        case BLE_GAP_EVT_PHY_UPDATE_REQUEST:
        {
            NRF_LOG_DEBUG("PHY update request.");
            ble_gap_phys_t const phys =
            {
                .rx_phys = BLE_GAP_PHY_AUTO,
                .tx_phys = BLE_GAP_PHY_AUTO,
            };
            err_code = sd_ble_gap_phy_update(p_ble_evt->evt.gap_evt.conn_handle, &phys);
            APP_ERROR_CHECK(err_code);
        } break;

        case BLE_GATTC_EVT_TIMEOUT:
        {
            // Disconnect on GATT client timeout event.
            NRF_LOG_DEBUG("GATT client timeout.");
            err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gattc_evt.conn_handle,
                                             BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
            APP_ERROR_CHECK(err_code);
        } break;

        case BLE_GATTS_EVT_TIMEOUT:
        {
            // Disconnect on GATT server timeout event.
            NRF_LOG_DEBUG("GATT server timeout.");
            err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gatts_evt.conn_handle,
                                             BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
            APP_ERROR_CHECK(err_code);
        } break;

        default:
            // No implementation needed.
            break;
    }
}


/**@brief Function for initializing the BLE stack.
 *
 * @details Initializes the SoftDevice and the BLE event interrupts.
 */
static void ble_stack_init(void)
{
    ret_code_t err_code;

    err_code = nrf_sdh_enable_request();
    APP_ERROR_CHECK(err_code);

    // Configure the BLE stack using the default settings.
    // Fetch the start address of the application RAM.
    uint32_t ram_start = 0;
    err_code = nrf_sdh_ble_default_cfg_set(APP_BLE_CONN_CFG_TAG, &ram_start);
    APP_ERROR_CHECK(err_code);

    // Enable BLE stack.
    err_code = nrf_sdh_ble_enable(&ram_start);
    APP_ERROR_CHECK(err_code);

    // Register a handler for BLE events.
    NRF_SDH_BLE_OBSERVER(m_ble_observer, APP_BLE_OBSERVER_PRIO, ble_evt_handler, NULL);
}



/**@brief Function for handling database discovery events.
 *
 * @details This function is a callback function to handle events from the database discovery module.
 *          Depending on the UUIDs that are discovered, this function forwards the events
 *          to their respective services.
 *
 * @param[in] p_event  Pointer to the database discovery event.
 */
static void db_disc_handler(ble_db_discovery_evt_t * p_evt)
{
    ble_nus_c_on_db_disc_evt(&m_ble_nus_c[p_evt->conn_handle], p_evt);
}


/** @brief Database discovery initialization.
 */
static void db_discovery_init(void)
{
    ret_code_t err_code = ble_db_discovery_init(db_disc_handler);
    APP_ERROR_CHECK(err_code);
}


/**@brief Function for initializing power management.
 */
static void power_management_init(void)
{
    ret_code_t err_code;
    err_code = nrf_pwr_mgmt_init();
    APP_ERROR_CHECK(err_code);
}


/**@brief Function for handling the idle state (main loop).
 *
 * @details This function handles any pending log operations, then sleeps until the next event occurs.
 */
static void idle_state_handle(void)
{
    if (NRF_LOG_PROCESS() == false)
    {
        nrf_pwr_mgmt_run();
    }
}


/** @brief Function for initializing the log module.
 */
static void log_init(void)
{
    ret_code_t err_code = NRF_LOG_INIT(NULL);
    APP_ERROR_CHECK(err_code);

    NRF_LOG_DEFAULT_BACKENDS_INIT();
}


/** @brief Function for initializing the timer.
 */
static void timer_init(void)
{
    ret_code_t err_code = app_timer_init();
    APP_ERROR_CHECK(err_code);
}

/**@brief Application main function.
 */
void timer_handler(nrf_timer_event_t event_type, void * p_context)
{

}

/**@brief Function for initializing the GATT module.
 */
void gatt_evt_handler(nrf_ble_gatt_t * p_gatt, nrf_ble_gatt_evt_t const * p_evt)
{
    if (p_evt->evt_id == NRF_BLE_GATT_EVT_ATT_MTU_UPDATED)
    {
        NRF_LOG_INFO("ATT MTU exchange completed.");

        m_ble_nus_max_data_len = p_evt->params.att_mtu_effective - OPCODE_LENGTH - HANDLE_LENGTH;
        NRF_LOG_INFO("Ble NUS max data length set to 0x%X(%d)", m_ble_nus_max_data_len, m_ble_nus_max_data_len);
    }
}

/**@brief Function for initializing the GATT library. */
void gatt_init(void)
{
    ret_code_t err_code;

    err_code = nrf_ble_gatt_init(&m_gatt, gatt_evt_handler);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_gatt_att_mtu_central_set(&m_gatt, NRF_SDH_BLE_GATT_MAX_MTU_SIZE);
    APP_ERROR_CHECK(err_code);
}

/**@snippet [Handling the data received over UART] */
/**@brief  Function for initializing the UART module.
 */
static void uart_init(void)
{
    ret_code_t err_code;

    app_uart_comm_params_t const comm_params =
    {
        .rx_pin_no    = RX_PIN_NUMBER,
        .tx_pin_no    = TX_PIN_NUMBER,
        .rts_pin_no   = RTS_PIN_NUMBER,
        .cts_pin_no   = CTS_PIN_NUMBER,
        .flow_control = APP_UART_FLOW_CONTROL_DISABLED,
        .use_parity   = false,
        .baud_rate    = UART_BAUDRATE_BAUDRATE_Baud115200
    };

    APP_UART_FIFO_INIT(&comm_params,
                       UART_RX_BUF_SIZE,
                       UART_TX_BUF_SIZE,
                       uart_event_handle,
                       APP_IRQ_PRIORITY_LOWEST,
                       err_code);

    APP_ERROR_CHECK(err_code);
}
/**@snippet [UART Initialization] */


/**@brief Function for initializing the sampling event of saadc(Successive approximation analog-to-digital converter)
 */
void saadc_sampling_event_init(void)
{
    ret_code_t err_code;

    err_code = nrf_drv_ppi_init();
    APP_ERROR_CHECK(err_code);

    nrf_drv_timer_config_t timer_cfg = NRF_DRV_TIMER_DEFAULT_CONFIG;
    timer_cfg.frequency = NRF_TIMER_FREQ_31250Hz;
    err_code = nrf_drv_timer_init(&m_timer, &timer_cfg, timer_handler);
    APP_ERROR_CHECK(err_code);

    /* setup m_timer for compare event every 400ms */
    uint32_t ticks = nrf_drv_timer_ms_to_ticks(&m_timer, SAADC_SAMPLE_RATE);
    nrf_drv_timer_extended_compare(&m_timer,
                                   NRF_TIMER_CC_CHANNEL2,
                                   ticks,
                                   NRF_TIMER_SHORT_COMPARE2_CLEAR_MASK,
                                   false);
    nrf_drv_timer_enable(&m_timer);

    uint32_t timer_compare_event_addr = nrf_drv_timer_compare_event_address_get(&m_timer,
                                                                                NRF_TIMER_CC_CHANNEL2);
    uint32_t saadc_sample_task_addr   = nrf_drv_saadc_sample_task_get();

    /* setup ppi channel so that timer compare event is triggering sample task in SAADC */
    err_code = nrf_drv_ppi_channel_alloc(&m_ppi_channel);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_drv_ppi_channel_assign(m_ppi_channel,
                                          timer_compare_event_addr,
                                          saadc_sample_task_addr);
    APP_ERROR_CHECK(err_code);
}

/**@brief Function for enabling sampling event of saadc
 */
void saadc_sampling_event_enable(void)
{
    ret_code_t err_code = nrf_drv_ppi_channel_enable(m_ppi_channel);

    APP_ERROR_CHECK(err_code);
}

/**@brief Function for handling the event of saadc sampling result
 *
 * @param[in]   event   Event generated by analog-to-digital convert.
 */
void saadc_callback(nrf_drv_saadc_evt_t const * p_event)
{
    if (p_event->type == NRF_DRV_SAADC_EVT_DONE)
    {
        ret_code_t err_code;
        nrf_saadc_value_t bat_result;
        uint16_t          batt_lvl_in_milli_volts;
        uint8_t           percentage_batt_lvl;

        char saadc_temp[30];
        static uint8_t nus_temp2[30];

        err_code = nrf_drv_saadc_buffer_convert(p_event->data.done.p_buffer, SAMPLES_IN_BUFFER);
        APP_ERROR_CHECK(err_code);

        bat_result = p_event->data.done.p_buffer[BAT_CHANNEL];

        batt_lvl_in_milli_volts = ADC_RESULT_IN_MILLI_VOLTS(bat_result) +
                                  DIODE_FWD_VOLT_DROP_MILLIVOLTS;
        percentage_batt_lvl = battery_level_in_percent(batt_lvl_in_milli_volts);

//        sprintf(saadc_temp, "master : %d %d %d %d", p_event->data.done.p_buffer[FLAME_CHANNEL], p_event->data.done.p_buffer[SOUND_CHANNEL], p_event->data.done.p_buffer[HUMID_CHANNEL], percentage_batt_lvl);
//        printf("%s\n", saadc_temp);
//        printf("%d\n",p_event->data.done.p_buffer[FLAME_CHANNEL]);

        #ifdef SOUND_FFT_PRINT
        sound_counter++;

        fft_generate_sound_samples(&fft_input,
                             p_event->data.done.p_buffer[SOUND_CHANNEL]);

        if(sound_counter == FFT_TEST_OUT_SAMPLES_LEN)
        {
            fft_process(&fft_input,
            &arm_cfft_sR_f32_len256,
            &fft_output,
            FFT_TEST_OUT_SAMPLES_LEN);
            sound_counter = 0;
//            draw_fft_data(&fft_output, FFT_TEST_OUT_SAMPLES_LEN, GRAPH_WINDOW_HEIGHT);
            nrf_delay_ms(1000);
        }
        #endif
   }
}

/**@brief Function for initializing the saadc
 */
void saadc_init(void)
{
    ret_code_t err_code;

    nrf_drv_saadc_config_t saadc_config = NRF_DRV_SAADC_DEFAULT_CONFIG;
    saadc_config.resolution = NRF_SAADC_RESOLUTION_12BIT;

    ch_config_flame.gain = NRF_SAADC_GAIN1_4;
    ch_config_flame.reference = NRF_SAADC_REFERENCE_VDD4;
    ch_config_sound.gain = NRF_SAADC_GAIN1_4;
    ch_config_sound.reference = NRF_SAADC_REFERENCE_VDD4;
    ch_config_humid.gain = NRF_SAADC_GAIN1_4;
    ch_config_humid.reference = NRF_SAADC_REFERENCE_VDD4;
    ch_config_bat.gain = NRF_SAADC_GAIN1_4;
    ch_config_flame.reference = NRF_SAADC_REFERENCE_VDD4;

    err_code = nrf_drv_saadc_init(&saadc_config, saadc_callback);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_drv_saadc_channel_init(FLAME_CHANNEL, &ch_config_flame);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_drv_saadc_channel_init(SOUND_CHANNEL, &ch_config_sound);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_drv_saadc_channel_init(HUMID_CHANNEL, &ch_config_humid);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_drv_saadc_channel_init(BAT_CHANNEL, &ch_config_bat);
    APP_ERROR_CHECK(err_code);

    err_code = nrfx_saadc_buffer_convert(m_buffer_pool[0], SAMPLES_IN_BUFFER);
    APP_ERROR_CHECK(err_code);

    err_code = nrfx_saadc_buffer_convert(m_buffer_pool[1], SAMPLES_IN_BUFFER);
    APP_ERROR_CHECK(err_code);
}

void fft_generate_sound_samples(m_fft_input_f32 * p_input,
                                 int16_t  input_data)
{
      p_input->sound_input[sound_counter*2] = (float)input_data*0.0001;
      printf("%lf\n", p_input->sound_input[sound_counter*2]);
      p_input->sound_input[sound_counter*2 + 1] = 0;
}
                                 

static void fft_generate_samples(m_fft_input_f32 * p_input,
                                 float*   input_data)
{
      #ifdef ACCEL_FFT_PRINT
        // Real part.
      p_input->x_input[accel_counter*2] = input_data[0]-calib_offset[0];
      p_input->y_input[accel_counter*2] = input_data[1]-calib_offset[1];
      p_input->z_input[accel_counter*2] = input_data[2]-calib_offset[2];
//      printf("%lf\n", p_input[accel_counter*2]);

        // Img part.
      p_input->x_input[accel_counter*2 + 1] = 0;
      p_input->y_input[accel_counter*2 + 1] = 0;
      p_input->z_input[accel_counter*2 + 1] = 0;
      #endif
}


/**
 * @brief Function for processing generated sine wave samples.
 * @param[in] p_input        Pointer to input data array with complex number samples in time domain.
 * @param[in] p_input_struct Pointer to cfft instance structure describing input data.
 * @param[out] p_output      Pointer to processed data (bins) array in frequency domain.
 * @param[in] output_size    Processed data array size.
 */
void fft_process(m_fft_input_f32 *                   p_input,
                        const arm_cfft_instance_f32 * p_input_struct,
                        m_fft_output_f32 *                   p_output,
                        uint16_t                      output_size)
{
    #ifdef ACCEL_FFT_PRINT
    // Use CFFT module to process the data.
    arm_cfft_f32(p_input_struct, p_input->x_input, m_ifft_flag, m_do_bit_reverse);
    // Calculate the magnitude at each bin using Complex Magnitude Module function.
    arm_cmplx_mag_f32(p_input->x_input, p_output->x_output, output_size);

    arm_cfft_f32(p_input_struct, p_input->y_input, m_ifft_flag, m_do_bit_reverse);
    arm_cmplx_mag_f32(p_input->y_input, p_output->y_output, output_size);

    arm_cfft_f32(p_input_struct, p_input->z_input, m_ifft_flag, m_do_bit_reverse);
    arm_cmplx_mag_f32(p_input->z_input, p_output->z_output, output_size);
    #endif

    #ifdef SOUND_FFT_PRINT
    arm_cfft_f32(p_input_struct, p_input->sound_input, m_ifft_flag, m_do_bit_reverse);
    arm_cmplx_mag_f32(p_input->sound_input, p_output->sound_output, output_size);
    #endif
}

#ifdef FPU_INTERRUPT_MODE
/**
 * @brief FPU Interrupt handler. Clearing exception flag at the stack.
 *
 * Function clears exception flag in FPSCR register and at the stack. During interrupt handler
 * execution FPU registers might be copied to the stack (see lazy stacking option) and
 * it is necessary to clear data at the stack which will be recovered in the return from
 * interrupt handling.
 */
void FPU_IRQHandler(void)
{
    // Prepare pointer to stack address with pushed FPSCR register.
    uint32_t * fpscr = (uint32_t * )(FPU->FPCAR + FPU_FPSCR_REG_STACK_OFF);
    // Execute FPU instruction to activate lazy stacking.
    (void)__get_FPSCR();
    // Clear flags in stacked FPSCR register.
    *fpscr = *fpscr & ~(FPU_EXCEPTION_MASK);
}
#endif

/**
 * @brief Function for drawing line with given width.
 * @param[in] line_width Line width.
 */
static void draw_line(uint16_t line_width)
{
    uint32_t i;
    char     line[(int)(line_width/2) + 1];

    for (i = 0; i < (int)(line_width/2); i++)
    {
        line[i] = '-';
    }
    line[(int)(line_width/2)] = 0;
    NRF_LOG_RAW_INFO("%s\r\n", nrf_log_push(line));
}

/**
 * @brief Function for drawing line and processed data informations.
 * @param[in] input_sine_freq Input sine wave frequency.
 * @param[in] is_noisy        Flag if data is noisy.
 * @param[in] chart_width     Drawing chart height.
 */
static void draw_fft_header(float32_t input_sine_freq, bool is_noisy)
{
    NRF_LOG_RAW_INFO("Input: sine %uHz, noise: %s.\r\n", (uint16_t)input_sine_freq,
           (uint32_t)((is_noisy == true) ? "yes" : "no"));
}

/**
 * @brief Function for drawing ASCII data processed by FFT function.
 * @param[in] p_input_data Pointer to FFT data array.
 * @param[in] data_size    FFT array size.
 * @param[in] chart_height Drawing chart height.
 */
void draw_fft_data(m_fft_output_f32 * p_input_data, uint16_t data_size, uint16_t chart_height)
{
    uint32_t  graph_y, graph_x;
    float32_t curr_drawing_val;
    float32_t curr_percent;
    float32_t max_value;
    uint32_t  max_val_index;
    char      tmp_str[data_size + 1];

    // Search FFT max value in input array.
    arm_max_f32(p_input_data->OUTPUT_PORT, data_size, &max_value, &max_val_index);

    // Draw graph. Put space if number is less than currently processed, put '|' character otherwise.
    for (graph_y = chart_height; graph_y > 0; graph_y--)
    {
        curr_percent = ((graph_y - 1) / (chart_height * 1.f));
        curr_drawing_val = max_value * curr_percent;
        for (graph_x = 0; graph_x < (int)(data_size/2); graph_x++)
        {
            if (p_input_data->OUTPUT_PORT[graph_x] > curr_drawing_val)
            {
                tmp_str[graph_x] = '|';
            } else
            {
                tmp_str[graph_x] = ' ';
            }
        }
        tmp_str[data_size] = 0;
        NRF_LOG_RAW_INFO("%s\r\n", NRF_LOG_PUSH(tmp_str));
        NRF_LOG_FLUSH();
    }
    draw_line(data_size);
}

/**@brief Function for TWI (with transaction manager and twi_sensor) initialization.
 */
static void twi_init(void)
{
    uint32_t err_code;
    
    const nrf_drv_twi_config_t ii_config = {
       .scl                = ARDUINO_SCL_PIN,
       .sda                = ARDUINO_SDA_PIN,
       .frequency          = NRF_DRV_TWI_FREQ_400K,
       .interrupt_priority = APP_IRQ_PRIORITY_HIGHEST,
       .clear_bus_init     = false
    };

    err_code = nrf_twi_mngr_init(&m_nrf_twi_mngr, &ii_config);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_twi_sensor_init(&m_nrf_twi_sensor);
    APP_ERROR_CHECK(err_code);
}

/**@brief Function for TWI Reset (with transaction manager and twi_sensor) reset.
 */
static void twi_reset(void)
{
    uint32_t err_code;
    
    const nrf_drv_twi_config_t ii_config = {
       .scl                = ARDUINO_SCL_PIN,
       .sda                = ARDUINO_SDA_PIN,
       .frequency          = NRF_DRV_TWI_FREQ_400K,
       .interrupt_priority = APP_IRQ_PRIORITY_HIGHEST,
       .clear_bus_init     = true
    };

    nrf_twi_mngr_uninit(&m_nrf_twi_mngr);
    
    err_code = nrf_twi_mngr_init(&m_nrf_twi_mngr, &ii_config);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_twi_sensor_init(&m_nrf_twi_sensor);
    APP_ERROR_CHECK(err_code);  
}

/**@brief Function for converting decimal to 62 numtype notation.
 */
char* dto62(int decimal)
{
static char hexabindecimal[20] = {0, };
static char minus_hexabindecimal[20];
int position = 0;
int minus_flag = 0;

    while (1)
    {
        if(decimal<0)
        {
            minus_flag=1;
            decimal=decimal*-1;
        }
        int mod = decimal % 62; 
        if (mod < 10)
        {
            hexabindecimal[position] = 48 + mod;
        }
        else if(mod < 36)
        {
            hexabindecimal[position] = 65 + (mod - 10);
        }
        else
        {
            hexabindecimal[position] = 97 + (mod - 36);
        }

        decimal = decimal / 62;

        position++;

        if (decimal == 0)    break;
    }

    reverseString(hexabindecimal);

    if(minus_flag==1)
    {
        sprintf(minus_hexabindecimal,"-%s",hexabindecimal);
        return minus_hexabindecimal;
    }
    return hexabindecimal;
}


/**@brief Function for connection with the iis2dlpc_read_reg function and nrf52840 SDK.
 * @param[out] return Should be 0.
 */
int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)
{
    nrf_twi_sensor_reg_read(&m_nrf_twi_sensor, II_ADDR, reg, NULL, bufp, len);
    nrf_delay_us(500);
    NRF_LOG_FLUSH();
    return 0;
}

/**@brief Function for connection with the iis2dlpc_write_reg function and nrf52840 SDK.
 * @param[out] return Should be 0.
 */
int32_t platform_write(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)
{
    nrf_twi_sensor_reg_write(&m_nrf_twi_sensor, II_ADDR, reg, bufp, len);  
    nrf_delay_us(500);
    NRF_LOG_FLUSH();
    return 0;
}

/**@brief Function for initializing iis2dlpc sensor.
 * @details This function sets states of the iis2dlpc sensor and can affect
 *          the sensitivity of the sensor.
 *          If you failed to get device ID, you can push reset button to solve the issue easily
 */
 void platform_init(void)
{
    twi_init();
    dev_ctx.write_reg = platform_write;
    dev_ctx.read_reg = platform_read;
    dev_ctx.handle = NULL;
    iis2dlpc_device_id_get(&dev_ctx, &whoamI);
    while(whoamI != IIS2DLPC_ID)
    {
        printf("\r\ndevice_id_get failed\r\n");
        twi_reset();
        iis2dlpc_device_id_get(&dev_ctx, &whoamI);
    }
    iis2dlpc_reset_set(&dev_ctx, PROPERTY_ENABLE);
    do {
        iis2dlpc_reset_get(&dev_ctx, &rst);
    } while (rst);
    
    iis2dlpc_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
    iis2dlpc_full_scale_set(&dev_ctx, IIS2DLPC_16g);
    iis2dlpc_filter_path_set(&dev_ctx, IIS2DLPC_LPF_ON_OUT);
    iis2dlpc_filter_bandwidth_set(&dev_ctx, IIS2DLPC_ODR_DIV_20);
    iis2dlpc_power_mode_set(&dev_ctx, IIS2DLPC_HIGH_PERFORMANCE_LOW_NOISE);
    iis2dlpc_data_rate_set(&dev_ctx, IIS2DLPC_XL_ODR_1k6Hz);
    printf("\r\nall settings are completed\r\n");
}

void fft_calibration(void)
{
    static float sum_buffer[3];
    for(int calib_count = 0; calib_count < 255; calib_count++)
    {
        uint8_t reg;
        iis2dlpc_flag_data_ready_get(&dev_ctx, &reg);
        if(reg)
        {
            memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t));
    
            iis2dlpc_acceleration_raw_get(&dev_ctx, data_raw_acceleration.u8bit);
            for(int i = 0; i < 3; i++)
            {
                sum_buffer[i] += iis2dlpc_from_fs2_to_mg(data_raw_acceleration.i16bit[i]);
                nrf_delay_us(10);
            }
        }
        else  calib_count--;
    }
    for(int j = 0; j < 3; j++)
    {
        calib_offset[j] = sum_buffer[j]/256;
    }
}


/**@brief Function for activating main function of iis2dlpc sensor.
 * @details This function takes raw data from the sensor register and transmits it to the MCU,
 *          converts the data into readable and outputs it.
 *          If the state of the sensor is not ready to get new raw data, initialize all the 
 *          variables' value in this function every time. (except with current/previous_value)
 * @param[out] return can be anything.
 */
int iirun(void)
{
    uint8_t reg;
    iis2dlpc_flag_data_ready_get(&dev_ctx, &reg);
    if(reg)
    {
        memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t));
    
        iis2dlpc_acceleration_raw_get(&dev_ctx, data_raw_acceleration.u8bit);

        acceleration_mg[0] = iis2dlpc_from_fs2_to_mg(data_raw_acceleration.i16bit[0]);
        acceleration_mg[1] = iis2dlpc_from_fs2_to_mg(data_raw_acceleration.i16bit[1]);
        acceleration_mg[2] = iis2dlpc_from_fs2_to_mg(data_raw_acceleration.i16bit[2]);

        current_value[0] = acceleration_mg[0];
        current_value[1] = acceleration_mg[1];
        current_value[2] = acceleration_mg[2];

        memcpy(difference0,dto62(current_value[0]-previous_value[0]),sizeof(dto62(current_value[0]-previous_value[0])));
        memcpy(difference1,dto62(current_value[1]-previous_value[1]),sizeof(dto62(current_value[1]-previous_value[1])));
        memcpy(difference2,dto62(current_value[2]-previous_value[2]),sizeof(dto62(current_value[2]-previous_value[2])));
        memset(m_buffer, 0, sizeof(difference0)+sizeof(difference1)+sizeof(difference2)+3);
        sprintf(m_buffer, "%s %s %s", difference0, difference1, difference2);

        #ifdef ACCEL_FFT_PRINT
        accel_counter++;

        fft_generate_samples(&fft_input,
                             acceleration_mg);

        if(accel_counter == FFT_TEST_OUT_SAMPLES_LEN)
        {
            fft_process(&fft_input,
            &arm_cfft_sR_f32_len256,
            &fft_output,
            FFT_TEST_OUT_SAMPLES_LEN);
            accel_counter = 0;
            draw_fft_data(&fft_output, FFT_TEST_OUT_SAMPLES_LEN, GRAPH_WINDOW_HEIGHT);
            nrf_delay_ms(1000);
        }
        #endif

        NRF_LOG_FLUSH();
    
//        printf("               %s\n\r",m_buffer);
    }
    else
    {
        memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t));
    }
        memcpy(previous_value,current_value,sizeof(current_value));
        memset(m_buffer, 0x00, DIFF_SIZE*3); 
        memset(difference0, 0x00, DIFF_SIZE);
        memset(difference1, 0x00, DIFF_SIZE);
        memset(difference2, 0x00, DIFF_SIZE);

    return 0;
}

#ifdef TIMECHECK
void time_check(void)
{
    if(counter == 1023)
    {
        stop=DWT->CYCCNT;
        elapsed=stop-start;
        printf("%d\n", elapsed);
        counter = 0;
        start = DWT->CYCCNT;
    }
    counter++;
}
#endif

int main(void)
{
    bool noise = false;
    float32_t sine_freq;

    #ifdef FPU_INTERRUPT_MODE
    // Enable FPU interrupt
    NVIC_SetPriority(FPU_IRQn, APP_IRQ_PRIORITY_LOWEST);
    NVIC_ClearPendingIRQ(FPU_IRQn);
    NVIC_EnableIRQ(FPU_IRQn);
    #endif

    // Initialize.
    log_init();
    timer_init();
    uart_init();
    leds_init();
    power_management_init();
    ble_stack_init();
    gatt_init();
    db_discovery_init();
    platform_init();
    saadc_init();
    saadc_sampling_event_init();
    nus_c_init();
    ble_conn_state_init();
    scan_init();

    // Start execution.
    NRF_LOG_INFO("Multilink example started.");
//    scan_start();
    saadc_sampling_event_enable();    
    fft_calibration();

    // cycle counter setup
    #ifdef TIMECHECK
    CoreDebug->DEMCR |= 0x1000000;
    DWT->CYCCNT = 0;
    DWT->CTRL |= 0x1;
    #endif


    for (;;)
    {
        iirun();
        nrf_delay_us(10);
        idle_state_handle();

#ifndef FPU_INTERRUPT_MODE
        /* Clear FPSCR register and clear pending FPU interrupts. This code is base on
         * nRF5x_release_notes.txt in documentation folder. It is necessary part of code when
         * application using power saving mode and after handling FPU errors in polling mode.
         */
        __set_FPSCR(__get_FPSCR() & ~(FPU_EXCEPTION_MASK));
        (void) __get_FPSCR();
        NVIC_ClearPendingIRQ(FPU_IRQn);
#endif

    }
}

Best regards,

Baek

  • It's most likely not the SAADC that outputs zero value, but rather how the samples are processed or printed that corrupts the data. 

    You need to verify your data after each step where you've processed it, in order to find out where and how the data gets corrupted.