MPU6050のサンプルプログラム2
Dependencies: ConfigFile SDFileSystem mbed
Fork of LAURUS_program by
Revision 0:bc6f14fc60c7, committed 2015-05-15
- Comitter:
- ojan
- Date:
- Fri May 15 17:24:32 2015 +0000
- Child:
- 1:6cd6d2760856
- Commit message:
- Laurus integrated program (ver.0.0)
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ErrorLogger.cpp Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,11 @@ +#include "ErrorLogger.h" + +void InitLogger(Serial* dp) { + if(debugPort == dp) return; + debugPort = dp; +} + +void AbortWithMsg(const char* msg) { + debugPort->printf(msg); + abort(); +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ErrorLogger.h Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,13 @@ +#pragma once +#include "mbed.h" + +static Serial* debugPort = 0; + +void InitLogger(Serial* dp); + +/* + エラーメッセージを送信してプログラムを中断する + 引数: +*/ + +void AbortWithMsg(const char* msg); \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HMC5883L/HMC5883L.cpp Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,38 @@ +#include "mbed.h" +#include "HMC5883L.h" + +HMC5883L::HMC5883L(I2C* i2c) { + this->i2c = i2c; +} + +HMC5883L::~HMC5883L() { + i2c = NULL; +} + +int HMC5883L::init() { + return 1; +} + +int HMC5883L::read() { + char cmd[2] = {0x02, 0x01}; + int ret = i2c->write(hmc_addr, cmd, 2); + if(ret != 0) return 0; + + cmd[0] = 0x03; + i2c->write(hmc_addr, cmd, 1); + i2c->read(hmc_addr | 0x01, data.reg, 6, true); + + // データのHとLが逆に読み込まれているのでスワップする + for(int i=0; i<3; i++) { + char temp = 0; + temp = data.reg[i*2]; + data.reg[i*2] = data.reg[i*2+1]; + data.reg[i*2+1] = temp; + } + + int16_t temp = data.value[1]; + data.value[1] = data.value[2]; + data.value[2] = temp; + + return 1; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HMC5883L/HMC5883L.h Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,32 @@ +#pragma once + +const static int hmc_addr = 0x3c; + +typedef union { + char reg[6]; + struct { + uint8_t mag_x_L; + uint8_t mag_x_H; + uint8_t mag_y_L; + uint8_t mag_y_H; + uint8_t mag_z_L; + uint8_t mag_z_H; + } byte; + + int16_t value[3]; + +} HMC_DATA; + +class HMC5883L { +public: + HMC5883L(I2C* i2c); + ~HMC5883L(); + + int init(); + int read(); + + HMC_DATA data; + +private: + I2C* i2c; +}; \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LPS25H/LPS25H.cpp Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,89 @@ +//********************** +// LPS25H.cpp for mbed +// +// LPS25H lps25h(P0_5,P0_4); +// or +// I2C i2c(P0_5,P0_4); +// LPS25H lps25h(i2c); +// +// (C)Copyright 2014 All rights reserved by Y.Onodera +// http://einstlab.web.fc2.com +//********************** + +#include "mbed.h" +#include "LPS25H.h" + +/*LPS25H::LPS25H (PinName sda, PinName scl) : _i2c(sda, scl) { + init(); +}*/ +LPS25H::LPS25H (I2C* p_i2c) : _i2c(p_i2c) { + init(); +} + +void LPS25H::put(unsigned char a, unsigned char b) +{ + buf[0]=a; + buf[1]=b; + _i2c->write(LPS25H_ADDR, buf, 2); +} + + +void LPS25H::get(unsigned char a) +{ + buf[0] = a; + _i2c->write(LPS25H_ADDR, buf, 1, true); // no stop, repeated + _i2c->read( LPS25H_ADDR, buf, 1); + +} + +long LPS25H::pressure() +{ + + // XL first and H last + // get press_xl + get(LPS25H_PRESS_OUT_XL); + press.byte.LB=buf[0]; + // get tpress_low + get(LPS25H_PRESS_OUT_L); + press.byte.HB=buf[0]; + // get press_high + get(LPS25H_PRESS_OUT_H); + press.byte.UB=buf[0]; + return press.Val; + + // hPa = press.Val / 4096 + // Pa = press.Val / 40.96 +} + + +short LPS25H::temperature() +{ + + // L first and H last + // get tpress_low + get(LPS25H_TEMP_OUT_L); + temp.byte.LB=buf[0]; + // get press_high + get(LPS25H_TEMP_OUT_H); + temp.byte.HB=buf[0]; + return temp.S; + + // C = 42.5 + temp.S / 480 + // range:0 to 80C + // accuracy:+-2C +} + + +void LPS25H::init() +{ + // Resolution config + put(LPS25H_RES_CONF, 0x0A); + // FIFO 4samples moving average + put(LPS25H_FIFO_CTRL, 0xCF); + // FIFO Enable + put(LPS25H_CTRL_REG2, 0x40); + // Power ON Cycle=12.5Hz + put(LPS25H_CTRL_REG1, 0xB0); +} + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LPS25H/LPS25H.h Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,62 @@ +//********************** +// LPS25H.h for mbed +// +// (C)Copyright 2014 All rights reserved by Y.Onodera +// http://einstlab.web.fc2.com +//********************** + +#ifndef LPS25H_H_ +#define LPS25H_H_ + +#define LPS25H_ADDR 0xB8 +#define LPS25H_REF_P_XL 0x08 +#define LPS25H_REF_P_L 0x09 +#define LPS25H_REF_P_H 0x0A +#define LPS25H_WHO_AM_I 0x0F +#define LPS25H_RES_CONF 0x10 +#define LPS25H_CTRL_REG1 0x20 +#define LPS25H_CTRL_REG2 0x21 +#define LPS25H_CTRL_REG3 0x22 +#define LPS25H_CTRL_REG4 0x23 +#define LPS25H_INT_CFG 0x24 +#define LPS25H_INT_SOURCE 0x25 +#define LPS25H_STATUS_REG 0x27 +#define LPS25H_PRESS_OUT_XL 0x28 +#define LPS25H_PRESS_OUT_L 0x29 +#define LPS25H_PRESS_OUT_H 0x2A +#define LPS25H_TEMP_OUT_L 0x2B +#define LPS25H_TEMP_OUT_H 0x2C +#define LPS25H_FIFO_CTRL 0x2E +#define LPS25H_FIFO_STATUS 0x2F +#define LPS25H_THS_P_L 0x30 +#define LPS25H_THS_P_H 0x31 +#define LPS25H_RPDS_L 0x39 +#define LPS25H_RPDS_H 0x3A + +#include "mbed.h" +#include "typedef.h" + +class LPS25H{ +public: + LPS25H (PinName sda, PinName scl); + LPS25H (I2C* p_i2c); + void init(); + + void put(unsigned char a, unsigned char b); + void get(unsigned char a); + long pressure(); + short temperature(); + +protected: + + I2C* _i2c; + + DWORD_VAL press; + WORD_VAL temp; + char buf[2]; + +}; + +#endif /* LPS25H_H_ */ + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LPS25H/typedef.h Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,55 @@ +//********************** +// typedef for mbed +// +// Condition: +// +// (C)Copyright 2014 All rights reserved by Y.Onodera +// http://einstlab.web.fc2.com +//********************** +#ifndef TYPEDEF_H +#define TYPEDEF_H + +typedef unsigned char BYTE; /* 8-bit unsigned */ +typedef unsigned short int WORD; /* 16-bit unsigned */ +typedef unsigned int DWORD; /* 32-bit unsigned */ +typedef unsigned long long QWORD; /* 64-bit unsigned */ + + +typedef union +{ + WORD Val; + BYTE v[2]; + short S; + struct + { + BYTE LB; + BYTE HB; + } byte; +} WORD_VAL; + +typedef union +{ + DWORD Val; + WORD w[2]; + BYTE v[4]; + struct + { + WORD LW; + WORD HW; + } word; + struct + { + BYTE LB; + BYTE HB; + BYTE UB; + BYTE MB; + } byte; + struct + { + WORD_VAL low; + WORD_VAL high; + }wordUnion; + +} DWORD_VAL; + +#endif /* TYPEDEF_H */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050/MPU6050.cpp Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,37 @@ +#include "mbed.h" +#include "MPU6050.h" + +MPU6050::MPU6050(I2C* i2c) { + this->i2c = i2c; +} + +MPU6050::~MPU6050() { + i2c = NULL; +} + +int MPU6050::init() { + char cmd[2] = {0x6b, 0x00}; + int ret = i2c->write(mpu_addr, cmd, 2); + + if(ret != 0) return 0; + return 1; +} + +int MPU6050::read() { + char cmd[1] = {0x3b}; + int ret = i2c->write(mpu_addr, cmd, 1, true); + if(ret != 0) return 0; + + i2c->read(mpu_addr | 0x01, (char*)data.reg, 14, false); + + // データのHとLが逆に読み込まれているのでスワップする + for(int i=0; i<7; i++) { + uint8_t temp = 0; + temp = data.reg[i*2]; + data.reg[i*2] = data.reg[i*2+1]; + data.reg[i*2+1] = temp; + } + + return 1; + +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050/MPU6050.h Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,47 @@ +#pragma once + +const static int mpu_addr = 0xd0; + +typedef union { + uint8_t reg[14]; + struct { + uint8_t acc_x_L; + uint8_t acc_x_H; + uint8_t acc_y_L; + uint8_t acc_y_H; + uint8_t acc_z_L; + uint8_t acc_z_H; + + uint8_t T_L; + uint8_t T_H; + + uint8_t gyro_x_L; + uint8_t gyro_x_H; + uint8_t gyro_y_L; + uint8_t gyro_y_H; + uint8_t gyro_z_L; + uint8_t gyro_z_H; + } byte; + + struct { + int16_t acc[3]; + + int16_t T; + + int16_t gyro[3]; + } value; +} MPU_DATA; + +class MPU6050 { +public: + MPU6050(I2C* i2c); + ~MPU6050(); + + int init(); + int read(); + + MPU_DATA data; + +private: + I2C* i2c; +}; \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Vector/Vector.cpp Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,157 @@ +#include "mbed.h" +#include "myConstants.h" +#include "Vector.h" + + +Vector::Vector(int dim) : dim(dim), components(0){ + components = new float[dim]; + if (!components) AbortWithMsg("Memory Allocation Error"); + for(int i=0; i<dim; i++) { + components[i] = 0.0f; + } +} + + +Vector::~Vector() { + delete[] components; +} + +Vector::Vector(const Vector& v) : dim(v.dim), components(0) { + components = new float[dim]; + if (!components) AbortWithMsg("Memory Allocation Error"); + memcpy(components, v.GetpComponents(), sizeof(float)*dim); +} + +Vector& Vector::operator=(const Vector& v) { + if (this == &v) return *this; + dim = v.dim; + delete[] components; + components = new float[dim]; + if (!components) AbortWithMsg("Memory Allocation Error"); + memcpy(components, v.GetpComponents(), sizeof(float)*dim); + + return *this; +} + +Vector& Vector::operator*=(float c) { + for (int i = 0; i < dim; i++) { + components[i] *= c; + } + + return *this; +} + +Vector& Vector::operator/=(float c) { + if (fabs(c) < NEARLY_ZERO) AbortWithMsg("Division by Zero"); + for (int i = 0; i < dim; i++) { + components[i] /= c; + } + + return *this; +} + +Vector& Vector::operator+=(const Vector& v) { + if (dim != v.dim) AbortWithMsg("failed to add: Irregular Dimention"); + for (int i = 0; i < dim; i++) { + components[i] += v.components[i]; + } + + this->CleanUp(); + + return *this; +} + +Vector& Vector::operator-=(const Vector& v) { + if (dim != v.dim) AbortWithMsg("failed to subtract: Irregular Dimention"); + for (int i = 0; i < dim; i++) { + components[i] -= v.components[i]; + } + + this->CleanUp(); + + return *this; +} + +void Vector::SetComp(int dimNo, float val) { + if (dimNo > dim) AbortWithMsg("Index Out of Bounds Error"); + components[dimNo-1] = val; +} + +float Vector::GetNorm() const { + float norm = 0.0f; + for (int i = 0; i < dim; i++) { + norm += components[i] * components[i]; + } + return sqrt(norm); +} + +Vector Vector::Normalize() const { + float norm = GetNorm(); + Vector temp(*this); + for (int i = 0; i < dim; i++) { + temp.components[i] /= norm; + } + temp.CleanUp(); + return temp; +} + +void Vector::CleanUp() { + float maxComp = 0.0f; + for (int i = 0; i < dim; i++) { + if (fabs(components[i]) > maxComp) maxComp = fabs(components[i]); + } + if (maxComp > NEARLY_ZERO) { + for (int i = 0; i < dim; i++) { + if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f; + } + } +} + +Vector operator+(const Vector& lhv, const Vector& rhv) { + Vector retVec(lhv); + retVec += rhv; + return retVec; +} + +Vector operator-(const Vector& lhv, const Vector& rhv) { + Vector retVec(lhv); + retVec -= rhv; + return retVec; +} + +Vector Cross(const Vector& lhv, const Vector& rhv) { + if (lhv.GetDim() != 3) AbortWithMsg("failed to cross: variable 'dim' must be 3"); + if (lhv.GetDim() != rhv.GetDim()) AbortWithMsg("failed to cross: Irregular Dimention"); + + Vector retVec(lhv.GetDim()); + + for (int i = 0; i < lhv.GetDim(); i++) { + retVec.SetComp(i + 1, lhv.GetComp((i + 1) % 3 + 1) * rhv.GetComp((i + 2) % 3 + 1) + - lhv.GetComp((i + 2) % 3 + 1) * rhv.GetComp((i + 1) % 3 + 1)); + } + + return retVec; +} + +Vector operator*(const float c, const Vector& rhv) { + Vector retVec(rhv); + retVec *= c; + return retVec; +} + +Vector operator*(const Vector& lhv, const float c) { + Vector retVec(lhv); + retVec *= c; + return retVec; +} + +float operator*(const Vector& lhv, const Vector& rhv) { + if (lhv.GetDim() != rhv.GetDim()) AbortWithMsg("Irregular Dimention"); + float retVal = 0.0f; + + for (int i = 1; i <= lhv.GetDim(); i++) { + retVal += lhv.GetComp(i) * rhv.GetComp(i); + } + + return retVal; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Vector/Vector.h Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,51 @@ +#pragma once +#include "ErrorLogger.h" + +class Matrix; + +class Vector { +public: + Vector(int dim); + ~Vector(); + Vector(const Vector& v); + + Vector& operator=(const Vector& v); + Vector& operator*=(float c); + Vector& operator/=(float c); + Vector& operator+=(const Vector& v); + Vector& operator-=(const Vector& v); + + void SetComp(int dimNo, float val); + float GetNorm() const; + Vector Normalize() const; + + inline int GetDim() const { + return dim; + } + + inline const float* GetpComponents() const { + return (const float*)components; + } + + inline float GetComp(int dimNo) const { + if (dimNo > dim) AbortWithMsg("Index Out of Bounds Error !!"); + return components[dimNo-1]; + } + + void CleanUp(); + +private: + int dim; + float* components; + + Vector& operator*=(const Matrix& m); + Vector& operator*=(const Vector& m); +}; + +Vector operator+(const Vector& lhv, const Vector& rhv); +Vector operator-(const Vector& lhv, const Vector& rhv); +Vector Cross(const Vector& lhv, const Vector& rhv); +Vector operator*(const float c, const Vector& rhv); +Vector operator*(const Vector& lhv, const float c); +float operator*(const Vector& lhv, const Vector& rhv); +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,82 @@ +#include "mbed.h" +#include "MPU6050.h" +#include "HMC5883L.h" +#include "LPS25H.h" +#include "ErrorLogger.h" +#include "Vector.h" +#include "myConstants.h" + +/********** private define **********/ +#define TRUE 1 +#define FALSE 0 +/********** private macro **********/ +/********** private typedef **********/ +/********** private variables **********/ +I2C i2c(I2C_SDA, I2C_SCL); // I2Cポート +MPU6050 mpu(&i2c); // 加速度・角速度センサ +HMC5883L hmc(&i2c); // 地磁気センサ +LPS25H lps(&i2c); // 気圧センサ +Serial pc(SERIAL_TX, SERIAL_RX); // PC通信用シリアルポート +Ticker timer; // 割り込みタイマー + +const float freq = 0.01f; // 割り込み周期(s) + +int lps_cnt = 0; // 気圧センサ読み取りカウント +uint8_t INT_flag = TRUE; // 割り込み可否フラグ +float acc[3]; // 加速度(m/s^2) +float gyro[3]; // 角速度(deg/s) +float geomag[3]; // 地磁気(?) +float press; // 気圧(hPa) +/********** private functions **********/ +void INT_func(); // 割り込み用関数 +/********** main function **********/ + +int main() { + + i2c.frequency(400000); // I2Cの通信速度を400kHzに設定 + + if(!mpu.init()) AbortWithMsg("mpu6050 Initialize Error !!"); // mpu6050初期化 + if(!hmc.init()) AbortWithMsg("hmc5883l Initialize Error !!"); // hmc5883l初期化 + + timer.attach(&INT_func, freq); // 割り込み有効化 + + while(1) { + + INT_flag = FALSE; // 割り込みによる変数書き換えを阻止 + + // センサ類の全出力値をPCに送信 + printf("%f\t", acc[0]); + printf("%f\t", acc[1]); + printf("%f\t", acc[2]); + printf("%f\t", gyro[0]); + printf("%f\t", gyro[1]); + printf("%f\t", gyro[2]); + printf("%f\t", geomag[0]); + printf("%f\t", geomag[1]); + printf("%f\t", geomag[2]); + printf("%f\r\n", press); + + INT_flag = TRUE; // 割り込み許可 + + } +} + +void INT_func() { + if(INT_flag == FALSE) { + + mpu.read(); + hmc.read(); + + for(int i=0; i<3; i++) { + acc[i] = (float)mpu.data.value.acc[i] * ACC_LSB_TO_G * G_TO_MPSS; + gyro[i] = mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG; + geomag[i] = hmc.data.value[i]; + } + + // LPS25Hによる気圧の取得は10Hz + lps_cnt = (lps_cnt+1)%10; + if(lps_cnt == 0) { + press = (float)lps.pressure() * PRES_LSB_TO_HPA; + } + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/8ab26030e058 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/myConstants.h Fri May 15 17:24:32 2015 +0000 @@ -0,0 +1,11 @@ +#pragma once + +#define NEARLY_ZERO 0.000001f +#define ZERO_TOLERANCE 0.001f +#define RAD_TO_DEG 57.2957795f // 180 / π +#define DEG_TO_RAD 0.0174532925f // π / 180 +#define ACC_LSB_TO_G 0.0000610351562f // g/LSB (1/2^14 +#define G_TO_MPSS 9.8f // (m/s^2)/g +//#define GYRO_LSB_TO_DEG 0.0152671755f // deg/LSB (1/65.5 +#define GYRO_LSB_TO_DEG 0.00763358778f // deg/LSB (1/131 +#define PRES_LSB_TO_HPA 0.000244140625f // hPa/LSB (1/4096 \ No newline at end of file