unfinished

Dependents:   WRS_mechanamu_test WRS2019_master mbed_2018 mbed_2019_rx3 ... more

JY901.cpp

Committer:
sgrsn
Date:
2018-02-04
Revision:
1:6bcbd18a719a
Parent:
0:6ff4cad60b67
Child:
2:5af50b300137

File content as of revision 1:6bcbd18a719a:

#include "JY901.h"

JY901::JY901(PinName sda, PinName scl) : I2C(sda, scl)
{
}

void JY901::calibrateGyroAccel()
{
    start();
    write(IICADDR);
    write(CALSW);
    write(0x01);
    write(0x00);
    stop();
}

void JY901::calibrateMagnetic()
{
    start();
    write(IICADDR);
    write(CALSW);
    write(0x02);
    write(0x00);
    stop();
}

void JY901::calibrateHeight()
{
    start();
    write(IICADDR);
    write(CALSW);
    write(0x03);
    write(0x00);
    stop();
}

void JY901::endCalibrate()
{
    start();
    write(IICADDR);
    write(CALSW);
    write(0x00);
    write(0x00);
    stop();   
}

void JY901::calibrateAll(int time_ms)
{
    calibrateGyroAccel();
    calibrateMagnetic();
    calibrateHeight();
    wait_ms(time_ms);
    endCalibrate();
}

int JY901::getYear()
{
    char *data = getdata(YYMM);
    return (int)(*data);
}

int JY901::getMonth()
{
    char *data = getdata(YYMM);
    return (int)(*(data+1));
}

int JY901::getDay()
{
    char *data = getdata(DDHH);
    return (int)(*data);
}

int JY901::getHour()
{
    char *data = getdata(DDHH);
    return (int)(*(data+1));
}

int JY901::getMinute()
{
    char *data = getdata(MMSS);
    return (int)(*data);
}

int JY901::getSecond()
{
    char *data = getdata(MMSS);
    return (int)(*(data+1));
}

int JY901::getMillisecond()
{
    char *data = getdata(MS);
    return (int)((*(data+1) << 8) | *data);
}

float JY901::getXaxisAcceleration()
{
    char *data = getdata(AX);
    return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
}

float JY901::getYaxisAcceleration()
{
    char *data = getdata(AY);
    return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
}

float JY901::getZaxisAcceleration()
{
    char *data = getdata(AZ);
    return (float)((*(data+1) << 8) | *data) / 32768 * 16 * g;
}

float JY901::getXaxisAngularVelocity()
{
    char *data = getdata(GX);
    return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
}

float JY901::getYaxisAngularVelocity()
{
    char *data = getdata(GY);
    return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
}

float JY901::getZaxisAngularVelocity()
{
    char *data = getdata(GZ);
    return (float)((*(data+1) << 8 ) | *data) / 32768 * 2000;
}

float JY901::getXaxisMagnetic()
{
    char *data = getdata(HX);
    return (float)((*(data+1) << 8) | *data);
}

float JY901::getYaxisMagnetic()
{
    char *data = getdata(HY);
    return (float)((*(data+1) << 8) | *data);
}

float JY901::getZaxisMagnetic()
{
    char *data = getdata(HZ);
    return (float)((*(data+1) << 8) | *data);
}

float JY901::getXaxisAngle()
{
    char *data = getdata(Roll);
    return (float)((*(data+1) << 8) | *data) / 32768 * 180;
}

float JY901::getYaxisAngle()
{
    char *data = getdata(Pitch);
    return (float)((*(data+1) << 8) | *data) / 32768 * 180;
}

float JY901::getZaxisAngle()
{
    char *data = getdata(Yaw);
    return (float)((*(data+1) << 8) | *data) / 32768 * 180;
}

float JY901::getTemperature()
{
    char *data = getdata(TEMP);
    return (float)((*(data+1) << 8) | *data) / 100;
}

float JY901::getD0Status()
{
    char *data = getdata(D0Status);
    return (float)((*(data+1) << 8) | *data) / 4095;
}

float JY901::getD1Status()
{
    char *data = getdata(D1Status);
    return (float)((*(data+1) << 8) | *data) / 4095;
}

float JY901::getD2Status()
{
    char *data = getdata(D2Status);
    return (float)((*(data+1) << 8) | *data) / 4095;
}

float JY901::getD3Status()
{
    char *data = getdata(D3Status);
    return (float)((*(data+1) << 8) | *data) / 4095;
}

int JY901::getmode(float (JY901::*getFunc)(), const int kaisu)
{
    /*exampl
        printf("%d\r\n", obj.getmode(&JY901::getYawAngle, 10));
    */
    int yawangle[128] = {};
    int samecount[128] = {};
    int tmpcount[128] = {};
    int num = 0;
    for(int i = 0; i < kaisu; i++)
    {
        yawangle[i] = (int)((this->*getFunc)() + 0.5);
    }
    for(int i = 0; i < kaisu; i++)
    {
        for(int j = 0; j < kaisu; j++)
        {
            if(yawangle[i] == yawangle[j])
            {
                samecount[i]++;
            }
        }
        tmpcount[i] = samecount[i];
    }
    for(int i = 0; i < kaisu; i++)
    {
        for(int j = 0; j < i; j++)
        {
            if(tmpcount[i] > tmpcount[j])
            {
                int tmp = tmpcount[j];
                tmpcount[j] = tmpcount[i];
                tmpcount[i] = tmp;
            }
        }
    }
    int mostcount = tmpcount[0];
    for(int i = 0; i < kaisu; i++)
    {
        if(samecount[i] == mostcount)
        {
            num = i;
        }
    }
    return yawangle[num];
}

float JY901::getPressure() //not use
{
    char *dataL = getdata(PressureL);
    char *dataH = getdata(PressureH);
    return (float)((*(dataH+1) << 24) | (*dataH << 16) | (*(dataL+1) << 8) | *dataL);
}

float JY901::getHeight()   //not use
{
    char *dataL = getdata(HeightL);
    char *dataH = getdata(HeightH);
    return (float)( ( *(dataH+1) << 24) | (*dataH << 16) | (*(dataL+1) << 8) | *dataL );
}

float JY901::getLongitude()    //not use
{
    char *dataL = getdata(LonL);
    char *dataH = getdata(LonH);
    return (float)( ( *(dataH+1) << 24) | (*dataH << 16) | (*(dataL+1) << 8) | *dataL );
}

float JY901::getLatitude() //not use
{
    char *dataL = getdata(LatL);
    char *dataH = getdata(LatH);
    return (float)( ( *(dataH+1) << 24) | (*dataH << 16) | (*(dataL+1) << 8) | *dataL );
}

float JY901::getGPSHeight()
{
    char *data = getdata(GPSHeight);
    return (float)((*(data+1) << 8) | *data);
}
float JY901::getGPSYaw()
{
    char *data = getdata(GPSYaw);
    return (float)((*(data+1) << 8) | *data);
}
float JY901::getGPSspeed()
{   
    char *dataL = getdata(GPSVL);
    char *dataH = getdata(GPSVH);
    return (float)( ( *(dataH+1) << 24) | (*dataH << 16) | (*(dataL+1) << 8) | *dataL );
}

char *JY901::getdata(char registar)
{
    char data[2] = {};
    start();
    write(IICADDR);
    write(registar);
    read(IICADDR, data, 2);
    stop();
    return data;
}