Steven Mitchell / Mbed 2 deprecated HelloWorld_IKS01A2

afdfdf

Dependencies:   mbed weelio

main.cpp@18:9b9ec0b35b45, 2019-11-19 (annotated)

Committer:
mitch092
Date:
Tue Nov 19 00:42:38 2019 +0000
Revision:
18:9b9ec0b35b45
Parent:
13:fc873da5b445
Child:
19:29d3c7caea66
afdasf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cparata 0:69566eea0fba 1 /**
cparata 0:69566eea0fba 2 ******************************************************************************
cparata 0:69566eea0fba 3 * @file main.cpp
cparata 8:8f495e604424 4 * @author CLab
cparata 8:8f495e604424 5 * @version V1.0.0
cparata 8:8f495e604424 6 * @date 2-December-2016
mitch092 18:9b9ec0b35b45 7 * @brief Simple Example application for using the X_NUCLEO_IKS01A1
cparata 0:69566eea0fba 8 * MEMS Inertial & Environmental Sensor Nucleo expansion board.
cparata 0:69566eea0fba 9 ******************************************************************************
cparata 0:69566eea0fba 10 * @attention
cparata 0:69566eea0fba 11 *
cparata 0:69566eea0fba 12 * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
cparata 0:69566eea0fba 13 *
cparata 0:69566eea0fba 14 * Redistribution and use in source and binary forms, with or without modification,
cparata 0:69566eea0fba 15 * are permitted provided that the following conditions are met:
cparata 0:69566eea0fba 16 * 1. Redistributions of source code must retain the above copyright notice,
cparata 0:69566eea0fba 17 * this list of conditions and the following disclaimer.
cparata 0:69566eea0fba 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
cparata 0:69566eea0fba 19 * this list of conditions and the following disclaimer in the documentation
cparata 0:69566eea0fba 20 * and/or other materials provided with the distribution.
cparata 0:69566eea0fba 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
cparata 0:69566eea0fba 22 * may be used to endorse or promote products derived from this software
cparata 0:69566eea0fba 23 * without specific prior written permission.
cparata 0:69566eea0fba 24 *
cparata 0:69566eea0fba 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
cparata 0:69566eea0fba 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
cparata 0:69566eea0fba 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
cparata 0:69566eea0fba 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
cparata 0:69566eea0fba 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
cparata 0:69566eea0fba 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
cparata 0:69566eea0fba 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
cparata 0:69566eea0fba 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
cparata 0:69566eea0fba 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
cparata 0:69566eea0fba 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
cparata 0:69566eea0fba 35 *
cparata 0:69566eea0fba 36 ******************************************************************************
mitch092 18:9b9ec0b35b45 37 */
cparata 0:69566eea0fba 38
cparata 0:69566eea0fba 39 /* Includes */
cparata 0:69566eea0fba 40 #include "mbed.h"
davide.aliprandi@st.com 13:fc873da5b445 41 #include "XNucleoIKS01A2.h"
mitch092 18:9b9ec0b35b45 42 #include <iostream>
mitch092 18:9b9ec0b35b45 43 #include <cmath>
cparata 0:69566eea0fba 44
cparata 0:69566eea0fba 45 /* Instantiate the expansion board */
davide.aliprandi@st.com 13:fc873da5b445 46 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
cparata 0:69566eea0fba 47
cparata 0:69566eea0fba 48 /* Retrieve the composing elements of the expansion board */
davide.aliprandi@st.com 13:fc873da5b445 49 static LSM303AGRMagSensor *magnetometer = mems_expansion_board->magnetometer;
cparata 0:69566eea0fba 50 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
davide.aliprandi@st.com 13:fc873da5b445 51 static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
cparata 0:69566eea0fba 52
mitch092 18:9b9ec0b35b45 53
mitch092 18:9b9ec0b35b45 54
mitch092 18:9b9ec0b35b45 55
mitch092 18:9b9ec0b35b45 56
mitch092 18:9b9ec0b35b45 57 struct Vec3 {
mitch092 18:9b9ec0b35b45 58 float x, y, z;
mitch092 18:9b9ec0b35b45 59 };
mitch092 18:9b9ec0b35b45 60
mitch092 18:9b9ec0b35b45 61
mitch092 18:9b9ec0b35b45 62 static const Vec3 gyro_calibrate = {-0.42f, -1.61f, 1.05f};
cparata 0:69566eea0fba 63
mitch092 18:9b9ec0b35b45 64 Vec3 get_gyro()
mitch092 18:9b9ec0b35b45 65 {
mitch092 18:9b9ec0b35b45 66 Vec3 vec3;
cparata 0:69566eea0fba 67
mitch092 18:9b9ec0b35b45 68 int32_t axes[3];
mitch092 18:9b9ec0b35b45 69 acc_gyro->get_g_axes(axes);
cparata 0:69566eea0fba 70
mitch092 18:9b9ec0b35b45 71 vec3.x = axes[0]/1000.0f - gyro_calibrate.x;
mitch092 18:9b9ec0b35b45 72 vec3.y = axes[1]/1000.0f - gyro_calibrate.y;
mitch092 18:9b9ec0b35b45 73 vec3.z = axes[2]/1000.0f - gyro_calibrate.z;
cparata 0:69566eea0fba 74
mitch092 18:9b9ec0b35b45 75 return vec3;
mitch092 18:9b9ec0b35b45 76 }
mitch092 18:9b9ec0b35b45 77
mitch092 18:9b9ec0b35b45 78 Vec3 get_accel()
mitch092 18:9b9ec0b35b45 79 {
mitch092 18:9b9ec0b35b45 80 Vec3 vec3;
mitch092 18:9b9ec0b35b45 81 int32_t axes[3];
mitch092 18:9b9ec0b35b45 82 acc_gyro->get_x_axes(axes);
cparata 0:69566eea0fba 83
mitch092 18:9b9ec0b35b45 84 vec3.x = axes[0]/1000.0f + 0.013f;
mitch092 18:9b9ec0b35b45 85 vec3.y = axes[1]/1000.0f + 0.004f;
mitch092 18:9b9ec0b35b45 86 vec3.z = axes[2]/1000.0f - 0.032f;
mitch092 18:9b9ec0b35b45 87
mitch092 18:9b9ec0b35b45 88 return vec3;
mitch092 18:9b9ec0b35b45 89 }
cparata 0:69566eea0fba 90
mitch092 18:9b9ec0b35b45 91
cparata 0:69566eea0fba 92
mitch092 18:9b9ec0b35b45 93 void print_vec3(const char* str, const Vec3& vec3)
mitch092 18:9b9ec0b35b45 94 {
mitch092 18:9b9ec0b35b45 95 std::cout << str << vec3.x << " " << vec3.y << " " << vec3.z << "\r\n";
cparata 0:69566eea0fba 96 }
cparata 0:69566eea0fba 97
mitch092 18:9b9ec0b35b45 98
cparata 0:69566eea0fba 99 /* Simple main function */
mitch092 18:9b9ec0b35b45 100 int main()
mitch092 18:9b9ec0b35b45 101 {
mitch092 18:9b9ec0b35b45 102
mitch092 18:9b9ec0b35b45 103 /* Enable all sensors */
mitch092 18:9b9ec0b35b45 104 magnetometer->enable();
mitch092 18:9b9ec0b35b45 105 accelerometer->enable();
mitch092 18:9b9ec0b35b45 106 acc_gyro->enable_x();
mitch092 18:9b9ec0b35b45 107 acc_gyro->enable_g();
mitch092 18:9b9ec0b35b45 108
mitch092 18:9b9ec0b35b45 109 //Timer t;
mitch092 18:9b9ec0b35b45 110 //float dt = 0.02f;
mitch092 18:9b9ec0b35b45 111
mitch092 18:9b9ec0b35b45 112 std::cout << "\r\n--- Starting new run ---\r\n";
mitch092 18:9b9ec0b35b45 113
mitch092 18:9b9ec0b35b45 114 //Vec3 orientation = {};
mitch092 18:9b9ec0b35b45 115
mitch092 18:9b9ec0b35b45 116 //float sens = 0.488f;
mitch092 18:9b9ec0b35b45 117
mitch092 18:9b9ec0b35b45 118 Vec3 vec3, tilt;
mitch092 18:9b9ec0b35b45 119
mitch092 18:9b9ec0b35b45 120 // 180/pi -> converts radians to degrees.
mitch092 18:9b9ec0b35b45 121 float rad_to_deg = 57.2957795131f;
mitch092 18:9b9ec0b35b45 122
mitch092 18:9b9ec0b35b45 123
mitch092 18:9b9ec0b35b45 124 for(;;) {
mitch092 18:9b9ec0b35b45 125 //t.start();
mitch092 18:9b9ec0b35b45 126
mitch092 18:9b9ec0b35b45 127
mitch092 18:9b9ec0b35b45 128 //Vec3 gyro_reading = get_gyro();
mitch092 18:9b9ec0b35b45 129
mitch092 18:9b9ec0b35b45 130 //orientation.x += (gyro_reading.x * dt * sens);
mitch092 18:9b9ec0b35b45 131 //orientation.y += (gyro_reading.y * dt * sens);
mitch092 18:9b9ec0b35b45 132 //orientation.z += (gyro_reading.z * dt * sens);
mitch092 18:9b9ec0b35b45 133
mitch092 18:9b9ec0b35b45 134 //print_vec3("orientation: ", orientation);
mitch092 18:9b9ec0b35b45 135
mitch092 18:9b9ec0b35b45 136
mitch092 18:9b9ec0b35b45 137
mitch092 18:9b9ec0b35b45 138 vec3 = get_accel();
mitch092 18:9b9ec0b35b45 139
mitch092 18:9b9ec0b35b45 140 tilt.x = atan(vec3.x / sqrtf(vec3.y*vec3.y + vec3.z*vec3.z)) * rad_to_deg;
mitch092 18:9b9ec0b35b45 141 tilt.y = atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z)) * rad_to_deg;
mitch092 18:9b9ec0b35b45 142 tilt.z = atan(sqrtf(vec3.y*vec3.y + vec3.x*vec3.x) / vec3.z) * rad_to_deg;
mitch092 18:9b9ec0b35b45 143
mitch092 18:9b9ec0b35b45 144
cparata 0:69566eea0fba 145
mitch092 18:9b9ec0b35b45 146
mitch092 18:9b9ec0b35b45 147 print_vec3("acceleration: ", tilt);
mitch092 18:9b9ec0b35b45 148
mitch092 18:9b9ec0b35b45 149 std::cout << "\r\n" << std::flush;
mitch092 18:9b9ec0b35b45 150
mitch092 18:9b9ec0b35b45 151 t.stop();
mitch092 18:9b9ec0b35b45 152 if (dt - t.read() > 0) wait(dt - t.read());
mitch092 18:9b9ec0b35b45 153 dt = t.read();
mitch092 18:9b9ec0b35b45 154 }
mitch092 18:9b9ec0b35b45 155
mitch092 18:9b9ec0b35b45 156
mitch092 18:9b9ec0b35b45 157
mitch092 18:9b9ec0b35b45 158
mitch092 18:9b9ec0b35b45 159
mitch092 18:9b9ec0b35b45 160
mitch092 18:9b9ec0b35b45 161
mitch092 18:9b9ec0b35b45 162
mitch092 18:9b9ec0b35b45 163
mitch092 18:9b9ec0b35b45 164 //for(;;) {
mitch092 18:9b9ec0b35b45 165 //-0.42 -1.61 1.05
mitch092 18:9b9ec0b35b45 166 //magnetometer->get_m_axes(axes);
mitch092 18:9b9ec0b35b45 167 //std::cout << "LSM303AGR [mag/gauss]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n";
cparata 0:69566eea0fba 168
mitch092 18:9b9ec0b35b45 169 //accelerometer->get_x_axes(axes);
mitch092 18:9b9ec0b35b45 170 //std::cout << "LSM303AGR [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n";
mitch092 18:9b9ec0b35b45 171
cparata 0:69566eea0fba 172
mitch092 18:9b9ec0b35b45 173 //acc_gyro->get_x_axes(axes);
mitch092 18:9b9ec0b35b45 174 //std::cout << "LSM6DSL [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n";
mitch092 18:9b9ec0b35b45 175
mitch092 18:9b9ec0b35b45 176
mitch092 18:9b9ec0b35b45 177 //acc_gyro->get_g_axes(axes);
mitch092 18:9b9ec0b35b45 178 //std::cout << "LSM6DSL [gyro/dps]: " << axes[0]/1000.0f + 0.42f << " " << axes[1]/1000.0f + 1.61f << " " << axes[2]/1000.0f-1.05f << "\r\n";
cparata 0:69566eea0fba 179
mitch092 18:9b9ec0b35b45 180
mitch092 18:9b9ec0b35b45 181
mitch092 18:9b9ec0b35b45 182
cparata 0:69566eea0fba 183
mitch092 18:9b9ec0b35b45 184 //print_vec3("gyro/dps: ", get_gyro());
mitch092 18:9b9ec0b35b45 185 //std::cout << "\r\n" << std::flush;
mitch092 18:9b9ec0b35b45 186
cparata 0:69566eea0fba 187
mitch092 18:9b9ec0b35b45 188 //wait(1.5);
mitch092 18:9b9ec0b35b45 189 //}
cparata 0:69566eea0fba 190 }