Steven Mitchell
/
HelloWorld_IKS01A2
afdfdf
Diff: main.cpp
- Revision:
- 18:9b9ec0b35b45
- Parent:
- 13:fc873da5b445
- Child:
- 19:29d3c7caea66
--- a/main.cpp Wed Sep 27 15:48:21 2017 +0000 +++ b/main.cpp Tue Nov 19 00:42:38 2019 +0000 @@ -4,7 +4,7 @@ * @author CLab * @version V1.0.0 * @date 2-December-2016 - * @brief Simple Example application for using the X_NUCLEO_IKS01A1 + * @brief Simple Example application for using the X_NUCLEO_IKS01A1 * MEMS Inertial & Environmental Sensor Nucleo expansion board. ****************************************************************************** * @attention @@ -34,111 +34,157 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************** -*/ +*/ /* Includes */ #include "mbed.h" #include "XNucleoIKS01A2.h" +#include <iostream> +#include <cmath> /* Instantiate the expansion board */ static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5); /* Retrieve the composing elements of the expansion board */ static LSM303AGRMagSensor *magnetometer = mems_expansion_board->magnetometer; -static HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor; -static LPS22HBSensor *press_temp = mems_expansion_board->pt_sensor; static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro; static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer; -/* Helper function for printing floats & doubles */ -static char *print_double(char* str, double v, int decimalDigits=2) -{ - int i = 1; - int intPart, fractPart; - int len; - char *ptr; + + + + +struct Vec3 { + float x, y, z; +}; + + +static const Vec3 gyro_calibrate = {-0.42f, -1.61f, 1.05f}; - /* prepare decimal digits multiplicator */ - for (;decimalDigits!=0; i*=10, decimalDigits--); +Vec3 get_gyro() +{ + Vec3 vec3; - /* calculate integer & fractinal parts */ - intPart = (int)v; - fractPart = (int)((v-(double)(int)v)*i); + int32_t axes[3]; + acc_gyro->get_g_axes(axes); - /* fill in integer part */ - sprintf(str, "%i.", intPart); + vec3.x = axes[0]/1000.0f - gyro_calibrate.x; + vec3.y = axes[1]/1000.0f - gyro_calibrate.y; + vec3.z = axes[2]/1000.0f - gyro_calibrate.z; - /* prepare fill in of fractional part */ - len = strlen(str); - ptr = &str[len]; + return vec3; +} + +Vec3 get_accel() +{ + Vec3 vec3; + int32_t axes[3]; + acc_gyro->get_x_axes(axes); - /* fill in leading fractional zeros */ - for (i/=10;i>1; i/=10, ptr++) { - if (fractPart >= i) { - break; - } - *ptr = '0'; - } + vec3.x = axes[0]/1000.0f + 0.013f; + vec3.y = axes[1]/1000.0f + 0.004f; + vec3.z = axes[2]/1000.0f - 0.032f; + + return vec3; +} - /* fill in (rest of) fractional part */ - sprintf(ptr, "%i", fractPart); + - return str; +void print_vec3(const char* str, const Vec3& vec3) +{ + std::cout << str << vec3.x << " " << vec3.y << " " << vec3.z << "\r\n"; } + /* Simple main function */ -int main() { - uint8_t id; - float value1, value2; - char buffer1[32], buffer2[32]; - int32_t axes[3]; - - /* Enable all sensors */ - hum_temp->enable(); - press_temp->enable(); - magnetometer->enable(); - accelerometer->enable(); - acc_gyro->enable_x(); - acc_gyro->enable_g(); - - printf("\r\n--- Starting new run ---\r\n"); +int main() +{ + + /* Enable all sensors */ + magnetometer->enable(); + accelerometer->enable(); + acc_gyro->enable_x(); + acc_gyro->enable_g(); + + //Timer t; + //float dt = 0.02f; + + std::cout << "\r\n--- Starting new run ---\r\n"; + + //Vec3 orientation = {}; + + //float sens = 0.488f; + + Vec3 vec3, tilt; + + // 180/pi -> converts radians to degrees. + float rad_to_deg = 57.2957795131f; + + + for(;;) { + //t.start(); + + + //Vec3 gyro_reading = get_gyro(); + + //orientation.x += (gyro_reading.x * dt * sens); + //orientation.y += (gyro_reading.y * dt * sens); + //orientation.z += (gyro_reading.z * dt * sens); + + //print_vec3("orientation: ", orientation); + + + + vec3 = get_accel(); + + tilt.x = atan(vec3.x / sqrtf(vec3.y*vec3.y + vec3.z*vec3.z)) * rad_to_deg; + tilt.y = atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z)) * rad_to_deg; + tilt.z = atan(sqrtf(vec3.y*vec3.y + vec3.x*vec3.x) / vec3.z) * rad_to_deg; + + - hum_temp->read_id(&id); - printf("HTS221 humidity & temperature = 0x%X\r\n", id); - press_temp->read_id(&id); - printf("LPS22HB pressure & temperature = 0x%X\r\n", id); - magnetometer->read_id(&id); - printf("LSM303AGR magnetometer = 0x%X\r\n", id); - accelerometer->read_id(&id); - printf("LSM303AGR accelerometer = 0x%X\r\n", id); - acc_gyro->read_id(&id); - printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id); - - while(1) { - printf("\r\n"); + + print_vec3("acceleration: ", tilt); + + std::cout << "\r\n" << std::flush; + + t.stop(); + if (dt - t.read() > 0) wait(dt - t.read()); + dt = t.read(); + } + + + + + + + + + + //for(;;) { + //-0.42 -1.61 1.05 + //magnetometer->get_m_axes(axes); + //std::cout << "LSM303AGR [mag/gauss]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n"; - hum_temp->get_temperature(&value1); - hum_temp->get_humidity(&value2); - printf("HTS221: [temp] %7s C, [hum] %s%%\r\n", print_double(buffer1, value1), print_double(buffer2, value2)); - - press_temp->get_temperature(&value1); - press_temp->get_pressure(&value2); - printf("LPS22HB: [temp] %7s C, [press] %s mbar\r\n", print_double(buffer1, value1), print_double(buffer2, value2)); + //accelerometer->get_x_axes(axes); + //std::cout << "LSM303AGR [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n"; + - printf("---\r\n"); + //acc_gyro->get_x_axes(axes); + //std::cout << "LSM6DSL [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n"; + + + //acc_gyro->get_g_axes(axes); + //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"; - magnetometer->get_m_axes(axes); - printf("LSM303AGR [mag/mgauss]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]); - - accelerometer->get_x_axes(axes); - printf("LSM303AGR [acc/mg]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]); + + + - acc_gyro->get_x_axes(axes); - printf("LSM6DSL [acc/mg]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]); + //print_vec3("gyro/dps: ", get_gyro()); + //std::cout << "\r\n" << std::flush; + - acc_gyro->get_g_axes(axes); - printf("LSM6DSL [gyro/mdps]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]); - - wait(1.5); - } + //wait(1.5); + //} }