LELEC2811 - I&S / Mbed OS LELEC2811_LISDW12_Accelerometer

2019-2020 LIS2DW12 Accelerometer Project

Dependencies:   X_NUCLEO_IKS01A3

main.cpp@9:7cda26534126, 2019-09-17 (annotated)

Committer:
martlefebvre94
Date:
Tue Sep 17 08:57:48 2019 +0000
Revision:
9:7cda26534126
Parent:
8:fa346d946e7e
Child:
10:3a7580daadda
Multisensor project code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cparata 0:535249dc4bf5 1 /**
cparata 0:535249dc4bf5 2 ******************************************************************************
cparata 0:535249dc4bf5 3 * @file main.cpp
cparata 0:535249dc4bf5 4 * @author SRA
cparata 0:535249dc4bf5 5 * @version V1.0.0
cparata 0:535249dc4bf5 6 * @date 5-March-2019
cparata 5:7c883cce2bc4 7 * @brief Simple Example application for using the X_NUCLEO_IKS01A3
cparata 0:535249dc4bf5 8 * MEMS Inertial & Environmental Sensor Nucleo expansion board.
cparata 0:535249dc4bf5 9 ******************************************************************************
cparata 0:535249dc4bf5 10 * @attention
cparata 0:535249dc4bf5 11 *
cparata 0:535249dc4bf5 12 * <h2><center>&copy; COPYRIGHT(c) 2019 STMicroelectronics</center></h2>
cparata 0:535249dc4bf5 13 *
cparata 0:535249dc4bf5 14 * Redistribution and use in source and binary forms, with or without modification,
cparata 0:535249dc4bf5 15 * are permitted provided that the following conditions are met:
cparata 0:535249dc4bf5 16 * 1. Redistributions of source code must retain the above copyright notice,
cparata 0:535249dc4bf5 17 * this list of conditions and the following disclaimer.
cparata 0:535249dc4bf5 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
cparata 0:535249dc4bf5 19 * this list of conditions and the following disclaimer in the documentation
cparata 0:535249dc4bf5 20 * and/or other materials provided with the distribution.
cparata 0:535249dc4bf5 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
cparata 0:535249dc4bf5 22 * may be used to endorse or promote products derived from this software
cparata 0:535249dc4bf5 23 * without specific prior written permission.
cparata 0:535249dc4bf5 24 *
cparata 0:535249dc4bf5 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
cparata 0:535249dc4bf5 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
cparata 0:535249dc4bf5 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
cparata 0:535249dc4bf5 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
cparata 0:535249dc4bf5 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
cparata 0:535249dc4bf5 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
cparata 0:535249dc4bf5 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
cparata 0:535249dc4bf5 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
cparata 0:535249dc4bf5 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
cparata 0:535249dc4bf5 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
cparata 0:535249dc4bf5 35 *
cparata 0:535249dc4bf5 36 ******************************************************************************
cparata 5:7c883cce2bc4 37 */
cparata 0:535249dc4bf5 38
martlefebvre94 6:b2e247935342 39 /*
martlefebvre94 8:fa346d946e7e 40 LELEC2811 Multisensor IKS01A3 Project
martlefebvre94 6:b2e247935342 41 M. Lefebvre - 2019
martlefebvre94 6:b2e247935342 42 */
martlefebvre94 6:b2e247935342 43
cparata 0:535249dc4bf5 44 /* Includes */
martlefebvre94 6:b2e247935342 45 #include <stdlib.h>
martlefebvre94 8:fa346d946e7e 46 #include <time.h>
cparata 0:535249dc4bf5 47 #include "mbed.h"
cparata 0:535249dc4bf5 48 #include "XNucleoIKS01A3.h"
martlefebvre94 6:b2e247935342 49 #include "stm32l073xx.h"
martlefebvre94 6:b2e247935342 50 #include "stm32l0xx_hal_flash.h"
martlefebvre94 6:b2e247935342 51
martlefebvre94 6:b2e247935342 52 /* Defines */
martlefebvre94 8:fa346d946e7e 53 #define FS 25.0 // Sampling frequency (Hz)
martlefebvre94 8:fa346d946e7e 54
martlefebvre94 8:fa346d946e7e 55 // LIS2MDL Magnetometer
martlefebvre94 7:4a3b6202963e 56 #define LIS2MDL_ODR 50.0 // Output data rate (10, 20, 50 or 100 Hz)
martlefebvre94 7:4a3b6202963e 57 #define LIS2MDL_LP 0 // Power mode (0 for high-resolution mode, 1 for low-power mode)
martlefebvre94 7:4a3b6202963e 58 #define LIS2MDL_LPF 0 // Bandwidth (0 for ODR/2, 1 for ODR/4)
martlefebvre94 7:4a3b6202963e 59 #define LIS2MDL_COMP_TEMP_EN 1 // Temperature compensation (0 disabled, 1 enabled)
martlefebvre94 7:4a3b6202963e 60 #define LIS2MDL_OFF_CANC 1 // Offset cancellation (0 for no offset cancellation, 1 for offset cancellation, 2 for set pulse only at power-on)
martlefebvre94 8:fa346d946e7e 61 #define LIS2MDL_DATA_SIZE 12 // Number of bytes for LIS2MDL magnetometer data
martlefebvre94 8:fa346d946e7e 62
martlefebvre94 8:fa346d946e7e 63 // LPS22HH Pressure sensor
martlefebvre94 8:fa346d946e7e 64 #define P0 1013.26 // Sea level pressure (hPa)
martlefebvre94 8:fa346d946e7e 65 #define LPS22HH_ODR 50.0 // Output data rate (one-shot, 1, 10, 25, 50, 75, 100, 200 Hz)
martlefebvre94 8:fa346d946e7e 66 #define LPS22HH_LOW_NOISE_EN 1 // Low-noise (0 disabled, 1 enabled)
martlefebvre94 8:fa346d946e7e 67 #define LPS22HH_LPF_CFG 3 // Device bandwidth (0 for ODR/2, 2 for ODR/9, 3 for ODR/20)
martlefebvre94 8:fa346d946e7e 68
martlefebvre94 8:fa346d946e7e 69 // LIS2DW12 Accelerometer
martlefebvre94 8:fa346d946e7e 70 #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)
martlefebvre94 8:fa346d946e7e 71 #define LIS2DW12_FS 4 // Full-scale +-(2, 4, 8 or 16 g)
martlefebvre94 8:fa346d946e7e 72 #define LIS2DW12_BW_FILT 2 // Filter bandwidth (0 for ODR/2, 1 for ODR/4, 2 for ODR/10, 3 for ODR/20)
martlefebvre94 8:fa346d946e7e 73 #define LIS2DW12_LP_MODE 0 // Low-power modes 1 to 4 (1 gives the max. rms noise, 4 gives the min. rms noise)
martlefebvre94 8:fa346d946e7e 74 #define LIS2DW12_MODE 1 // Mode (0 for low-power, 1 for high-performance, 2 for single data conversion)
martlefebvre94 8:fa346d946e7e 75 #define LIS2DW12_LOW_NOISE 1 // Low-noise (0 disable, 1 enabled)
martlefebvre94 8:fa346d946e7e 76 #define LIS2DW12_POWER_MODE LIS2DW12_LP_MODE + (LIS2DW12_MODE << 2) + (LIS2DW12_LOW_NOISE << 4)
martlefebvre94 8:fa346d946e7e 77
martlefebvre94 8:fa346d946e7e 78 // HTS221 Relative humidity and temperature sensor
martlefebvre94 8:fa346d946e7e 79 #define HTS221_ODR 1 // Output data rate (one-shot, 1Hz, 7Hz, 12.5Hz)
martlefebvre94 8:fa346d946e7e 80 #define HTS221_HEATER 0 // Heater configuration (0 disabled, 1 enabled)
martlefebvre94 8:fa346d946e7e 81 #define HTS221_AVGH 32 // Humidity averaging (4 to 512)
martlefebvre94 8:fa346d946e7e 82 #define HTS221_AVGT 16 // Temperature averaging (2 to 256)
martlefebvre94 8:fa346d946e7e 83
martlefebvre94 8:fa346d946e7e 84 // LSM6DSO Accelerometer + gyroscope
martlefebvre94 8:fa346d946e7e 85 #define LSM6DSO_ODR_XL 12.5 // Accelerometer output data rate (12.5, 26, 52, 104, 208, 416, 833, 1.66k, 3.33k, 6.66kHz)
martlefebvre94 8:fa346d946e7e 86 #define LSM6DSO_FS_XL 4 // Accelerometer full scale (2, 4, 8, 16g)
martlefebvre94 8:fa346d946e7e 87 #define LSM6DSO_XL_HM_MODE 1 // Accelerometer high-performance mode (0 enabled, 1 disabled)
martlefebvre94 8:fa346d946e7e 88 #define LSM6DSO_XL_ULP_EN 0 // Accelerometer ultra-low-power configuration (0 disabled, 1 enabled)
martlefebvre94 8:fa346d946e7e 89 #define LSM6DSO_ODR_G 16 // Gyroscope output data rate (12.5, 26, 52, 104, 208, 416, 833, 1.66k, 3.33k, 6.66kHz)
martlefebvre94 8:fa346d946e7e 90 #define LSM6DSO_FS_G 1000 // Gyroscope full scale (250, 500, 1000, 2000dps)
martlefebvre94 8:fa346d946e7e 91
martlefebvre94 8:fa346d946e7e 92 /* Functions definition */
martlefebvre94 8:fa346d946e7e 93 bool acquisition_task(bool verbose);
martlefebvre94 8:fa346d946e7e 94 void read_task();
martlefebvre94 8:fa346d946e7e 95 void print_flash_info();
martlefebvre94 8:fa346d946e7e 96 bool erase_flash(bool verbose);
martlefebvre94 8:fa346d946e7e 97 bool write_flash(uint32_t Flash_addr, uint32_t* Flash_wdata, int32_t n_words, bool verbose);
martlefebvre94 8:fa346d946e7e 98 void read_flash(uint32_t Flash_addr, uint32_t* Flash_rdata, uint32_t n_bytes);
martlefebvre94 8:fa346d946e7e 99 void button1_enabled_cb(void);
martlefebvre94 8:fa346d946e7e 100 void button1_onpressed_cb(void);
martlefebvre94 8:fa346d946e7e 101 static char *print_double(char *str, double v);
martlefebvre94 8:fa346d946e7e 102 float pressure_to_altitude(double pressure);
martlefebvre94 6:b2e247935342 103
martlefebvre94 6:b2e247935342 104 /* Serial link */
martlefebvre94 6:b2e247935342 105 Serial pc(SERIAL_TX, SERIAL_RX);
martlefebvre94 6:b2e247935342 106
martlefebvre94 6:b2e247935342 107 /* Button */
martlefebvre94 6:b2e247935342 108 InterruptIn button1(USER_BUTTON);
martlefebvre94 6:b2e247935342 109 volatile bool button1_pressed = false; // Used in the main loop
martlefebvre94 6:b2e247935342 110 volatile bool button1_enabled = true; // Used for debouncing
martlefebvre94 6:b2e247935342 111 Timeout button1_timeout; // Used for debouncing
cparata 0:535249dc4bf5 112
cparata 0:535249dc4bf5 113 /* Instantiate the expansion board */
cparata 0:535249dc4bf5 114 static XNucleoIKS01A3 *mems_expansion_board = XNucleoIKS01A3::instance(D14, D15, D4, D5, A3, D6, A4);
cparata 0:535249dc4bf5 115
cparata 0:535249dc4bf5 116 /* Retrieve the composing elements of the expansion board */
cparata 0:535249dc4bf5 117 static LIS2MDLSensor *magnetometer = mems_expansion_board->magnetometer;
cparata 0:535249dc4bf5 118 static HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor;
cparata 0:535249dc4bf5 119 static LPS22HHSensor *press_temp = mems_expansion_board->pt_sensor;
cparata 0:535249dc4bf5 120 static LSM6DSOSensor *acc_gyro = mems_expansion_board->acc_gyro;
cparata 0:535249dc4bf5 121 static LIS2DW12Sensor *accelerometer = mems_expansion_board->accelerometer;
cparata 0:535249dc4bf5 122 static STTS751Sensor *temp = mems_expansion_board->t_sensor;
cparata 0:535249dc4bf5 123
martlefebvre94 8:fa346d946e7e 124 /* Main */
martlefebvre94 8:fa346d946e7e 125 int main()
martlefebvre94 8:fa346d946e7e 126 {
martlefebvre94 8:fa346d946e7e 127 uint8_t id;
martlefebvre94 8:fa346d946e7e 128 float read_reg, read_reg_1;
martlefebvre94 8:fa346d946e7e 129 uint8_t read_reg_int, read_reg_int_1, read_reg_int_2;
martlefebvre94 8:fa346d946e7e 130
martlefebvre94 8:fa346d946e7e 131 bool save_data = false;
martlefebvre94 8:fa346d946e7e 132 uint32_t Flash_addr = FLASH_BANK2_BASE;
martlefebvre94 8:fa346d946e7e 133
martlefebvre94 8:fa346d946e7e 134 /* Serial link configuration */
martlefebvre94 8:fa346d946e7e 135 pc.baud(115200);
martlefebvre94 8:fa346d946e7e 136
martlefebvre94 8:fa346d946e7e 137 /* Button configuration */
martlefebvre94 8:fa346d946e7e 138 button1.fall(callback(button1_onpressed_cb)); // Attach ISR to handle button press event
martlefebvre94 8:fa346d946e7e 139
martlefebvre94 8:fa346d946e7e 140 /* Reset message */
martlefebvre94 8:fa346d946e7e 141 printf("\n\r**************************************************\n\r");
martlefebvre94 8:fa346d946e7e 142 printf("LELEC2811 IKS01A3 Multisensor Program\n\r");
martlefebvre94 8:fa346d946e7e 143 printf("**************************************************\n\r");
martlefebvre94 8:fa346d946e7e 144
martlefebvre94 8:fa346d946e7e 145 /* LIS2MDL magnetometer sensor configuration */
martlefebvre94 8:fa346d946e7e 146 magnetometer->enable();
martlefebvre94 8:fa346d946e7e 147 printf("/***** LIS2MDL magnetometer configuration *****/\r\n");
martlefebvre94 8:fa346d946e7e 148
martlefebvre94 8:fa346d946e7e 149 magnetometer->read_id(&id);
martlefebvre94 8:fa346d946e7e 150 printf("LIS2MDL magnetometer = 0x%X\r\n", id);
martlefebvre94 8:fa346d946e7e 151
martlefebvre94 8:fa346d946e7e 152 magnetometer->set_m_odr(LIS2MDL_ODR);
martlefebvre94 8:fa346d946e7e 153 magnetometer->get_m_odr(&read_reg);
martlefebvre94 8:fa346d946e7e 154 printf("LIS2MDL ODR = %1.1f [Hz]\r\n", read_reg);
martlefebvre94 8:fa346d946e7e 155
martlefebvre94 8:fa346d946e7e 156 magnetometer->set_m_lp(LIS2MDL_LP);
martlefebvre94 8:fa346d946e7e 157 magnetometer->get_m_lp(&read_reg_int);
martlefebvre94 8:fa346d946e7e 158 printf("LIS2MDL LP = %1d\r\n", read_reg_int);
martlefebvre94 8:fa346d946e7e 159
martlefebvre94 8:fa346d946e7e 160 magnetometer->set_m_lpf(LIS2MDL_LPF);
martlefebvre94 8:fa346d946e7e 161 magnetometer->get_m_lpf(&read_reg_int);
martlefebvre94 8:fa346d946e7e 162 printf("LIS2MDL LPF = %1d\r\n", read_reg_int);
martlefebvre94 8:fa346d946e7e 163
martlefebvre94 8:fa346d946e7e 164 magnetometer->set_m_comp_temp_en(LIS2MDL_COMP_TEMP_EN);
martlefebvre94 8:fa346d946e7e 165 magnetometer->get_m_comp_temp_en(&read_reg_int);
martlefebvre94 8:fa346d946e7e 166 printf("LIS2MDL COMP_TEMP_EN = %1d\r\n", read_reg_int);
martlefebvre94 8:fa346d946e7e 167
martlefebvre94 8:fa346d946e7e 168 magnetometer->set_m_off_canc(LIS2MDL_OFF_CANC);
martlefebvre94 8:fa346d946e7e 169 magnetometer->get_m_off_canc(&read_reg_int);
martlefebvre94 8:fa346d946e7e 170 printf("LIS2MDL OFF_CANC = %1d\r\n", read_reg_int);
martlefebvre94 8:fa346d946e7e 171
martlefebvre94 8:fa346d946e7e 172 /* LPS22HH pressure sensor configuration */
martlefebvre94 8:fa346d946e7e 173 press_temp->enable();
martlefebvre94 8:fa346d946e7e 174 printf("/***** LPS22HH pressure sensor configuration *****/\r\n");
martlefebvre94 8:fa346d946e7e 175
martlefebvre94 8:fa346d946e7e 176 press_temp->read_id(&id);
martlefebvre94 8:fa346d946e7e 177 printf("LPS22HH pressure = 0x%X\r\n", id);
martlefebvre94 8:fa346d946e7e 178
martlefebvre94 8:fa346d946e7e 179 press_temp->set_odr(LPS22HH_ODR, LPS22HH_LOW_NOISE_EN);
martlefebvre94 8:fa346d946e7e 180 press_temp->get_odr(&read_reg, &read_reg_int);
martlefebvre94 8:fa346d946e7e 181 printf("LPS22HH ODR = %1.1f [Hz]\r\n", read_reg);
martlefebvre94 8:fa346d946e7e 182 printf("LPS22HH LOW_NOISE_EN = %1d\r\n", read_reg_int);
martlefebvre94 8:fa346d946e7e 183
martlefebvre94 8:fa346d946e7e 184 press_temp->set_lpfp_cfg(LPS22HH_LPF_CFG);
martlefebvre94 8:fa346d946e7e 185 press_temp->get_lpfp_cfg(&read_reg_int);
martlefebvre94 8:fa346d946e7e 186 printf("LPS22HH LPF_CFG = %1d\r\n", read_reg_int);
martlefebvre94 8:fa346d946e7e 187
martlefebvre94 8:fa346d946e7e 188 /* LIS2DW12 accelerometer sensor configuration */
martlefebvre94 8:fa346d946e7e 189 accelerometer->enable_x();
martlefebvre94 8:fa346d946e7e 190 printf("/***** LIS2DW12 accelerometer sensor configuration *****/\r\n");
martlefebvre94 8:fa346d946e7e 191
martlefebvre94 8:fa346d946e7e 192 accelerometer->read_id(&id);
martlefebvre94 8:fa346d946e7e 193 printf("LIS2DW12 accelerometer = 0x%X\r\n", id);
martlefebvre94 8:fa346d946e7e 194
martlefebvre94 8:fa346d946e7e 195 accelerometer->set_x_odr(LIS2DW12_ODR);
martlefebvre94 8:fa346d946e7e 196 accelerometer->get_x_odr(&read_reg);
martlefebvre94 8:fa346d946e7e 197 printf("LIS2DW12 ODR = %1.3f [Hz]\r\n", read_reg);
martlefebvre94 8:fa346d946e7e 198
martlefebvre94 8:fa346d946e7e 199 accelerometer->set_x_fs(LIS2DW12_FS);
martlefebvre94 8:fa346d946e7e 200 accelerometer->get_x_fs(&read_reg);
martlefebvre94 8:fa346d946e7e 201 printf("LIS2DW12 FS = %1.3f [g]\r\n", read_reg);
martlefebvre94 8:fa346d946e7e 202
martlefebvre94 8:fa346d946e7e 203 accelerometer->set_x_bw_filt(LIS2DW12_BW_FILT);
martlefebvre94 8:fa346d946e7e 204 accelerometer->get_x_bw_filt(&read_reg_int);
martlefebvre94 8:fa346d946e7e 205 printf("LIS2DW12 BW_FILT = %1d\r\n", read_reg_int);
martlefebvre94 8:fa346d946e7e 206
martlefebvre94 8:fa346d946e7e 207 accelerometer->set_x_power_mode(LIS2DW12_POWER_MODE);
martlefebvre94 8:fa346d946e7e 208 accelerometer->get_x_power_mode(&read_reg_int, &read_reg_int_1, &read_reg_int_2);
martlefebvre94 8:fa346d946e7e 209 printf("LIS2DW12 LP_MODE = %1d\r\n", read_reg_int);
martlefebvre94 8:fa346d946e7e 210 printf("LIS2DW12 MODE = %1d\r\n", read_reg_int_1);
martlefebvre94 8:fa346d946e7e 211 printf("LIS2DW12 LOW_NOISE = %1d\r\n", read_reg_int_2);
martlefebvre94 8:fa346d946e7e 212
martlefebvre94 8:fa346d946e7e 213 /* HTS221 relative humidity and temperature sensor configuration */
martlefebvre94 8:fa346d946e7e 214 hum_temp->enable();
martlefebvre94 8:fa346d946e7e 215 printf("/***** HTS221 humidity sensor configuration *****/\r\n");
martlefebvre94 8:fa346d946e7e 216
martlefebvre94 8:fa346d946e7e 217 hum_temp->read_id(&id);
martlefebvre94 8:fa346d946e7e 218 printf("HTS221 humidity & temperature = 0x%X\r\n", id);
martlefebvre94 8:fa346d946e7e 219
martlefebvre94 8:fa346d946e7e 220 hum_temp->set_odr(HTS221_ODR);
martlefebvre94 8:fa346d946e7e 221 hum_temp->get_odr(&read_reg);
martlefebvre94 8:fa346d946e7e 222 printf("HTS221 ODR = %1.3f [Hz]\r\n", read_reg);
martlefebvre94 8:fa346d946e7e 223
martlefebvre94 8:fa346d946e7e 224 hum_temp->set_heater(HTS221_HEATER);
martlefebvre94 8:fa346d946e7e 225 hum_temp->get_heater(&read_reg_int);
martlefebvre94 8:fa346d946e7e 226 printf("HTS221 HEATER = %1d\r\n", read_reg_int);
martlefebvre94 8:fa346d946e7e 227
martlefebvre94 8:fa346d946e7e 228 hum_temp->set_avg(HTS221_AVGH, HTS221_AVGT);
martlefebvre94 8:fa346d946e7e 229 hum_temp->get_avg(&read_reg, &read_reg_1);
martlefebvre94 8:fa346d946e7e 230 printf("HTS221 AVGH = %1.0f\r\n", read_reg);
martlefebvre94 8:fa346d946e7e 231 printf("HTS221 AVGT = %1.0f\r\n", read_reg_1);
martlefebvre94 8:fa346d946e7e 232
martlefebvre94 8:fa346d946e7e 233 /* STTS751 Temperature sensor configuration */
martlefebvre94 8:fa346d946e7e 234 temp->enable();
martlefebvre94 8:fa346d946e7e 235 printf("/***** STTS751 temperature sensor configuration *****/\r\n");
martlefebvre94 8:fa346d946e7e 236
martlefebvre94 8:fa346d946e7e 237 temp->read_id(&id);
martlefebvre94 8:fa346d946e7e 238 printf("STTS751 temperature = 0x%X\r\n", id);
martlefebvre94 8:fa346d946e7e 239
martlefebvre94 8:fa346d946e7e 240 /* LSM6DSO Accelerometer and gyroscope configuration */
martlefebvre94 8:fa346d946e7e 241 acc_gyro->enable_x();
martlefebvre94 8:fa346d946e7e 242 acc_gyro->enable_g();
martlefebvre94 8:fa346d946e7e 243 printf("/***** LSM6DSO accelerometer and gyroscope sensor configuration *****/\r\n");
martlefebvre94 9:7cda26534126 244 printf("Test\r\n");
martlefebvre94 8:fa346d946e7e 245
martlefebvre94 8:fa346d946e7e 246 acc_gyro->read_id(&id);
martlefebvre94 8:fa346d946e7e 247 printf("LSM6DSO accelerometer & gyroscope = 0x%X\r\n", id);
martlefebvre94 8:fa346d946e7e 248
martlefebvre94 8:fa346d946e7e 249 acc_gyro->set_x_odr(LSM6DSO_ODR_XL);
martlefebvre94 8:fa346d946e7e 250 acc_gyro->get_x_odr(&read_reg);
martlefebvre94 8:fa346d946e7e 251 printf("LSM6DSO ODR_XL = %1.3f [Hz]\r\n", read_reg);
martlefebvre94 8:fa346d946e7e 252
martlefebvre94 8:fa346d946e7e 253 acc_gyro->set_x_fs(LSM6DSO_FS_XL);
martlefebvre94 8:fa346d946e7e 254 acc_gyro->get_x_fs(&read_reg);
martlefebvre94 8:fa346d946e7e 255 printf("LSM6DSO FS_XL = %1.3f [g]\r\n", read_reg);
martlefebvre94 8:fa346d946e7e 256
martlefebvre94 8:fa346d946e7e 257 acc_gyro->set_x_power_mode(LSM6DSO_XL_HM_MODE, LSM6DSO_XL_ULP_EN);
martlefebvre94 8:fa346d946e7e 258 acc_gyro->get_x_power_mode(&read_reg_int, &read_reg_int_1);
martlefebvre94 8:fa346d946e7e 259 printf("LSM6DSO XL_HM_MODE = %1d\r\n", read_reg_int);
martlefebvre94 8:fa346d946e7e 260 printf("LSM6DSO XL_ULP_EN = %1d\r\n", read_reg_int_1);
martlefebvre94 8:fa346d946e7e 261
martlefebvre94 8:fa346d946e7e 262 acc_gyro->set_g_odr(LSM6DSO_ODR_G);
martlefebvre94 8:fa346d946e7e 263 acc_gyro->get_g_odr(&read_reg);
martlefebvre94 8:fa346d946e7e 264 printf("LSM6DSO ODR_G = %1.3f [Hz]\r\n", read_reg);
martlefebvre94 8:fa346d946e7e 265
martlefebvre94 8:fa346d946e7e 266 acc_gyro->set_g_fs(LSM6DSO_FS_XL);
martlefebvre94 8:fa346d946e7e 267 acc_gyro->get_g_fs(&read_reg);
martlefebvre94 8:fa346d946e7e 268 printf("LSM6DSO FS_G = %1.3f [dps]\r\n", read_reg);
martlefebvre94 8:fa346d946e7e 269
martlefebvre94 8:fa346d946e7e 270 /* Print Flash memory information */
martlefebvre94 8:fa346d946e7e 271 print_flash_info();
martlefebvre94 8:fa346d946e7e 272
martlefebvre94 8:fa346d946e7e 273 /* Information for the user */
martlefebvre94 8:fa346d946e7e 274 printf("Press blue button to start data acquisition\r\n");
martlefebvre94 8:fa346d946e7e 275 printf("Press 'R' to read previously measured data\r\n");
martlefebvre94 8:fa346d946e7e 276
martlefebvre94 8:fa346d946e7e 277 /* Acquisition loop */
martlefebvre94 8:fa346d946e7e 278 while(1) {
martlefebvre94 8:fa346d946e7e 279 // Start saving data when button is pushed
martlefebvre94 8:fa346d946e7e 280 if (button1_pressed) {
martlefebvre94 8:fa346d946e7e 281 button1_pressed = false;
martlefebvre94 8:fa346d946e7e 282 save_data = true;
martlefebvre94 8:fa346d946e7e 283 erase_flash(false);
martlefebvre94 8:fa346d946e7e 284 printf("Acquiring data...\r\n");
martlefebvre94 8:fa346d946e7e 285 printf("Press blue button to stop data acquisition\r\n");
martlefebvre94 8:fa346d946e7e 286 Flash_addr = FLASH_BANK2_BASE;
martlefebvre94 8:fa346d946e7e 287 }
martlefebvre94 8:fa346d946e7e 288
martlefebvre94 8:fa346d946e7e 289 if (save_data) {
martlefebvre94 8:fa346d946e7e 290 // Acquisition task
martlefebvre94 8:fa346d946e7e 291 save_data = acquisition_task(true);
martlefebvre94 8:fa346d946e7e 292 }
martlefebvre94 8:fa346d946e7e 293 else {
martlefebvre94 8:fa346d946e7e 294 // Read task
martlefebvre94 8:fa346d946e7e 295 read_task();
martlefebvre94 8:fa346d946e7e 296 }
martlefebvre94 8:fa346d946e7e 297 }
martlefebvre94 8:fa346d946e7e 298 }
martlefebvre94 8:fa346d946e7e 299
martlefebvre94 8:fa346d946e7e 300 /* Acquisition task */
martlefebvre94 8:fa346d946e7e 301 bool acquisition_task(bool verbose)
martlefebvre94 8:fa346d946e7e 302 {
martlefebvre94 8:fa346d946e7e 303 int32_t m_axes[3];
martlefebvre94 8:fa346d946e7e 304 int32_t acc_axes[3];
martlefebvre94 8:fa346d946e7e 305 int32_t acc_axes_1[3];
martlefebvre94 8:fa346d946e7e 306 int32_t gyro_axes[3];
martlefebvre94 8:fa346d946e7e 307 float pressure_value, hum_value, temp_value, temp_value_1;
martlefebvre94 8:fa346d946e7e 308 char buffer[32];
martlefebvre94 8:fa346d946e7e 309
martlefebvre94 8:fa346d946e7e 310 uint32_t buffer_size = FLASH_PAGE_SIZE/4;
martlefebvre94 8:fa346d946e7e 311 uint32_t data_ind = 0;
martlefebvre94 8:fa346d946e7e 312 uint32_t data_buffer[buffer_size];
martlefebvre94 8:fa346d946e7e 313
martlefebvre94 8:fa346d946e7e 314 uint32_t Flash_addr = FLASH_BANK2_BASE;
martlefebvre94 8:fa346d946e7e 315
martlefebvre94 8:fa346d946e7e 316 while (Flash_addr <= FLASH_BANK2_END-FLASH_PAGE_SIZE+1) {
martlefebvre94 8:fa346d946e7e 317 // Read sensors data
martlefebvre94 8:fa346d946e7e 318 magnetometer->get_m_axes(m_axes);
martlefebvre94 8:fa346d946e7e 319 press_temp->get_pressure(&pressure_value);
martlefebvre94 8:fa346d946e7e 320 accelerometer->get_x_axes(acc_axes);
martlefebvre94 8:fa346d946e7e 321 hum_temp->get_temperature(&temp_value);
martlefebvre94 8:fa346d946e7e 322 hum_temp->get_humidity(&hum_value);
martlefebvre94 8:fa346d946e7e 323 temp->get_temperature(&temp_value_1);
martlefebvre94 8:fa346d946e7e 324 acc_gyro->get_x_axes(acc_axes_1);
martlefebvre94 8:fa346d946e7e 325 acc_gyro->get_g_axes(gyro_axes);
martlefebvre94 8:fa346d946e7e 326
martlefebvre94 8:fa346d946e7e 327 // Save data to Flash memory
martlefebvre94 8:fa346d946e7e 328 for (int i=0; i<3; i++) {
martlefebvre94 8:fa346d946e7e 329 // Write page in Flash memory
martlefebvre94 8:fa346d946e7e 330 if (data_ind >= buffer_size) {
martlefebvre94 8:fa346d946e7e 331 write_flash(Flash_addr, &data_buffer[0], buffer_size, false);
martlefebvre94 8:fa346d946e7e 332 Flash_addr += FLASH_PAGE_SIZE;
martlefebvre94 8:fa346d946e7e 333 data_ind = 0;
martlefebvre94 8:fa346d946e7e 334 }
martlefebvre94 8:fa346d946e7e 335
martlefebvre94 8:fa346d946e7e 336 // Write data to buffer
martlefebvre94 8:fa346d946e7e 337 data_buffer[data_ind] = (uint32_t) m_axes[i];
martlefebvre94 8:fa346d946e7e 338 data_ind++;
martlefebvre94 8:fa346d946e7e 339 }
martlefebvre94 8:fa346d946e7e 340
martlefebvre94 8:fa346d946e7e 341 // Print data in terminal
martlefebvre94 8:fa346d946e7e 342 if (verbose) {
martlefebvre94 8:fa346d946e7e 343 printf("LIS2MDL: [mag/mgauss] %6d, %6d, %6d\r\n", ((uint32_t) m_axes[0]), ((uint32_t) m_axes[1]), ((uint32_t) m_axes[2]));
martlefebvre94 8:fa346d946e7e 344 printf("LPS22HH: [press/mbar] %1.3f, [alt/m] %1.3f\r\n", pressure_value, pressure_to_altitude(pressure_value));
martlefebvre94 8:fa346d946e7e 345 printf("HTS221: [temp/deg C] %1.3f, [hum/%%] %1.3f\r\n", temp_value, hum_value);
martlefebvre94 8:fa346d946e7e 346 printf("STTS751 [temp/deg C] %1.3f\r\n", temp_value_1);
martlefebvre94 8:fa346d946e7e 347 printf("LIS2DW12: [acc/mg] %6d, %6d, %6d\r\n", ((uint32_t) acc_axes[0]), ((uint32_t) acc_axes[1]), ((uint32_t) acc_axes[2]));
martlefebvre94 8:fa346d946e7e 348 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]));
martlefebvre94 8:fa346d946e7e 349 printf("LSM6DSO: [gyro/mdps] %6d, %6d, %6d\r\n", ((uint32_t) gyro_axes[0]), ((uint32_t) gyro_axes[1]), ((uint32_t) gyro_axes[2]));
martlefebvre94 8:fa346d946e7e 350 }
martlefebvre94 8:fa346d946e7e 351
martlefebvre94 8:fa346d946e7e 352 // Wait for acquisition period
martlefebvre94 8:fa346d946e7e 353 wait(1/FS);
martlefebvre94 8:fa346d946e7e 354
martlefebvre94 8:fa346d946e7e 355 // Stop saving data when button is pushed
martlefebvre94 8:fa346d946e7e 356 if (button1_pressed) {
martlefebvre94 8:fa346d946e7e 357 button1_pressed = false;
martlefebvre94 8:fa346d946e7e 358 // Save remaining data to Flash memory
martlefebvre94 8:fa346d946e7e 359 write_flash(Flash_addr, &data_buffer[0], data_ind, false);
martlefebvre94 8:fa346d946e7e 360 printf("Data acquisition stopped\r\n");
martlefebvre94 8:fa346d946e7e 361 printf("Press 'R' to read the data\r\n");
martlefebvre94 8:fa346d946e7e 362 return false;
martlefebvre94 8:fa346d946e7e 363 }
martlefebvre94 8:fa346d946e7e 364 }
martlefebvre94 8:fa346d946e7e 365 printf("Data acquisition stopped\r\n");
martlefebvre94 8:fa346d946e7e 366 printf("Press 'R' to read the data\r\n");
martlefebvre94 8:fa346d946e7e 367 return false;
martlefebvre94 8:fa346d946e7e 368 }
martlefebvre94 8:fa346d946e7e 369
martlefebvre94 8:fa346d946e7e 370 /* Read task */
martlefebvre94 8:fa346d946e7e 371 void read_task()
martlefebvre94 8:fa346d946e7e 372 {
martlefebvre94 8:fa346d946e7e 373 char pc_input;
martlefebvre94 8:fa346d946e7e 374 uint32_t Flash_rdata[3];
martlefebvre94 8:fa346d946e7e 375 bool flash_empty = false;
martlefebvre94 8:fa346d946e7e 376
martlefebvre94 8:fa346d946e7e 377 // Read terminal input
martlefebvre94 8:fa346d946e7e 378 if (pc.readable()) {
martlefebvre94 8:fa346d946e7e 379 pc_input = pc.getc();
martlefebvre94 8:fa346d946e7e 380 }
martlefebvre94 8:fa346d946e7e 381 else {
martlefebvre94 8:fa346d946e7e 382 pc_input = ' ';
martlefebvre94 8:fa346d946e7e 383 }
martlefebvre94 8:fa346d946e7e 384
martlefebvre94 8:fa346d946e7e 385 // Read Flash memory if 'R' is pressed
martlefebvre94 8:fa346d946e7e 386 if ((pc_input == 'r') || (pc_input == 'R')) {
martlefebvre94 8:fa346d946e7e 387 // Data labels
martlefebvre94 8:fa346d946e7e 388 printf("mag_X\tmag_Y\tmag_Z\r\n");
martlefebvre94 8:fa346d946e7e 389
martlefebvre94 8:fa346d946e7e 390 // Read 1st Flash data
martlefebvre94 8:fa346d946e7e 391 uint32_t Flash_addr_temp = FLASH_BANK2_BASE;
martlefebvre94 8:fa346d946e7e 392 read_flash(Flash_addr_temp, &Flash_rdata[0], LIS2MDL_DATA_SIZE);
martlefebvre94 8:fa346d946e7e 393
martlefebvre94 8:fa346d946e7e 394 // Read Flash data
martlefebvre94 8:fa346d946e7e 395 while ((Flash_addr_temp <= FLASH_BANK2_END-LIS2MDL_DATA_SIZE+1) && !flash_empty) {
martlefebvre94 8:fa346d946e7e 396 // Print read data in the terminal
martlefebvre94 8:fa346d946e7e 397 printf("%6d\t%6d\t%6d\r\n", Flash_rdata[0], Flash_rdata[1], Flash_rdata[2]);
martlefebvre94 8:fa346d946e7e 398 Flash_addr_temp += LIS2MDL_DATA_SIZE;
martlefebvre94 8:fa346d946e7e 399
martlefebvre94 8:fa346d946e7e 400 // Check if the next address is not empty (erased Flash only contains 0)
martlefebvre94 8:fa346d946e7e 401 if (Flash_addr_temp <= FLASH_BANK2_END-LIS2MDL_DATA_SIZE+1) {
martlefebvre94 8:fa346d946e7e 402 read_flash(Flash_addr_temp, &Flash_rdata[0], LIS2MDL_DATA_SIZE);
martlefebvre94 8:fa346d946e7e 403 if ((Flash_rdata[0] == 0) && (Flash_rdata[1] == 0) && (Flash_rdata[2] == 0)) {
martlefebvre94 8:fa346d946e7e 404 flash_empty = true;
martlefebvre94 8:fa346d946e7e 405 }
martlefebvre94 8:fa346d946e7e 406 }
martlefebvre94 8:fa346d946e7e 407 }
martlefebvre94 8:fa346d946e7e 408 }
martlefebvre94 8:fa346d946e7e 409 }
martlefebvre94 8:fa346d946e7e 410
martlefebvre94 8:fa346d946e7e 411 /* Print Flash memory info */
martlefebvre94 8:fa346d946e7e 412 void print_flash_info()
martlefebvre94 8:fa346d946e7e 413 {
martlefebvre94 8:fa346d946e7e 414 printf("**************************************************\n\r");
martlefebvre94 8:fa346d946e7e 415 printf("/***** Flash memory info *****/\r\n");
martlefebvre94 8:fa346d946e7e 416 printf("Flash size: %d [B]\r\n", FLASH_SIZE);
martlefebvre94 8:fa346d946e7e 417 printf("Flash page size: %d [B]\r\n", FLASH_PAGE_SIZE);
martlefebvre94 8:fa346d946e7e 418 printf("Flash nb of pages: %d \r\n", FLASH_SIZE/FLASH_PAGE_SIZE);
martlefebvre94 8:fa346d946e7e 419 printf("Flash bank 1 base address: 0x%X\r\n", FLASH_BASE);
martlefebvre94 8:fa346d946e7e 420 printf("Flash bank 1 end address: 0x%X\r\n", FLASH_BANK1_END);
martlefebvre94 8:fa346d946e7e 421 printf("Flash bank 2 base address: 0x%X\r\n", FLASH_BANK2_BASE);
martlefebvre94 8:fa346d946e7e 422 printf("Flash bank 2 end address: 0x%X\r\n", FLASH_BANK2_END);
martlefebvre94 8:fa346d946e7e 423 printf("**************************************************\n\r");
martlefebvre94 8:fa346d946e7e 424 }
martlefebvre94 8:fa346d946e7e 425
martlefebvre94 6:b2e247935342 426 /* Erase content of Flash memory */
martlefebvre94 6:b2e247935342 427 bool erase_flash(bool verbose)
cparata 0:535249dc4bf5 428 {
martlefebvre94 6:b2e247935342 429 printf("Erasing Flash memory...\r\n");
martlefebvre94 6:b2e247935342 430
martlefebvre94 6:b2e247935342 431 // Unlock Flash memory
martlefebvre94 6:b2e247935342 432 HAL_FLASH_Unlock();
cparata 0:535249dc4bf5 433
martlefebvre94 6:b2e247935342 434 // Erase Flash memory
martlefebvre94 6:b2e247935342 435 FLASH_EraseInitTypeDef eraser;
martlefebvre94 6:b2e247935342 436 uint32_t Flash_addr = FLASH_BANK2_BASE;
martlefebvre94 6:b2e247935342 437 uint32_t page_error = 0;
martlefebvre94 6:b2e247935342 438 int32_t page = 1;
martlefebvre94 6:b2e247935342 439
martlefebvre94 6:b2e247935342 440 while (Flash_addr < FLASH_BANK2_END) {
martlefebvre94 6:b2e247935342 441 eraser.TypeErase = FLASH_TYPEERASE_PAGES;
martlefebvre94 6:b2e247935342 442 eraser.PageAddress = Flash_addr;
martlefebvre94 6:b2e247935342 443 eraser.NbPages = 1;
martlefebvre94 6:b2e247935342 444 if(HAL_OK != HAL_FLASHEx_Erase(&eraser, &page_error)) {
martlefebvre94 6:b2e247935342 445 if (verbose) {printf("Flash erase failed!\r\n");}
martlefebvre94 6:b2e247935342 446 printf("Error 0x%X\r\n", page_error);
martlefebvre94 6:b2e247935342 447 HAL_FLASH_Lock();
martlefebvre94 6:b2e247935342 448 return false;
martlefebvre94 6:b2e247935342 449 }
martlefebvre94 6:b2e247935342 450 if (verbose) {printf("Erased page %d at address: 0x%X\r\n", page, Flash_addr);}
martlefebvre94 6:b2e247935342 451 Flash_addr += FLASH_PAGE_SIZE;
martlefebvre94 6:b2e247935342 452 page++;
martlefebvre94 6:b2e247935342 453 }
martlefebvre94 6:b2e247935342 454
martlefebvre94 6:b2e247935342 455 if (verbose) {printf("Flash erase succesful!\r\n");}
martlefebvre94 6:b2e247935342 456 return true;
martlefebvre94 6:b2e247935342 457 }
cparata 0:535249dc4bf5 458
martlefebvre94 6:b2e247935342 459 /* Write Flash memory */
martlefebvre94 8:fa346d946e7e 460 bool write_flash(uint32_t Flash_addr, uint32_t* Flash_wdata, int32_t n_words, bool verbose)
martlefebvre94 6:b2e247935342 461 {
martlefebvre94 8:fa346d946e7e 462 clock_t time;
martlefebvre94 8:fa346d946e7e 463 if (verbose) {time = clock();}
martlefebvre94 8:fa346d946e7e 464
martlefebvre94 6:b2e247935342 465 // Unlock Flash memory
martlefebvre94 6:b2e247935342 466 HAL_FLASH_Unlock();
martlefebvre94 6:b2e247935342 467
martlefebvre94 6:b2e247935342 468 // Write Flash memory
martlefebvre94 8:fa346d946e7e 469 for (int i=0; i<n_words; i++) {
martlefebvre94 8:fa346d946e7e 470 if (HAL_OK != HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, Flash_addr, Flash_wdata[i])) {
martlefebvre94 8:fa346d946e7e 471 if (verbose) {printf("Flash write failed!\r\n");}
martlefebvre94 8:fa346d946e7e 472 HAL_FLASH_Lock();
martlefebvre94 8:fa346d946e7e 473 return false;
martlefebvre94 8:fa346d946e7e 474 }
martlefebvre94 8:fa346d946e7e 475 Flash_addr += 4;
martlefebvre94 6:b2e247935342 476 }
martlefebvre94 6:b2e247935342 477 if (verbose) {printf("Flash write succesful!\r\n");}
martlefebvre94 8:fa346d946e7e 478
martlefebvre94 6:b2e247935342 479 HAL_FLASH_Lock();
martlefebvre94 8:fa346d946e7e 480
martlefebvre94 8:fa346d946e7e 481 if (verbose) {
martlefebvre94 8:fa346d946e7e 482 time = clock() - time;
martlefebvre94 8:fa346d946e7e 483 printf("Time to write: %1.6f [s]\r\n", (((double) time)/CLOCKS_PER_SEC));
martlefebvre94 8:fa346d946e7e 484 }
martlefebvre94 8:fa346d946e7e 485
martlefebvre94 6:b2e247935342 486 return true;
martlefebvre94 6:b2e247935342 487 }
cparata 0:535249dc4bf5 488
martlefebvre94 6:b2e247935342 489 /* Read Flash memory */
martlefebvre94 6:b2e247935342 490 void read_flash(uint32_t Flash_addr, uint32_t* Flash_rdata, uint32_t n_bytes)
martlefebvre94 6:b2e247935342 491 {
martlefebvre94 6:b2e247935342 492 memcpy(Flash_rdata, (uint32_t*) Flash_addr, n_bytes);
martlefebvre94 6:b2e247935342 493 }
cparata 0:535249dc4bf5 494
martlefebvre94 6:b2e247935342 495 /* Enables button when bouncing is over */
martlefebvre94 6:b2e247935342 496 void button1_enabled_cb(void)
martlefebvre94 6:b2e247935342 497 {
martlefebvre94 6:b2e247935342 498 button1_enabled = true;
cparata 0:535249dc4bf5 499 }
cparata 0:535249dc4bf5 500
martlefebvre94 6:b2e247935342 501 /* ISR handling button pressed event */
martlefebvre94 6:b2e247935342 502 void button1_onpressed_cb(void)
martlefebvre94 6:b2e247935342 503 {
martlefebvre94 6:b2e247935342 504 if (button1_enabled) { // Disabled while the button is bouncing
martlefebvre94 6:b2e247935342 505 button1_enabled = false;
martlefebvre94 6:b2e247935342 506 button1_pressed = true; // To be read by the main loop
martlefebvre94 6:b2e247935342 507 button1_timeout.attach(callback(button1_enabled_cb), 0.3); // Debounce time 300 ms
martlefebvre94 6:b2e247935342 508 }
martlefebvre94 6:b2e247935342 509 }
martlefebvre94 6:b2e247935342 510
martlefebvre94 8:fa346d946e7e 511 /* Helper function for printing floats & doubles */
martlefebvre94 8:fa346d946e7e 512 static char *print_double(char *str, double v)
martlefebvre94 6:b2e247935342 513 {
martlefebvre94 8:fa346d946e7e 514 int decimalDigits = 6;
martlefebvre94 8:fa346d946e7e 515 int i = 1;
martlefebvre94 8:fa346d946e7e 516 int intPart, fractPart;
martlefebvre94 8:fa346d946e7e 517 int len;
martlefebvre94 8:fa346d946e7e 518 char *ptr;
martlefebvre94 8:fa346d946e7e 519
martlefebvre94 8:fa346d946e7e 520 /* prepare decimal digits multiplicator */
martlefebvre94 8:fa346d946e7e 521 for (; decimalDigits != 0; i *= 10, decimalDigits--);
martlefebvre94 8:fa346d946e7e 522
martlefebvre94 8:fa346d946e7e 523 /* calculate integer & fractinal parts */
martlefebvre94 8:fa346d946e7e 524 intPart = (int)v;
martlefebvre94 8:fa346d946e7e 525 fractPart = (int)((v - (double)(int)v) * i);
martlefebvre94 8:fa346d946e7e 526
martlefebvre94 8:fa346d946e7e 527 /* fill in integer part */
martlefebvre94 8:fa346d946e7e 528 sprintf(str, "%i.", intPart);
martlefebvre94 8:fa346d946e7e 529
martlefebvre94 8:fa346d946e7e 530 /* prepare fill in of fractional part */
martlefebvre94 8:fa346d946e7e 531 len = strlen(str);
martlefebvre94 8:fa346d946e7e 532 ptr = &str[len];
martlefebvre94 8:fa346d946e7e 533
martlefebvre94 8:fa346d946e7e 534 /* fill in leading fractional zeros */
martlefebvre94 8:fa346d946e7e 535 for (i /= 10; i > 1; i /= 10, ptr++) {
martlefebvre94 8:fa346d946e7e 536 if (fractPart >= i) {
martlefebvre94 8:fa346d946e7e 537 break;
martlefebvre94 6:b2e247935342 538 }
martlefebvre94 8:fa346d946e7e 539 *ptr = '0';
martlefebvre94 6:b2e247935342 540 }
martlefebvre94 6:b2e247935342 541
martlefebvre94 8:fa346d946e7e 542 /* fill in (rest of) fractional part */
martlefebvre94 8:fa346d946e7e 543 sprintf(ptr, "%i", fractPart);
martlefebvre94 8:fa346d946e7e 544
martlefebvre94 8:fa346d946e7e 545 return str;
martlefebvre94 6:b2e247935342 546 }
martlefebvre94 6:b2e247935342 547
martlefebvre94 8:fa346d946e7e 548 /* Pressure to altitude conversion */
martlefebvre94 8:fa346d946e7e 549 float pressure_to_altitude(double pressure)
cparata 5:7c883cce2bc4 550 {
martlefebvre94 8:fa346d946e7e 551 return 44330.77 * (1-pow(pressure/P0, 0.1902632));
martlefebvre94 8:fa346d946e7e 552 }