File content as of revision 13:f4ad8550374a:

/**
 ******************************************************************************
 * @file    main.cpp
 * @author  SRA
 * @version V1.0.0
 * @date    5-March-2019
 * @brief   Simple Example application for using the X_NUCLEO_IKS01A3
 *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 2019 STMicroelectronics</center></h2>
 *
 * 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 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 STMicroelectronics nor the names of its contributors
 *      may be used to endorse or promote products derived from this software
 *      without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
 *
 ******************************************************************************
*/

/*
    LELEC2811 Multisensor IKS01A3 Project
    M. Lefebvre - 2019-2020
*/

/* Includes */
#include <stdlib.h>
#include <time.h>
#include "mbed.h"
#include "XNucleoIKS01A3.h"
#include "stm32l073xx.h"
#include "stm32l0xx_hal_flash.h"

/* Defines */
#define FS                      25.0    // Sampling frequency (Hz)

// LIS2MDL Magnetometer
#define LIS2MDL_ODR             50.0    // Output data rate (10, 20, 50 or 100 Hz)
#define LIS2MDL_LP              0       // Power mode (0 for high-resolution mode, 1 for low-power mode)
#define LIS2MDL_LPF             0       // Bandwidth (0 for ODR/2, 1 for ODR/4)
#define LIS2MDL_COMP_TEMP_EN    1       // Temperature compensation (0 disabled, 1 enabled)
#define LIS2MDL_OFF_CANC        1       // Offset cancellation (0 for no offset cancellation, 1 for offset cancellation, 2 for set pulse only at power-on)
#define LIS2MDL_DATA_SIZE       12      // Number of bytes for LIS2MDL magnetometer data

// LPS22HH Pressure sensor
#define P0                      1013.26 // Sea level pressure (hPa)
#define LPS22HH_ODR             50.0    // Output data rate (one-shot, 1, 10, 25, 50, 75, 100, 200 Hz)
#define LPS22HH_LOW_NOISE_EN    1       // Low-noise (0 disabled, 1 enabled)
#define LPS22HH_LPF_CFG         3       // Device bandwidth (0 for ODR/2, 2 for ODR/9, 3 for ODR/20)
#define LPS22HH_DATA_SIZE       8       // Number of bytes for LPS22HH pressure sensor data

// LIS2DW12 Accelerometer
#define LIS2DW12_ODR            4       // Output data rate (0 power down, 1 HP 12.5Hz/LP 1.6Hz, 2 for 12.5Hz, 3 for 25Hz, 4 for 50Hz, 5 for 100Hz, 6 for 200Hz, 7 for HP 400Hz/LP 200Hz, 8 for HP 800Hz/LP 200Hz, 9 for HP 1600Hz/LP 200Hz)
#define LIS2DW12_FS             4       // Full-scale +-(2, 4, 8 or 16 g)
#define LIS2DW12_BW_FILT        2       // Filter bandwidth (0 for ODR/2, 1 for ODR/4, 2 for ODR/10, 3 for ODR/20)
#define LIS2DW12_LP_MODE        0       // Low-power modes 1 to 4 (1 gives the max. rms noise, 4 gives the min. rms noise)
#define LIS2DW12_MODE           1       // Mode (0 for low-power, 1 for high-performance, 2 for single data conversion)
#define LIS2DW12_LOW_NOISE      1       // Low-noise (0 disable, 1 enabled)
#define LIS2DW12_POWER_MODE     LIS2DW12_LP_MODE + (LIS2DW12_MODE << 2) + (LIS2DW12_LOW_NOISE << 4)

// HTS221 Relative humidity and temperature sensor
#define HTS221_ODR              1       // Output data rate (one-shot, 1Hz, 7Hz, 12.5Hz)
#define HTS221_HEATER           0       // Heater configuration (0 disabled, 1 enabled)
#define HTS221_AVGH             32      // Humidity averaging (4 to 512)
#define HTS221_AVGT             16      // Temperature averaging (2 to 256)

// LSM6DSO Accelerometer + gyroscope
#define LSM6DSO_ODR_XL          12.5    // Accelerometer output data rate (12.5, 26, 52, 104, 208, 416, 833, 1.66k, 3.33k, 6.66kHz)
#define LSM6DSO_FS_XL           4       // Accelerometer full scale (2, 4, 8, 16g)
#define LSM6DSO_XL_HM_MODE      1       // Accelerometer high-performance mode (0 enabled, 1 disabled)
#define LSM6DSO_XL_ULP_EN       0       // Accelerometer ultra-low-power configuration (0 disabled, 1 enabled)
#define LSM6DSO_LPF2_XL_EN      1       // Accelerometer high-resolution selection (0 for 1st stage of digital filtering, 1 for 2nd stage)
#define LSM6DSO_HP_SLOPE_XL_EN  0       // Accelerometer high-pass filter selection (0 for low-pass, 1 for high-pass)
#define LSM6DSO_HPCF_XL         2       // Accelerometer filter configuration and cutoff setting (0 for ODR/4, 1 for ODR/10, 2 for ODR/20, 3 for ODR/45, 4 for ODR/100, 5 for ODR/200, 6 for ODR/400, 7 for ODR/800)      
#define LSM6DSO_ODR_G           16      // Gyroscope output data rate (12.5, 26, 52, 104, 208, 416, 833, 1.66k, 3.33k, 6.66kHz)
#define LSM6DSO_FS_G            1000    // Gyroscope full scale (250, 500, 1000, 2000dps)
#define LSM6DSO_G_HM_MODE       1       // Gyroscope high-performance mode (0 enabled, 1 disabled)
#define LSM6DSO_LPF1_SEL_G      1       // Gyroscope digital LPF1 enable (0 disabled, 1 enabled)
#define LSM6DSO_FTYPE           3       // Gyroscope LPF1 bandwidth selection (0 ultra light, 1 very light, 2 light, 3 medium, 4 strong, 5 very strong, 6 aggressive, 7 xtreme)
#define LSM6DSO_HP_EN_G         1       // Gyroscope digital HPF enable (0 HPF disabled, 1 HPF enabled)
#define LSM6DSO_HPM_G           2       // Gyroscope HPF cutoff selection (0 for 16mHz, 1 for 65mHz, 10 for 260 mHz, 11 for 1.04Hz)

/* Functions definition */
bool acquisition_task(bool verbose);
void read_task();
void print_flash_info();
bool erase_flash(bool verbose);
bool write_flash(uint32_t Flash_addr, uint32_t* Flash_wdata, int32_t n_words, bool verbose);
void read_flash(uint32_t Flash_addr, uint32_t* Flash_rdata, uint32_t n_bytes);
void button1_enabled_cb(void);
void button1_onpressed_cb(void);
static char *print_double(char *str, double v);
float pressure_to_altitude(double pressure);
uint32_t FloatToUint(float n);
float UintToFloat(uint32_t n);

/* Serial link */
Serial pc(SERIAL_TX, SERIAL_RX);

/* Button */
InterruptIn button1(USER_BUTTON);
volatile bool button1_pressed = false; // Used in the main loop
volatile bool button1_enabled = true; // Used for debouncing
Timeout button1_timeout; // Used for debouncing

/* Instantiate the expansion board */
static XNucleoIKS01A3 *mems_expansion_board = XNucleoIKS01A3::instance(D14, D15, D4, D5, A3, D6, A4);

/* Retrieve the composing elements of the expansion board */
static LIS2MDLSensor *magnetometer = mems_expansion_board->magnetometer;
static HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor;
static LPS22HHSensor *press_temp = mems_expansion_board->pt_sensor;
static LSM6DSOSensor *acc_gyro = mems_expansion_board->acc_gyro;
static LIS2DW12Sensor *accelerometer = mems_expansion_board->accelerometer;
static STTS751Sensor *temp = mems_expansion_board->t_sensor;

/* Main */
int main()
{
    uint8_t id;
    float read_reg, read_reg_1;
    uint8_t read_reg_int, read_reg_int_1, read_reg_int_2;
    
    bool save_data = false;
    uint32_t Flash_addr = FLASH_BANK2_BASE;

    /* Serial link configuration */
    pc.baud(115200);
    
    /* Button configuration */
    button1.fall(callback(button1_onpressed_cb)); // Attach ISR to handle button press event
    
    /* Reset message */
    printf("\n\r**************************************************\n\r");
    printf("LELEC2811 IKS01A3 Multisensor Program\n\r");
    printf("**************************************************\n\r");
    
    /* LIS2MDL magnetometer sensor configuration */
    magnetometer->enable();
    printf("/***** LIS2MDL magnetometer configuration *****/\r\n");
    
    magnetometer->read_id(&id);
    printf("LIS2MDL magnetometer = 0x%X\r\n", id);
    
    magnetometer->set_m_odr(LIS2MDL_ODR);
    magnetometer->get_m_odr(&read_reg);
    printf("LIS2MDL ODR = %1.1f [Hz]\r\n", read_reg);
    
    magnetometer->set_m_lp(LIS2MDL_LP);
    magnetometer->get_m_lp(&read_reg_int);
    printf("LIS2MDL LP = %1d\r\n", read_reg_int);
    
    magnetometer->set_m_lpf(LIS2MDL_LPF);
    magnetometer->get_m_lpf(&read_reg_int);
    printf("LIS2MDL LPF = %1d\r\n", read_reg_int);
    
    magnetometer->set_m_comp_temp_en(LIS2MDL_COMP_TEMP_EN);
    magnetometer->get_m_comp_temp_en(&read_reg_int);
    printf("LIS2MDL COMP_TEMP_EN = %1d\r\n", read_reg_int);
    
    magnetometer->set_m_off_canc(LIS2MDL_OFF_CANC);
    magnetometer->get_m_off_canc(&read_reg_int);
    printf("LIS2MDL OFF_CANC = %1d\r\n", read_reg_int);
    
    /* LPS22HH pressure sensor configuration */
    press_temp->enable();
    printf("/***** LPS22HH pressure sensor configuration *****/\r\n");
    
    press_temp->read_id(&id);
    printf("LPS22HH pressure = 0x%X\r\n", id);
    
    press_temp->set_odr(LPS22HH_ODR, LPS22HH_LOW_NOISE_EN);
    press_temp->get_odr(&read_reg, &read_reg_int);
    printf("LPS22HH ODR = %1.1f [Hz]\r\n", read_reg);
    printf("LPS22HH LOW_NOISE_EN = %1d\r\n", read_reg_int);
    
    press_temp->set_lpfp_cfg(LPS22HH_LPF_CFG);
    press_temp->get_lpfp_cfg(&read_reg_int);
    printf("LPS22HH LPF_CFG = %1d\r\n", read_reg_int);
    
    /* LIS2DW12 accelerometer sensor configuration */
    accelerometer->enable_x();
    printf("/***** LIS2DW12 accelerometer sensor configuration *****/\r\n");
    
    accelerometer->read_id(&id);
    printf("LIS2DW12 accelerometer = 0x%X\r\n", id);
    
    accelerometer->set_x_odr(LIS2DW12_ODR);
    accelerometer->get_x_odr(&read_reg);
    printf("LIS2DW12 ODR = %1.3f [Hz]\r\n", read_reg);
    
    accelerometer->set_x_fs(LIS2DW12_FS);
    accelerometer->get_x_fs(&read_reg);
    printf("LIS2DW12 FS = %1.3f [g]\r\n", read_reg);
    
    accelerometer->set_x_bw_filt(LIS2DW12_BW_FILT);
    accelerometer->get_x_bw_filt(&read_reg_int);
    printf("LIS2DW12 BW_FILT = %1d\r\n", read_reg_int);
    
    accelerometer->set_x_power_mode(LIS2DW12_POWER_MODE);
    accelerometer->get_x_power_mode(&read_reg_int, &read_reg_int_1, &read_reg_int_2);
    printf("LIS2DW12 LP_MODE = %1d\r\n", read_reg_int);
    printf("LIS2DW12 MODE = %1d\r\n", read_reg_int_1);
    printf("LIS2DW12 LOW_NOISE = %1d\r\n", read_reg_int_2);
    
    /* HTS221 relative humidity and temperature sensor configuration */
    hum_temp->enable();    
    printf("/***** HTS221 humidity sensor configuration *****/\r\n");
    
    hum_temp->read_id(&id);
    printf("HTS221 humidity & temperature = 0x%X\r\n", id);
    
    hum_temp->set_odr(HTS221_ODR);
    hum_temp->get_odr(&read_reg);
    printf("HTS221 ODR = %1.3f [Hz]\r\n", read_reg);
    
    hum_temp->set_heater(HTS221_HEATER);
    hum_temp->get_heater(&read_reg_int);
    printf("HTS221 HEATER = %1d\r\n", read_reg_int);
    
    hum_temp->set_avg(HTS221_AVGH, HTS221_AVGT);
    hum_temp->get_avg(&read_reg, &read_reg_1);
    printf("HTS221 AVGH = %1.0f\r\n", read_reg);
    printf("HTS221 AVGT = %1.0f\r\n", read_reg_1);
    
    /* STTS751 Temperature sensor configuration */
    temp->enable();
    printf("/***** STTS751 temperature sensor configuration *****/\r\n");
    
    temp->read_id(&id);
    printf("STTS751 temperature = 0x%X\r\n", id);
    
    /* LSM6DSO Accelerometer and gyroscope configuration */
    acc_gyro->enable_x();
    acc_gyro->enable_g();
    printf("/***** LSM6DSO accelerometer and gyroscope sensor configuration *****/\r\n");
    
    acc_gyro->read_id(&id);
    printf("LSM6DSO accelerometer & gyroscope = 0x%X\r\n", id);
    
    acc_gyro->set_x_odr(LSM6DSO_ODR_XL);
    acc_gyro->get_x_odr(&read_reg);
    printf("LSM6DSO ODR_XL = %1.3f [Hz]\r\n", read_reg);
    
    acc_gyro->set_x_fs(LSM6DSO_FS_XL);
    acc_gyro->get_x_fs(&read_reg);
    printf("LSM6DSO FS_XL = %1.3f [g]\r\n", read_reg);
    
    acc_gyro->set_x_power_mode(LSM6DSO_XL_HM_MODE, LSM6DSO_XL_ULP_EN);
    acc_gyro->get_x_power_mode(&read_reg_int, &read_reg_int_1);
    printf("LSM6DSO XL_HM_MODE = %1d\r\n", read_reg_int);
    printf("LSM6DSO XL_ULP_EN = %1d\r\n", read_reg_int_1);
    
    acc_gyro->set_x_lpf2_en(LSM6DSO_LPF2_XL_EN);
    acc_gyro->get_x_lpf2_en(&read_reg_int);
    printf("LSM6DSO LPF2_XL_EN = %1d\r\n", read_reg_int);
    
    acc_gyro->set_x_filter_config(LSM6DSO_HP_SLOPE_XL_EN, LSM6DSO_HPCF_XL);
    acc_gyro->get_x_filter_config(&read_reg_int, &read_reg_int_1);
    printf("LSM6DSO HP_SLOPE_XL_EN = %1d\r\n", read_reg_int);
    printf("LSM6DSO HPCF_XL = %1d\r\n", read_reg_int_1);
    
    acc_gyro->set_g_odr(LSM6DSO_ODR_G);
    acc_gyro->get_g_odr(&read_reg);
    printf("LSM6DSO ODR_G = %1.3f [Hz]\r\n", read_reg);
    
    acc_gyro->set_g_fs(LSM6DSO_FS_XL);
    acc_gyro->get_g_fs(&read_reg);
    printf("LSM6DSO FS_G = %1.3f [dps]\r\n", read_reg);
    
    acc_gyro->set_g_power_mode(LSM6DSO_G_HM_MODE);
    acc_gyro->get_g_power_mode(&read_reg_int);
    printf("LSM6DSO G_HM_MODE = %1d\r\n", read_reg_int);
    
    acc_gyro->set_g_lpf_config(LSM6DSO_LPF1_SEL_G, LSM6DSO_FTYPE);
    acc_gyro->get_g_lpf_config(&read_reg_int, &read_reg_int_1);
    printf("LSM6DSO LPF1_SEL_G = %1d\r\n", read_reg_int);
    printf("LSM6DSO FTYPE = %1d\r\n", read_reg_int_1);
    
    acc_gyro->set_g_hpf_config(LSM6DSO_HP_EN_G, LSM6DSO_HPM_G);
    acc_gyro->get_g_hpf_config(&read_reg_int, &read_reg_int_1);
    printf("LSM6DSO HP_EN_G = %1d\r\n", read_reg_int);
    printf("LSM6DSO HPM_G = %1d\r\n", read_reg_int_1);
    
    /* Print Flash memory information */
    print_flash_info();
    
    /* Information for the user */
    printf("Press blue button to start data acquisition\r\n");
    printf("Press 'R' to read previously measured data\r\n");
    
    /* Acquisition loop */
    while(1) {
        // Start saving data when button is pushed
        if (button1_pressed) {
            button1_pressed = false;
            save_data = true;
            erase_flash(false);
            printf("Acquiring data...\r\n");
            printf("Press blue button to stop data acquisition\r\n");
            Flash_addr = FLASH_BANK2_BASE;
        }
        
        if (save_data) {
            // Acquisition task
            save_data = acquisition_task(true);
        }
        else {
            // Read task
            read_task();
        }
    }
}

/* Acquisition task */
bool acquisition_task(bool verbose)
{
    int32_t m_axes[3];
    int32_t acc_axes[3];
    int32_t acc_axes_1[3];
    int32_t gyro_axes[3];
    float pressure_value, hum_value, temp_value, temp_value_1;
    char buffer[32];
    
    uint32_t buffer_size = FLASH_PAGE_SIZE/4;
    uint32_t data_ind = 0;
    uint32_t data_buffer[buffer_size];
    
    uint32_t Flash_addr = FLASH_BANK2_BASE;

    while (Flash_addr <= FLASH_BANK2_END-FLASH_PAGE_SIZE+1) {
        // Read sensors data
        magnetometer->get_m_axes(m_axes);
        press_temp->get_pressure(&pressure_value);
        accelerometer->get_x_axes(acc_axes);
        hum_temp->get_temperature(&temp_value);
        hum_temp->get_humidity(&hum_value);
        temp->get_temperature(&temp_value_1);
        acc_gyro->get_x_axes(acc_axes_1);
        acc_gyro->get_g_axes(gyro_axes);
        
        // Save data to Flash memory
        for (int i=0; i<3; i++) {
            // Write page in Flash memory
            if (data_ind >= buffer_size) {
                write_flash(Flash_addr, &data_buffer[0], buffer_size, false);
                Flash_addr += FLASH_PAGE_SIZE;
                data_ind = 0;
            }
            
            // Write data to buffer
            data_buffer[data_ind] = (uint32_t) m_axes[i];
            data_ind++;
        }
        
        // Print data in terminal
        if (verbose) {
            printf("LIS2MDL: [mag/mgauss] %6d, %6d, %6d\r\n", ((uint32_t) m_axes[0]), ((uint32_t) m_axes[1]), ((uint32_t) m_axes[2]));
            printf("LPS22HH: [press/mbar] %1.3f, [alt/m] %1.3f\r\n", pressure_value, pressure_to_altitude(pressure_value));
            printf("HTS221: [temp/deg C] %1.3f, [hum/%%] %1.3f\r\n", temp_value, hum_value);
            printf("STTS751 [temp/deg C] %1.3f\r\n", temp_value_1);
            printf("LIS2DW12: [acc/mg] %6d, %6d, %6d\r\n", ((uint32_t) acc_axes[0]), ((uint32_t) acc_axes[1]), ((uint32_t) acc_axes[2]));
            printf("LSM6DSO: [acc/mg] %6d, %6d, %6d\r\n", ((uint32_t) acc_axes_1[0]), ((uint32_t) acc_axes_1[1]), ((uint32_t) acc_axes_1[2]));
            printf("LSM6DSO: [gyro/mdps] %6d, %6d, %6d\r\n", ((uint32_t) gyro_axes[0]), ((uint32_t) gyro_axes[1]), ((uint32_t) gyro_axes[2]));
        }
        
        // Wait for acquisition period
        wait(1/FS);
        
        // Stop saving data when button is pushed
        if (button1_pressed) {
            button1_pressed = false;
            // Save remaining data to Flash memory
            write_flash(Flash_addr, &data_buffer[0], data_ind, false);
            printf("Data acquisition stopped\r\n");
            printf("Press 'R' to read the data\r\n");
            return false;
        }
    }
    printf("Data acquisition stopped\r\n");
    printf("Press 'R' to read the data\r\n");
    return false;
}

/* Read task */
void read_task()
{
    char pc_input;
    uint32_t Flash_rdata[3];
    bool flash_empty = false;
    
    // Read terminal input
    if (pc.readable()) {
        pc_input = pc.getc();
    }
    else {
        pc_input = ' ';
    }
    
    // Read Flash memory if 'R' is pressed
    if ((pc_input == 'r') || (pc_input == 'R')) {
        // Data labels
        printf("mag_X\tmag_Y\tmag_Z\r\n");
        
        // Read 1st Flash data
        uint32_t Flash_addr_temp = FLASH_BANK2_BASE;
        read_flash(Flash_addr_temp, &Flash_rdata[0], LIS2MDL_DATA_SIZE);
        
        // Read Flash data
        while ((Flash_addr_temp <= FLASH_BANK2_END-LIS2MDL_DATA_SIZE+1) && !flash_empty) {
            // Print read data in the terminal
            printf("%6d\t%6d\t%6d\r\n", Flash_rdata[0], Flash_rdata[1], Flash_rdata[2]);
            Flash_addr_temp += LIS2MDL_DATA_SIZE;
            
            // Check if the next address is not empty (erased Flash only contains 0)
            if (Flash_addr_temp <= FLASH_BANK2_END-LIS2MDL_DATA_SIZE+1) {
                read_flash(Flash_addr_temp, &Flash_rdata[0], LIS2MDL_DATA_SIZE);
                if ((Flash_rdata[0] == 0) && (Flash_rdata[1] == 0) && (Flash_rdata[2] == 0)) {
                    flash_empty = true;
                }
            }
        }
    }
}

/* Print Flash memory info */
void print_flash_info()
{
    printf("**************************************************\n\r");
    printf("/***** Flash memory info *****/\r\n");
    printf("Flash size: %d [B]\r\n", FLASH_SIZE);
    printf("Flash page size: %d [B]\r\n", FLASH_PAGE_SIZE);
    printf("Flash nb of pages: %d \r\n", FLASH_SIZE/FLASH_PAGE_SIZE);
    printf("Flash bank 1 base address: 0x%X\r\n", FLASH_BASE);
    printf("Flash bank 1 end address: 0x%X\r\n", FLASH_BANK1_END);
    printf("Flash bank 2 base address: 0x%X\r\n", FLASH_BANK2_BASE);
    printf("Flash bank 2 end address: 0x%X\r\n", FLASH_BANK2_END);
    printf("**************************************************\n\r");
}

/* Erase content of Flash memory */
bool erase_flash(bool verbose)
{
    printf("Erasing Flash memory...\r\n");
    
    // Unlock Flash memory
    HAL_FLASH_Unlock();

    // Erase Flash memory
    FLASH_EraseInitTypeDef eraser;
    uint32_t Flash_addr = FLASH_BANK2_BASE;
    uint32_t page_error = 0;
    int32_t page = 1;
    
    while (Flash_addr < FLASH_BANK2_END) {
        eraser.TypeErase = FLASH_TYPEERASE_PAGES;
        eraser.PageAddress = Flash_addr;
        eraser.NbPages = 1;
        if(HAL_OK != HAL_FLASHEx_Erase(&eraser, &page_error)) {
            if (verbose) {printf("Flash erase failed!\r\n");}
            printf("Error 0x%X\r\n", page_error);
            HAL_FLASH_Lock();
            return false;
        }
        if (verbose) {printf("Erased page %d at address: 0x%X\r\n", page, Flash_addr);}
        Flash_addr += FLASH_PAGE_SIZE;
        page++;
    }
    
    if (verbose) {printf("Flash erase succesful!\r\n");}
    return true;
}

/* Write Flash memory */
bool write_flash(uint32_t Flash_addr, uint32_t* Flash_wdata, int32_t n_words, bool verbose)
{
    clock_t time;
    if (verbose) {time = clock();}
    
    // Unlock Flash memory
    HAL_FLASH_Unlock();
    
    // Write Flash memory
    for (int i=0; i<n_words; i++) {
        if (HAL_OK != HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, Flash_addr, Flash_wdata[i])) {
            if (verbose) {printf("Flash write failed!\r\n");}
            HAL_FLASH_Lock();
            return false;
        }
        Flash_addr += 4;
    }
    if (verbose) {printf("Flash write succesful!\r\n");}
    
    HAL_FLASH_Lock();
    
    if (verbose) {
        time = clock() - time;
        printf("Time to write: %1.6f [s]\r\n", (((double) time)/CLOCKS_PER_SEC));
    }
    
    return true;
}

/* Read Flash memory */
void read_flash(uint32_t Flash_addr, uint32_t* Flash_rdata, uint32_t n_bytes)
{
    memcpy(Flash_rdata, (uint32_t*) Flash_addr, n_bytes);
}

/* Enables button when bouncing is over */
void button1_enabled_cb(void)
{
    button1_enabled = true;
}

/* ISR handling button pressed event */
void button1_onpressed_cb(void)
{
    if (button1_enabled) { // Disabled while the button is bouncing
        button1_enabled = false;
        button1_pressed = true; // To be read by the main loop
        button1_timeout.attach(callback(button1_enabled_cb), 0.3); // Debounce time 300 ms
    }
}

/* Helper function for printing floats & doubles */
static char *print_double(char *str, double v)
{
    int decimalDigits = 6;
    int i = 1;
    int intPart, fractPart;
    int len;
    char *ptr;

    /* prepare decimal digits multiplicator */
    for (; decimalDigits != 0; i *= 10, decimalDigits--);

    /* calculate integer & fractinal parts */
    intPart = (int)v;
    fractPart = (int)((v - (double)(int)v) * i);

    /* fill in integer part */
    sprintf(str, "%i.", intPart);

    /* prepare fill in of fractional part */
    len = strlen(str);
    ptr = &str[len];

    /* fill in leading fractional zeros */
    for (i /= 10; i > 1; i /= 10, ptr++) {
        if (fractPart >= i) {
            break;
        }
        *ptr = '0';
    }

    /* fill in (rest of) fractional part */
    sprintf(ptr, "%i", fractPart);

    return str;
}

/* Pressure to altitude conversion */
float pressure_to_altitude(double pressure)
{
    return 44330.77 * (1-pow(pressure/P0, 0.1902632));
}

uint32_t FloatToUint(float n)
{
   return (uint32_t)(*(uint32_t*)&n);
}
 
float UintToFloat(uint32_t n)
{
   return (float)(*(float*)&n);
}