MPU6050のサンプルプログラム2

Dependencies:   ConfigFile SDFileSystem mbed

Fork of LAURUS_program by LAURUS

GMS6_CR6/GMS6_CR6.cpp

Committer:
ojan
Date:
2015-05-24
Revision:
1:6cd6d2760856
Child:
4:45dc5590abc0

File content as of revision 1:6cd6d2760856:

#include "mbed.h"
#include "GMS6_CR6.h"

GMS6_CR6::GMS6_CR6(Serial* ps, Serial* pc): p_port(ps), p_pc(pc) {
    p_port->baud(4800);
    pointer = 0;
    INT_flag = 0;
    p_port->attach(this, &GMS6_CR6::INT_Rx, Serial::RxIrq);
}

GMS6_CR6::~GMS6_CR6() {
    p_port = NULL;
}

void GMS6_CR6::read() {
    while(INT_flag);
    int ret = sscanf(buff2, "$GPGGA,%f,%f,%c,%f,%c,%d,%d", 
                &time, &raw_latitude, &lat_hem, &raw_longitude, &lng_hem);
    if(!ret) {
        p_pc->printf("sscanf Error\r\n");
        return;
    }
    
    int deg_lat = (int)raw_latitude / 100;
    float min_lat = (raw_latitude - (float)(deg_lat*100)) / 60.0f;
    latitude = (float)deg_lat + min_lat;
    
    int deg_lng = (int)raw_longitude / 100;
    float min_lng = (raw_longitude - (float)(deg_lng*100)) / 60.0f;
    longitude = (float)deg_lng + min_lng;
    
    
}

void GMS6_CR6::INT_Rx() {
    buff1[pointer] = p_port->getc();
    
    if(buff1[pointer] != '\r') {
        pointer = (pointer+1)%BuffSize;
    } else {
        if(strstr((const char*)buff1, "GPGGA") != NULL) {
            buff1[pointer] = '\0';
            INT_flag = 1;
            strcpy(buff2, (const char*)buff1);
            INT_flag = 0;
            pointer = 0;
        }
    }
}