LAURUS
output only raw data (acceleration, anguler rate, geomagnetism, air pressure)
Dependencies: mbed SDFileSystem ConfigFile
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