Revision 0:029ef267b1bc, committed 2017-02-10

Comitter:

Nike3221

Date:

Fri Feb 10 08:39:51 2017 +0000

Child:

1:2992de38cf3e

Commit message:

tanekon

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MAG3110.cpp	Fri Feb 10 08:39:51 2017 +0000
@@ -0,0 +1,194 @@
+
+#include "MAG3110.h"
+#include "mbed.h"
+#include "MotionSensor.h"
+
+/******************************************************************************
+ * Constructors
+ ******************************************************************************/
+MAG3110::MAG3110(PinName sda, PinName scl): m_i2c(sda, scl), 
+    m_addr(0x1d)
+{
+    char cmd[2];
+
+    cmd[0] = MAG_CTRL_REG2;
+    cmd[1] = 0x80;
+    m_i2c.write(m_addr, cmd, 2);
+
+}
+
+void MAG3110::enable(void) {
+    uint8_t data[2];
+    readRegs( MAG_CTRL_REG1, &data[1], 1);
+    data[1] |= 0x01;
+    data[0] = MAG_CTRL_REG1;
+    writeRegs(data, 2);
+}
+
+
+void MAG3110::disable(void) {
+    uint8_t data[2];
+    readRegs( MAG_CTRL_REG1, &data[1], 1);
+    data[1] &= 0xFE;
+    data[0] = MAG_CTRL_REG1;
+    writeRegs(data, 2);
+}
+
+
+void MAG3110::readRegs(int addr, uint8_t * data, int len)
+{
+    char cmd[1];
+
+    cmd[0] = addr;
+    m_i2c.write( m_addr, cmd, 1, true);
+    m_i2c.read( m_addr, (char *) data, len);
+    return;
+}
+
+
+void MAG3110::writeRegs(uint8_t * data, int len) {
+    m_i2c.write(m_addr, (char *)data, len);
+}
+
+
+uint32_t MAG3110::whoAmI() {
+    uint8_t who_am_i = 0;
+    readRegs(MAG_WHOAMI, &who_am_i, 1);
+    return (uint32_t) who_am_i;
+}
+
+uint32_t MAG3110::dataReady(void) {
+    uint8_t stat = 0;
+    readRegs(MAG_DR_STATUS, &stat, 1);
+    return (uint32_t) stat;
+}
+
+uint32_t MAG3110::sampleRate(uint32_t f) {
+    return(50); // for now sample rate is fixed at 50Hz
+}
+
+
+void MAG3110::getX(float * x) {
+    *x = (float(getMagAxis(MAG_OUT_X_MSB)) * 0.1f);
+}
+
+void MAG3110::getY(float * y) {
+    *y = (float(getMagAxis(MAG_OUT_Y_MSB)) * 0.1f);
+}
+
+void MAG3110::getZ(float * z) {
+    *z = (float(getMagAxis(MAG_OUT_Z_MSB)) * 0.1f);
+}
+
+void MAG3110::getX(int16_t * d) {
+    *d = getMagAxis(MAG_OUT_X_MSB);
+}
+
+void MAG3110::getY(int16_t * d) {
+    *d = getMagAxis(MAG_OUT_Y_MSB);
+}
+
+void MAG3110::getZ(int16_t * d) {
+    *d = getMagAxis(MAG_OUT_Z_MSB);
+}
+
+int16_t MAG3110::getMagAxis(uint8_t addr) {
+    int16_t acc;
+    uint8_t res[2];
+    readRegs(addr, res, 2);
+
+    acc = (res[0] << 8) | res[1];
+
+    return acc;
+}
+
+
+void MAG3110::getAxis(MotionSensorDataUnits &data) {
+    int16_t t[3];
+    uint8_t res[6];
+
+    readRegs(MAG_OUT_X_MSB, res, 6);
+    t[0] = (res[0] << 8) | res[1];
+    t[1] = (res[2] << 8) | res[3];
+    t[2] = (res[4] << 8) | res[5];
+    data.x = ((float) t[0]) * 0.1f;
+    data.y = ((float) t[1]) * 0.1f;
+    data.z = ((float) t[2]) * 0.1f;
+}
+
+
+void MAG3110::getAxis(MotionSensorDataCounts &data) {
+
+    uint8_t res[6];
+    readRegs(MAG_OUT_X_MSB, res, 6);
+
+    data.x = (res[0] << 8) | res[1];
+    data.y = (res[2] << 8) | res[3];
+    data.z = (res[4] << 8) | res[5];
+}
+/*
+
+// read a register per, pass first reg value, reading 2 bytes increments register
+// Reads MSB first then LSB
+int MAG3110::readVal(char regAddr)
+{
+    char cmd[2];
+    int16_t t;
+    cmd[0] = regAddr;
+    if(_i2c.write(m_addr, cmd, 1)) {
+        printf("MAG3110 write error\r\n");
+        _i2c.stop();
+        _i2c.start();
+        }
+
+    cmd[0] = 0x00;
+    cmd[1] = 0x00;
+    _i2c.read(m_addr, cmd, 2);
+    t = (cmd[0] * 256) + (unsigned short) cmd[1];
+    return ((int) t); //concatenate the MSB and LSB
+}
+
+
+float MAG3110::getHeading()
+{
+    int xVal = readVal(MAG_OUT_X_MSB);
+    int yVal = readVal(MAG_OUT_Y_MSB);
+    return (atan2((double)(yVal - _avgY),(double)(xVal - _avgX)))*180/PI;
+}
+
+void MAG3110::getValues(int *xVal, int *yVal, int *zVal)
+{
+    *xVal = readVal(MAG_OUT_X_MSB);
+    *yVal = readVal(MAG_OUT_Y_MSB);
+    *zVal = readVal(MAG_OUT_Z_MSB);
+}
+
+void MAG3110::ReadXYZ(float * mag)
+{
+    int x, y, z;
+    x = readVal(MAG_OUT_X_MSB);
+    y = readVal(MAG_OUT_Y_MSB);
+    z = readVal(MAG_OUT_Z_MSB);
+    mag[0] = (float) x / 10.0;
+    mag[1] = (float) y / 10.0;
+    mag[2] = (float) z / 10.0;
+    
+}
+
+void MAG3110::ReadXYZraw(int16_t * mag_raw)
+{
+    mag_raw[0] = readVal(MAG_OUT_X_MSB);
+    mag_raw[1] = readVal(MAG_OUT_Y_MSB);
+    mag_raw[2] = readVal(MAG_OUT_Z_MSB);
+}
+
+void MAG3110::setCalibration(int minX, int maxX, int minY, int maxY )
+{
+    _avgX=(maxX+minX)/2;
+    _avgY=(maxY+minY)/2;
+}
+*/
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MAG3110.h	Fri Feb 10 08:39:51 2017 +0000
@@ -0,0 +1,114 @@
+/*
+ * MAG3110 Sensor Library for mbed
+ * TODO: Add proper header
+ */
+ 
+#ifndef MAG3110_H
+#define MAG3110_H
+ 
+#include "mbed.h"
+#include "MotionSensor.h"
+ 
+#define PI 3.14159265359
+ 
+#define MAG_ADDR 0x1D
+ 
+// define registers
+#define MAG_DR_STATUS 0x00
+#define MAG_OUT_X_MSB 0x01
+#define MAG_OUT_X_LSB 0x02
+#define MAG_OUT_Y_MSB 0x03
+#define MAG_OUT_Y_LSB 0x04
+#define MAG_OUT_Z_MSB 0x05
+#define MAG_OUT_Z_LSB 0x06
+#define MAG_WHOAMI  0x07
+#define MAG_SYSMOD    0x08
+#define MAG_OFF_X_MSB 0x09
+#define MAG_OFF_X_LSB 0x0A
+#define MAG_OFF_Y_MSB 0x0B
+#define MAG_OFF_Y_LSB 0x0C
+#define MAG_OFF_Z_MSB 0x0D
+#define MAG_OFF_Z_LSB 0x0E
+#define MAG_DIE_TEMP  0x0F
+#define MAG_CTRL_REG1 0x10
+#define MAG_CTRL_REG2 0x11
+ 
+// what should WHO_AM_I return?
+#define MAG_3110_WHO_AM_I_VALUE 0xC4
+ 
+ 
+// Fields in registers
+// CTRL_REG1: dr2,dr1,dr0  os1,os0  fr tm ac
+ 
+// Sampling rate from 80Hz down to 0.625Hz
+#define MAG_3110_SAMPLE80 0
+#define MAG_3110_SAMPLE40 0x20
+#define MAG_3110_SAMPLE20 0x40
+#define MAG_3110_SAMPLE10 0x60
+#define MAG_3110_SAMPLE5 0x80
+#define MAG_3110_SAMPLE2_5 0xA0
+#define MAG_3110_SAMPLE1_25 0xC0
+#define MAG_3110_SAMPLE0_625 0xE0
+ 
+// How many samples to average (lowers data rate)
+#define MAG_3110_OVERSAMPLE1 0
+#define MAG_3110_OVERSAMPLE2 0x08
+#define MAG_3110_OVERSAMPLE3 0x10
+#define MAG_3110_OVERSAMPLE4 0x18
+ 
+// read only 1 byte per axis
+#define MAG_3110_FASTREAD 0x04
+// do one measurement (even if in standby mode)
+#define MAG_3110_TRIGGER 0x02
+// put in active mode
+#define MAG_3110_ACTIVE 0x01
+ 
+// CTRL_REG2: AUTO_MRST_EN  _ RAW MAG_RST _ _ _ _ _
+// reset sensor after each reading
+#define MAG_3110_AUTO_MRST_EN 0x80
+// don't subtract user offsets
+#define MAG_3110_RAW 0x20
+// reset magnetic sensor after too-large field
+#define MAG_3110_MAG_RST 0x10
+ 
+// DR_STATUS Register ZYXOW ZOW YOW XOW ZYXDR ZDR YDR XDR
+#define MAG_3110_ZYXDR  0x08
+ 
+/**
+ * MAG3110 Class to read X/Y/Z data from the magentometer
+ *
+ */
+class MAG3110 : public MotionSensor
+{
+public:
+    /**
+     * Main constructor
+     * @param sda SDA pin
+     * @param sdl SCL pin
+     * @param addr addr of the I2C peripheral
+     */
+    MAG3110(PinName sda, PinName scl);
+ 
+    void enable(void);
+    void disable(void);
+    uint32_t sampleRate(uint32_t fequency);
+    uint32_t whoAmI(void);
+    uint32_t dataReady(void);
+    void getX(int16_t * x);
+    void getY(int16_t * y);
+    void getZ(int16_t * z);
+    void getX(float * x);
+    void getY(float * y);
+    void getZ(float * z);
+    void getAxis(MotionSensorDataCounts &data);
+    void getAxis(MotionSensorDataUnits &data);
+    void readRegs(int addr, uint8_t * data, int len);
+  
+private:
+  I2C m_i2c;
+  char m_addr;
+  int16_t getMagAxis(uint8_t addr);
+  void writeRegs(uint8_t * data, int len);
+ 
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotionSensor.lib	Fri Feb 10 08:39:51 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/components/code/MotionSensor/#4d6e28d4a18a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Fri Feb 10 08:39:51 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/mbed_official/code/SDFileSystem/#8db0d3b02cec

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Feb 10 08:39:51 2017 +0000
@@ -0,0 +1,272 @@
+/*
+ * MAG3110 Sensor Library for mbed
+ * TODO: Add proper header
+ */
+
+
+#include "mbed.h"
+#include "MotionSensor.h"
+#include "MAG3110.h"
+#include "math.h"
+//#include "SDFileSystem.h"
+
+//SDFileSystem sd(p5, p6, p7, p8, "sd");
+//Serial pc(USBTX,USBRX);
+Serial pc(p9,p10);
+
+int16_t x,y,z;
+LocalFileSystem local("local");
+MAG3110 mag(p28,p27);
+Serial gps(p13,p14);
+
+DigitalIn local_on(p20);
+DigitalIn local_del(p19);
+DigitalIn north(p21);
+DigitalIn east(p22);
+DigitalIn south(p23);
+DigitalIn west(p24);
+DigitalIn cds(p29);
+
+
+DigitalOut bit1(LED4);
+DigitalOut bit2(LED3);
+DigitalOut bit3(LED2);
+DigitalOut bit4(LED1);
+
+char gps_c[250];
+int offset_x,offset_y;
+int x_max,y_max,x_min,y_min;
+int count_offset,count_gps;
+int a,b,c,d,e,f,g;
+float x_target,y_target,d_target;
+float longitude_target = 36.11707;
+float latitude_target = 139.4720;
+float utc_time,latitude,longitude,speed,course,utc_date,gps_magnetic;
+char pos_status;
+float direction,x_dat,y_dat;
+char angle_c;
+
+
+void init(){
+    
+    x_max = -32500;//x,yの最大値を初期化
+    y_max = -32500;
+    x_min = 32500;
+    y_min = 32500;
+    
+}
+
+int gps_save(){
+               a=0;
+               count_gps = 0;
+               pc.printf("sequence2\n");
+               
+               while(a <= 159 && count_gps < 3)
+                  {  
+                   bit4=1;
+                   
+                   for(a=0; gps_c[a-1] != '\r' && gps_c[a] != '\n'; a++)
+                       {
+                         gps_c[a] = gps.getc();
+                         pc.printf("%c \n",gps_c[a]);
+                       }
+                       
+                   bit4=0;
+                   
+                   bit2=1;
+                   if(memcmp(gps_c,"$GPRMC",6) == 0)
+                       {  
+                           sscanf(gps_c,"$GPRMC,%f,%c,%f,N,%f,E,%f,%f,%f,%f,W",&utc_time,&pos_status,&latitude,&longitude,&speed,&course,&utc_date,&gps_magnetic);
+                           pc.printf("$GPRMC,%f,%c,%f,N,%f,E,%f,%f,%f,%f,W \n",utc_time,pos_status,latitude,longitude,speed,course,utc_date,gps_magnetic);
+                           
+                           if(latitude!=0)
+                            {bit3=1;}
+                           //mkdir("/sd/gps", 0777);
+                           
+                            if(local_on == 1)
+                                  {
+                                          FILE *fp = fopen("/local/gps.txt", "a");
+                                          if(fp == NULL) {
+                                                          error("Could not open file for write\n");
+                                                         } 
+
+                                          fprintf(fp,"$GPRMC,%f,%c,%f,N,%f,E,%f,%f,%f,%f,W \n",utc_time,pos_status,latitude,longitude,speed,course,utc_date,gps_magnetic);
+                                          fclose(fp); 
+                                  }
+                            bit3=0;      
+                            count_gps++;
+                        }
+                        
+                      /*else if(memcmp(gps_c,"$GPGGA",6) == 0)
+                         { sscanf(gps_c,"$GPGGA,%f,%f,N,%f,E",&utc_time,&latitude,&longitude);
+                           pc.printf("$GPGGA,%f,%f,N,%f,E \n",utc_time,latitude,longitude);
+                           
+                           if(latitude!=0)
+                            {bit3=1;}
+                          // mkdir("/sd/gps", 0777);
+                           if(local_on == 1){
+                                          FILE *fp = fopen("/local/gps.txt", "a");
+                                          if(fp == NULL) {
+                                                          error("Could not open file for write\n");
+                                                         } 
+
+                                          fprintf(fp,"$GPGGA,%f,%f,N,%f,E \n",utc_time,latitude,longitude);
+                                          fclose(fp); }
+                           bit3=0;
+                           count_gps++;
+                        }*/
+                        
+                     else if(memcmp(gps_c,"$GPGLL",6) == 0)
+                         { sscanf(gps_c,"$GPGLL,%f,N,%f,E \n",&latitude,&longitude);
+                           pc.printf("$GPGLL,%f,N,%f,E \n",latitude,longitude);
+                           
+                           if(latitude!=0)
+                            {bit3=1;}
+                           //mkdir("/sd/gps", 0777);
+                           
+                            if(local_on == 1)
+                                {
+                                          FILE *fp = fopen("/local/gps.txt", "a");
+                                          if(fp == NULL) {
+                                                          error("Could not open file for write\n");
+                                                         } 
+
+                                          fprintf(fp,"$GPGLL,%f,N,%f,E \n",latitude,longitude);
+                                          fclose(fp); 
+                                }
+                           bit3=0;
+                           count_gps++;
+                          }
+                    bit2=0;
+               }
+               
+               
+              return 0;
+}
+
+int offset(int x_rd,int y_rd){
+    
+    if(x_max == 0 || y_max == 0 || x_min == 0 || y_min == 0){
+        init();}//最大値、最小値に0が入った時初期化
+    
+    if(x_max<x_rd && x_rd != 0){
+        x_max=x_rd;}
+        
+    if(y_max<y_rd && y_rd != 0){
+        y_max=y_rd;}
+    
+    if(x_min>x_rd && x_rd != 0){
+        x_min=x_rd;}
+        
+    if(y_min>y_rd && y_rd != 0){
+        y_min=y_rd;}
+        
+    offset_x = ((abs(x_min)+abs(x_max))/2);
+    offset_y = ((abs(y_min)+abs(y_max))/2);
+    
+    if(count_offset < 301){
+           count_offset++;}
+    if(count_offset > 300){//300個分のサンプ取得
+           x_dat = x + offset_x;
+           y_dat = y - offset_y;}
+        
+    return 0;    
+}
+
+void cal_gps(){
+           y_target = (latitude_target-latitude)*111319.49;
+           x_target = (longitude_target-longitude)*cos(latitude*(3.1415926535/180))*111319.49;
+           d_target = sqrt(pow(x,2.0)*pow(y,2.0));     
+}
+
+void cal_con(){
+           
+           direction = (atan(y_dat/x_dat))*57.2958;
+           //direction = (atan(y_cal/x_cal))*57.2958;
+           
+           /*if(0 < direction && 89 > direction){
+               angle_c = 'N';}
+               
+           else if(90 < direction && 179 > direction){
+               angle_c = 'E';}
+               
+           else if(180 < direction && 269 > direction || -180 < direction && -91 > direction){
+               angle_c = 'S';}
+               
+           else if(270 < direction && 359 > direction || -90 < direction && -1 > direction){
+               angle_c = 'W';}
+        
+           else{ angle_c = '?';}*/        
+           
+           if(north == 1){
+               angle_c = 'N';}
+               
+           else if(east == 1){
+               angle_c = 'E';}
+               
+           else if(south == 1){
+               angle_c = 'S';}
+               
+           else if(west == 1){
+               angle_c = 'W';}
+        
+           else{ angle_c = '?';}    
+    
+}
+
+void con_save(){
+
+    bit1=1;//led4を1にしてデータを保存
+    FILE *fp = fopen("/local/commpas_new.txt","a");
+               fprintf(fp,"%c,%f,%f,%f,%f\n",angle_c,d_target,x_dat,y_dat,direction);
+               //fprintf(fp,"%d,%d,%d,%d,%d\n",x,y,z,offset_x,offset_y);
+               fclose(fp);
+    bit1=0;    
+}
+
+void dele(){
+    
+    bit2=1;//led3を1
+               
+    remove("/local/commpas_new.txt");
+    remove("/local/gps.txt");
+    
+    bit2=0;
+}
+
+int main(){
+    
+    mag.enable();//コンパス有効化
+    mag.sampleRate(0x40);
+    init(); //初期化
+    
+    while(1){
+           
+           if(cds == 0){
+           while(1){
+           //void disable(void);
+           //uint32_t whoAmI(void);
+           //uint32_t dataReady(void);
+           mag.getX(&x);
+           mag.getY(&y);
+           mag.getZ(&z);
+           //void getX(float * x);
+           //void getY(float * y);
+           //void getZ(float * z);
+           //void getAxis(MotionSensorDataCounts &data);
+           //void getAxis(MotionSensorDataUnits &data);
+           //void readRegs(int addr, uint8_t * data, int len);
+           offset(x,y);//x,y オフセット計算
+           //cal(x_dat,y_dat);
+           cal_con();
+           cal_gps();
+           if(local_on == 1){con_save();}//pin20がhighの時、データ保存
+           if(local_del == 1){dele();}//pin19がhighの時、保存したデータを消去
+           gps_save();
+           pc.printf("<%c> x: %d , y: %d , x_dat: %f , y_dat: %f , direction: %f \n",angle_c,x,y,x_dat,y_dat,direction);    
+           //pc.printf("x_dat: %d  y_dat: %d  x: %d  offset: %d x_max: %d  x_min: %d\n",x_dat,y_dat,x,offset,x_max,x_min);
+            }
+           }
+    
+     }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Feb 10 08:39:51 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/abea610beb85
\ No newline at end of file