main.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    main.cpp
00004  * @author  SRA
00005  * @version V1.0.0
00006  * @date    5-March-2019
00007  * @brief   Simple Example application for using the X_NUCLEO_IKS01A3
00008  *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
00009  ******************************************************************************
00010  * @attention
00011  *
00012  * <h2><center>&copy; COPYRIGHT(c) 2019 STMicroelectronics</center></h2>
00013  *
00014  * Redistribution and use in source and binary forms, with or without modification,
00015  * are permitted provided that the following conditions are met:
00016  *   1. Redistributions of source code must retain the above copyright notice,
00017  *      this list of conditions and the following disclaimer.
00018  *   2. Redistributions in binary form must reproduce the above copyright notice,
00019  *      this list of conditions and the following disclaimer in the documentation
00020  *      and/or other materials provided with the distribution.
00021  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00022  *      may be used to endorse or promote products derived from this software
00023  *      without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00028  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00029  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00030  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00031  *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00033  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00034  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  ******************************************************************************
00037 */
00038 
00039 /*
00040     LELEC2811 Magnetometer LIS2MDL Project
00041     M. Lefebvre - 2019-2020
00042 */
00043 
00044 /* Includes */
00045 #include <stdlib.h>
00046 #include "mbed.h"
00047 #include "XNucleoIKS01A3.h"
00048 #include "stm32l073xx.h"
00049 #include "stm32l0xx_hal_flash.h"
00050 
00051 /* Defines */
00052 #define FS                      5.0     // Readout frequency (Hz) - /!\ Must be below 100Hz
00053 #define FLASH_WRITE_TIME        0.00328 // Flash write time (s)
00054 
00055 #define LIS2MDL_ODR             20.0    // Output data rate (10, 20, 50 or 100 Hz)
00056 #define LIS2MDL_LP              1       // Power mode (0 for high-resolution mode, 1 for low-power mode)
00057 #define LIS2MDL_LPF             0       // Bandwidth (0 for ODR/2, 1 for ODR/4)
00058 #define LIS2MDL_COMP_TEMP_EN    1       // Temperature compensation (0 disabled, 1 enabled)
00059 #define LIS2MDL_OFF_CANC        0       // Offset cancellation (0 disabled, 1 enabled, 2 for set pulse only at power-on)
00060 #define LIS2MDL_DATA_SIZE       12      // Number of bytes for LIS2MDL magnetometer data
00061 
00062 #define TS                      (1/FS)-((LIS2MDL_DATA_SIZE/4)*FLASH_WRITE_TIME)
00063 
00064 /* Functions definition */
00065 bool acquisition_task(bool verbose);
00066 void read_task();
00067 void print_flash_info();
00068 bool erase_flash(bool verbose);
00069 bool write_flash(uint32_t Flash_addr, uint32_t* Flash_wdata, int32_t n_words, bool verbose);
00070 void read_flash(uint32_t Flash_addr, uint32_t* Flash_rdata, uint32_t n_bytes);
00071 void button1_enabled_cb(void);
00072 void button1_onpressed_cb(void);
00073 static char *print_double(char *str, double v);
00074 uint32_t FloatToUint(float n);
00075 float UintToFloat(uint32_t n);
00076 
00077 /* Serial link */
00078 Serial pc(SERIAL_TX, SERIAL_RX);
00079 
00080 /* Button */
00081 InterruptIn button1(USER_BUTTON);
00082 volatile bool button1_pressed = false; // Used in the main loop
00083 volatile bool button1_enabled = true; // Used for debouncing
00084 Timeout button1_timeout; // Used for debouncing
00085 
00086 /* Instantiate the expansion board */
00087 static XNucleoIKS01A3 *mems_expansion_board = XNucleoIKS01A3::instance(D14, D15, D4, D5, A3, D6, A4);
00088 
00089 /* Retrieve the composing elements of the expansion board */
00090 static LIS2MDLSensor *magnetometer = mems_expansion_board->magnetometer;
00091 static HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor;
00092 static LPS22HHSensor *press_temp = mems_expansion_board->pt_sensor;
00093 static LSM6DSOSensor *acc_gyro = mems_expansion_board->acc_gyro;
00094 static LIS2DW12Sensor *accelerometer = mems_expansion_board->accelerometer;
00095 static STTS751Sensor *temp = mems_expansion_board->t_sensor;
00096 
00097 /* Main */
00098 int main()
00099 {
00100     uint8_t id;
00101     float read_reg;
00102     uint8_t read_reg_int;
00103     
00104     bool save_data = false;
00105     uint32_t Flash_addr = FLASH_BANK2_BASE;
00106 
00107     /* Serial link configuration */
00108     pc.baud(115200);
00109     
00110     /* Button configuration */
00111     button1.fall(callback(button1_onpressed_cb)); // Attach ISR to handle button press event
00112     
00113     /* Reset message */
00114     printf("\n\r**************************************************\n\r");
00115     printf("LELEC2811 LIS2MDL Magnetometer Program\n\r");
00116     printf("**************************************************\n\r");
00117     
00118     /* LIS2MDL magnetometer sensor configuration */
00119     magnetometer->enable();
00120     printf("/***** LIS2MDL magnetometer configuration *****/\r\n");
00121     
00122     magnetometer->read_id(&id);
00123     printf("LIS2MDL magnetometer = 0x%X\r\n", id);
00124     
00125     magnetometer->set_m_odr(LIS2MDL_ODR);
00126     magnetometer->get_m_odr(&read_reg);
00127     printf("LIS2MDL ODR = %1.1f [Hz]\r\n", read_reg);
00128     
00129     magnetometer->set_m_lp(LIS2MDL_LP);
00130     magnetometer->get_m_lp(&read_reg_int);
00131     printf("LIS2MDL LP = %1d\r\n", read_reg_int);
00132     
00133     magnetometer->set_m_lpf(LIS2MDL_LPF);
00134     magnetometer->get_m_lpf(&read_reg_int);
00135     printf("LIS2MDL LPF = %1d\r\n", read_reg_int);
00136     
00137     magnetometer->set_m_comp_temp_en(LIS2MDL_COMP_TEMP_EN);
00138     magnetometer->get_m_comp_temp_en(&read_reg_int);
00139     printf("LIS2MDL COMP_TEMP_EN = %1d\r\n", read_reg_int);
00140     
00141     magnetometer->set_m_off_canc(LIS2MDL_OFF_CANC);
00142     magnetometer->get_m_off_canc(&read_reg_int);
00143     printf("LIS2MDL OFF_CANC = %1d\r\n", read_reg_int);
00144     
00145     /* Print Flash memory information */
00146     print_flash_info();
00147     
00148     /* Information for the user */
00149     printf("Press blue button to start data acquisition\r\n");
00150     printf("Press 'R' to read previously measured data\r\n");
00151     
00152     /* Acquisition loop */
00153     while(1) {
00154         // Start saving data when button is pushed
00155         if (button1_pressed) {
00156             button1_pressed = false;
00157             save_data = true;
00158             erase_flash(false);
00159             printf("Acquiring data...\r\n");
00160             printf("Press blue button to stop data acquisition\r\n");
00161             Flash_addr = FLASH_BANK2_BASE;
00162         }
00163         
00164         if (save_data) {
00165             // Acquisition task
00166             save_data = acquisition_task(false);
00167         }
00168         else {
00169             // Read task
00170             read_task();
00171         }
00172     }
00173 }
00174 
00175 /* Acquisition task */
00176 bool acquisition_task(bool verbose)
00177 {
00178     uint32_t Flash_addr = FLASH_BANK2_BASE;
00179     int32_t m_axes[3];
00180 
00181     while (Flash_addr <= FLASH_BANK2_END-FLASH_PAGE_SIZE+1) {
00182         // Read magnetometer data
00183         magnetometer->get_m_axes(m_axes);
00184         
00185         // Write page in Flash memory
00186         write_flash(Flash_addr, (uint32_t*) &m_axes[0], 3, false);
00187         Flash_addr += LIS2MDL_DATA_SIZE;
00188         
00189         // Print data in terminal
00190         if (verbose) {
00191             printf("LIS2MDL [mag/mgauss]:  %6d, %6d, %6d\r\n", ((uint32_t) m_axes[0]), ((uint32_t) m_axes[1]), ((uint32_t) m_axes[2]));
00192         }
00193         
00194         // Wait for acquisition period
00195         wait(TS);
00196         
00197         // Stop saving data when button is pushed
00198         if (button1_pressed) {
00199             button1_pressed = false;
00200             printf("Data acquisition stopped\r\n");
00201             printf("Press 'R' to read the data\r\n");
00202             return false;
00203         }
00204     }
00205     printf("Data acquisition stopped\r\n");
00206     printf("Press 'R' to read the data\r\n");
00207     return false;
00208 }
00209 
00210 /* Read task */
00211 void read_task()
00212 {
00213     char pc_input;
00214     uint32_t Flash_rdata[3];
00215     bool flash_empty = false;
00216     
00217     // Read terminal input
00218     if (pc.readable()) {
00219         pc_input = pc.getc();
00220     }
00221     else {
00222         pc_input = ' ';
00223     }
00224     
00225     // Read Flash memory if 'R' is pressed
00226     if ((pc_input == 'r') || (pc_input == 'R')) {
00227         // Data labels
00228         printf("mag_X\tmag_Y\tmag_Z\r\n");
00229         
00230         // Read 1st Flash data
00231         uint32_t Flash_addr_temp = FLASH_BANK2_BASE;
00232         read_flash(Flash_addr_temp, &Flash_rdata[0], LIS2MDL_DATA_SIZE);
00233         
00234         // Read Flash data
00235         while ((Flash_addr_temp <= FLASH_BANK2_END-LIS2MDL_DATA_SIZE+1) && !flash_empty) {
00236             // Print read data in the terminal
00237             printf("%6d\t%6d\t%6d\r\n", Flash_rdata[0], Flash_rdata[1], Flash_rdata[2]);
00238             Flash_addr_temp += LIS2MDL_DATA_SIZE;
00239             
00240             // Check if the next address is not empty (erased Flash only contains 0)
00241             if (Flash_addr_temp <= FLASH_BANK2_END-LIS2MDL_DATA_SIZE+1) {
00242                 read_flash(Flash_addr_temp, &Flash_rdata[0], LIS2MDL_DATA_SIZE);
00243                 if ((Flash_rdata[0] == 0) && (Flash_rdata[1] == 0) && (Flash_rdata[2] == 0)) {
00244                     flash_empty = true;
00245                 }
00246             }
00247         }
00248     }
00249 }
00250 
00251 /* Print Flash memory info */
00252 void print_flash_info()
00253 {
00254     printf("**************************************************\n\r");
00255     printf("/***** Flash memory info *****/\r\n");
00256     printf("Flash size: %d [B]\r\n", FLASH_SIZE);
00257     printf("Flash page size: %d [B]\r\n", FLASH_PAGE_SIZE);
00258     printf("Flash nb of pages: %d \r\n", FLASH_SIZE/FLASH_PAGE_SIZE);
00259     printf("Flash bank 1 base address: 0x%X\r\n", FLASH_BASE);
00260     printf("Flash bank 1 end address: 0x%X\r\n", FLASH_BANK1_END);
00261     printf("Flash bank 2 base address: 0x%X\r\n", FLASH_BANK2_BASE);
00262     printf("Flash bank 2 end address: 0x%X\r\n", FLASH_BANK2_END);
00263     printf("**************************************************\n\r");
00264 }
00265 
00266 /* Erase content of Flash memory */
00267 bool erase_flash(bool verbose)
00268 {
00269     printf("Erasing Flash memory...\r\n");
00270     
00271     // Unlock Flash memory
00272     HAL_FLASH_Unlock();
00273 
00274     // Erase Flash memory
00275     FLASH_EraseInitTypeDef eraser;
00276     uint32_t Flash_addr = FLASH_BANK2_BASE;
00277     uint32_t page_error = 0;
00278     int32_t page = 1;
00279     
00280     while (Flash_addr < FLASH_BANK2_END) {
00281         eraser.TypeErase = FLASH_TYPEERASE_PAGES;
00282         eraser.PageAddress = Flash_addr;
00283         eraser.NbPages = 1;
00284         if(HAL_OK != HAL_FLASHEx_Erase(&eraser, &page_error)) {
00285             if (verbose) {printf("Flash erase failed!\r\n");}
00286             printf("Error 0x%X\r\n", page_error);
00287             HAL_FLASH_Lock();
00288             return false;
00289         }
00290         if (verbose) {printf("Erased page %d at address: 0x%X\r\n", page, Flash_addr);}
00291         Flash_addr += FLASH_PAGE_SIZE;
00292         page++;
00293     }
00294     
00295     if (verbose) {printf("Flash erase succesful!\r\n");}
00296     return true;
00297 }
00298 
00299 /* Write Flash memory */
00300 bool write_flash(uint32_t Flash_addr, uint32_t* Flash_wdata, int32_t n_words, bool verbose)
00301 {
00302     // Unlock Flash memory
00303     HAL_FLASH_Unlock();
00304     
00305     // Write Flash memory
00306     for (int i=0; i<n_words; i++) {
00307         if (HAL_OK != HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, Flash_addr, Flash_wdata[i])) {
00308             if (verbose) {printf("Flash write failed!\r\n");}
00309             HAL_FLASH_Lock();
00310             return false;
00311         }
00312         Flash_addr += 4;
00313     }
00314     if (verbose) {printf("Flash write succesful!\r\n");}
00315     HAL_FLASH_Lock();
00316     return true;
00317 }
00318 
00319 /* Read Flash memory */
00320 void read_flash(uint32_t Flash_addr, uint32_t* Flash_rdata, uint32_t n_bytes)
00321 {
00322     memcpy(Flash_rdata, (uint32_t*) Flash_addr, n_bytes);
00323 }
00324 
00325 /* Enables button when bouncing is over */
00326 void button1_enabled_cb(void)
00327 {
00328     button1_enabled = true;
00329 }
00330 
00331 /* ISR handling button pressed event */
00332 void button1_onpressed_cb(void)
00333 {
00334     if (button1_enabled) { // Disabled while the button is bouncing
00335         button1_enabled = false;
00336         button1_pressed = true; // To be read by the main loop
00337         button1_timeout.attach(callback(button1_enabled_cb), 0.3); // Debounce time 300 ms
00338     }
00339 }
00340 
00341 /* Helper function for printing floats & doubles */
00342 static char *print_double(char *str, double v)
00343 {
00344     int decimalDigits = 6;
00345     int i = 1;
00346     int intPart, fractPart;
00347     int len;
00348     char *ptr;
00349 
00350     /* prepare decimal digits multiplicator */
00351     for (; decimalDigits != 0; i *= 10, decimalDigits--);
00352 
00353     /* calculate integer & fractinal parts */
00354     intPart = (int)v;
00355     fractPart = (int)((v - (double)(int)v) * i);
00356 
00357     /* fill in integer part */
00358     sprintf(str, "%i.", intPart);
00359 
00360     /* prepare fill in of fractional part */
00361     len = strlen(str);
00362     ptr = &str[len];
00363 
00364     /* fill in leading fractional zeros */
00365     for (i /= 10; i > 1; i /= 10, ptr++) {
00366         if (fractPart >= i) {
00367             break;
00368         }
00369         *ptr = '0';
00370     }
00371 
00372     /* fill in (rest of) fractional part */
00373     sprintf(ptr, "%i", fractPart);
00374 
00375     return str;
00376 }
00377 
00378 uint32_t FloatToUint(float n)
00379 {
00380    return (uint32_t)(*(uint32_t*)&n);
00381 }
00382  
00383 float UintToFloat(uint32_t n)
00384 {
00385    return (float)(*(float*)&n);
00386 }