File content as of revision 29:2da9b8d03c0b:

//計器プログラム

#include "mbed.h"
#include "rtos.h"
#include "Fusokukei.h"
//#include "xMPU6050.h"
#include "MPU6050_DMP6.h"
#include "SDFileSystem.h"
#include "BufferedSoftSerial.h"
#include "Cadence.h"

#define KX_VALUE_MIN 0.4
#define KX_VALUE_MAX 0.8
#define SOUDA_DATAS_NUM 18
#define YOKUTAN_DATAS_NUM 14
#define WRITE_DATAS_NUM 18
#define AIR_LOOP_TIME 0.01
#define WRITE_DATAS_LOOP_TIME 1
#define ROLL_R_MAX_DEG 2
#define ROLL_L_MAX_DEG 2
#define SD_WRITE_NUM 20
#define INIT_SERVO_PERIOD_MS 20

#define MPU_LOOP_TIME 0.01
#define MPU_DELT_MIN 250

Mutex ssMutex;

//-----------------------------------(resetInterrupt def)
extern "C" void mbed_reset();
InterruptIn resetPin(p26);
Timer resetTimeCount;
void resetInterrupt(){
    while(resetPin){
        resetTimeCount.start();
        if(resetTimeCount.read()>3) mbed_reset();
    }
    resetTimeCount.reset();
}
//-------------------------------------------------------


Cadence cadence(p13,p14);

RawSerial pc(USBTX,USBRX);
//RawSerial Android(p13,p14);
BufferedSoftSerial twe(p11,p12);
RawSerial Android(p9,p10);
BufferedSoftSerial soudaSerial(p17,p18);
//Ticker writeDatasTicker;
Timer writeTimer;

InterruptIn FusokukeiPin(p22);
Ticker FusokukeiTicker;
Fusokukei air;
volatile int air_kaitensu= 0;

float sum = 0;
uint32_t sumCount = 0;
//MPU6050 mpu6050;
float yaw,pitch,roll;
float pre_ypr[3];
float offset_ypr[3];
Timer t;
//
//AnalogIn kx_X(p17);
//AnalogIn kx_Y(p16);
//AnalogIn kx_Z(p15);
//float KX_X,KX_Y,KX_Z;


//Timer sonarTimer;
//InterruptIn sonarPin(p22);
AnalogIn sonarPin(p15);
//double sonarDistTime=0;
double sonarV = 0.0, sonarDist = 0.0;

DigitalOut RollAlarmR(p20);
DigitalOut RollAlarmL(p19);
//DigitalOut led(LED1);//for mpu
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

SDFileSystem sd(p5, p6, p7, p8, "sd");
FILE* fp;

PwmOut kisokuServo(p26);
PwmOut geikakuServo(p22);

char soudaDatas[SOUDA_DATAS_NUM];
float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
volatile int write_datas_index = 0;

void cadenceDataReceive();
void air_countUp();
void call_calcAirSpeed();
void init();
void FusokukeiInit();
void SdInit();
void MpuInit();
//void SonarInit();
void mpuProcessing();
void DataReceiveFromSouda();
void SDprintf(void const *argument);
void WriteDatas();
//float calcAttackAngle();
//float calcKXdeg(float x);

void cadenceDataReceive(){
    while(1){
        cadence.readData();
        Thread::wait(0.01);
    }
}

void air_countUp(){
    air_kaitensu++;
}

void call_calcAirSpeed(){
    air.calcAirSpeed(air_kaitensu);
    air_kaitensu = 0;
    led3 = !led3;
}

//void sonarInterruptStart(){
//    sonarTimer.start();
//    led3 = 1;
//}
//void sonarInterruptStop(){
//    sonarTimer.stop();
//    led3 = 0;
//    sonarDistTime = sonarTimer.read_us();
//    sonarTimer.reset();
//}

void init(){
//--------------------------------------(resetInterrupt init)
    resetPin.rise(resetInterrupt);
    resetPin.mode(PullDown);
//-----------------------------------------------------------
    twe.baud(115200);
    Android.baud(9600);
    soudaSerial.baud(9600);
    kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
    geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
    FusokukeiInit();
//    MpuInit(); //mpuProcessing内で
//    SonarInit();
//    SdInit();//SDprintf 内で
}

void FusokukeiInit(){
    FusokukeiPin.rise(&air_countUp);
    FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
}

double calcPulse(int deg){
    return (0.0006+(deg/180.0)*(0.00235-0.00045));
}

void SdInit(){
    mkdir("/sd/mydir", 0777);
    fp = fopen("/sd/mydir/sdtest2.csv", "w");
    if(fp == NULL) {
        error("Could not open file for write\n");
    }
    fprintf(fp, "Hello fun SD Card World!\n\r");
    fclose(fp);
}

void MpuInit(){
//    i2c.frequency(400000);  // use fast (400 kHz) I2C
//    t.start();
//    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
//    if (whoami == 0x68) { // WHO_AM_I should always be 0x68
//        Thread::wait(1);
//        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
//        Thread::wait(1);
//        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
//            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
//            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
//            mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
//            Thread::wait(2);
//        } else {
//        }
//    } else {
//        pc.printf("MPU6050 not ready...\n\r");
//        //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
//    }
    using namespace MPU6050DMP6;
    setup();
    do{
        for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i];
        getYPR();
        Thread::wait(0.01);
    }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) );
    for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i];
}

//void SonarInit(){
//    sonarPin.rise(&sonarInterruptStart);
//    sonarPin.fall(&sonarInterruptStop);
//}
void sonarCalc(void const *argument){
//    return sonarDistTime*0.018624 - 13.511;
    while(1){
        sonarV = 0;
        for(int i =0; i<20; i++){
            sonarV += sonarPin.read();
            Thread::wait(0.01);
//            wait(0.01);
        }
        sonarDist = (sonarV/20)*2062.5;
        Thread::wait(0.01);
//        wait(0.01);
    }
}

//void mpuProcessing(void const *argument){
//    MpuInit();
//    while(1){
//        Thread::wait(0.1);
//        if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
//            mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
//            mpu6050.getAres();
//            ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
//            ay = (float)accelCount[1]*aRes - accelBias[1];
//            az = (float)accelCount[2]*aRes - accelBias[2];
//            mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
//            mpu6050.getGres();
//            gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
//            gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
//            gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
//            tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
//            temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
//        }
//        Now = t.read_us();
//        deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
//        lastUpdate = Now;
//        sum += deltat;
//        sumCount++;
//        if(lastUpdate - firstUpdate > 10000000.0f) {
//            beta = 0.04;  // decrease filter gain after stabilized
//            zeta = 0.015; // increasey bias drift gain after stabilized
//        }
//        mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
//        delt_t = t.read_ms() - count;
//        if (delt_t > MPU_DELT_MIN) {
//            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
//            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
//            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
//            pitch *= 180.0f / PI;
//            yaw   *= 180.0f / PI;
//            roll  *= 180.0f / PI;
//            myled= !myled;
//            count = t.read_ms();
//            sum = 0;
//            sumCount = 0;
//        }
//    }
//}
void mpuProcessing(void const *argument){
    using namespace MPU6050DMP6;
    setup();
    do{
        for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i];
        getYPR();
        Thread::wait(0.01);
    }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) );
    for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i];
//    MpuInit();
    while(1){
        getYPR();
        yaw = (ypr[0] - offset_ypr[0]) *180/M_PI;
        pitch = (ypr[1] - offset_ypr[1]) *180/M_PI;
        roll = (ypr[2] - offset_ypr[2]) *180/M_PI;
        Thread::wait(0.01);
    }
}

void DataReceiveFromSouda(void const *argument){
    while(1){
        led2 = !led2;
        for(int i = 0; i < SOUDA_DATAS_NUM; i++){
            if(soudaSerial.readable()) {
//                ssMutex.lock();
                soudaDatas[i] = (char)soudaSerial.getc();
//                ssMutex.unlock();
                if(soudaDatas[i]==';') i=-1;
            }else i--;
        }
    }
}

void SDprintf(void const *argument){
    SdInit();
    while(1){
        if(write_datas_index == SD_WRITE_NUM-1){
//            led4 = !led4;
            fp = fopen("/sd/mydir/sdtest.csv", "a");
            if(fp == NULL) {
                error("Could not open file for write\n");
            }
            for(int i = 0; i < SD_WRITE_NUM; i++){
                for(int j = 0; j < WRITE_DATAS_NUM; j++){
                    fprintf(fp,"%f,", writeDatas[i][j]);
                }
            }
            fprintf(fp,"\n\r");
            fclose(fp);
            write_datas_index=0;
        }
        Thread::wait(0.01);
    }
}

void WriteDatas(){
    int i;
    for(i = 0; i < 6; i++){ //翼端のmpu
        if(!(i%2)){
            writeDatas[write_datas_index][i] = (int)( (soudaDatas[i*2]<<8) + soudaDatas[(i+1)*2] );
        }else{
            writeDatas[write_datas_index][i] = (int)( (soudaDatas[i*2-1]<<8) + soudaDatas[i*2+1] );
        }
//        writeDatas[write_datas_index][i] = i;
    }
    for(i = 6; i < 12; i++){//翼端のV,操舵
        //writeDatas[write_datas_index][i] = 0.0;    
        writeDatas[write_datas_index][i] = (float)soudaDatas[i+6];
    }
    writeDatas[write_datas_index][i++] = pitch;
    writeDatas[write_datas_index][i++] = roll;
    writeDatas[write_datas_index][i++] = yaw;
    writeDatas[write_datas_index][i++] = airSpeed;
    writeDatas[write_datas_index][i++] = sonarDist;
    writeDatas[write_datas_index][i++] = cadence.cadence;
    //writeDatas[write_datas_index][i++] = writeTimer.read();
    for(i = 0; i < WRITE_DATAS_NUM; i++){
        pc.printf("%f   ", writeDatas[write_datas_index][i]);
//        twe.printf("%f,", writeDatas[write_datas_index][i]);
    }
    pc.printf("\n\r");
//    twe.printf("\n\r");
    if(write_datas_index < SD_WRITE_NUM-1){
        write_datas_index++;
    } 
    for(int i = 0; i < SOUDA_DATAS_NUM; i++){
//        pc.printf("%i   ",soudaDatas[i]);
        ssMutex.lock();
        twe.printf("%i,",soudaDatas[i]);
        ssMutex.unlock();
    }
    
    if(Android.writeable()){
        Android.printf("%f,%f,%f",airSpeed,roll,0);
    }
    
    ssMutex.lock();
    twe.printf("%f,%f,%f,",pitch,roll,yaw);
//    twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
    twe.printf("%f,\r\n",airSpeed); 
    ssMutex.unlock();
//    pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
    //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
//    pc.printf("%f\n\r",airSpeed);
    //SDprintf();
    pc.printf("%d\n\r",write_datas_index);
}

void WriteDatasF(){
    pc.printf("airSpeed:%f\n\r",airSpeed);
}

//float calcKXdeg(float x){
//    return -310.54*x+156.65;
//}

//float calcAttackAngle(){
//    return pitch-calcKXdeg(kx_Z.read());
//}

void RollAlarm(){ 
    if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
        RollAlarmL = 1;
    }
    else{
        RollAlarmL = 0;
    }
    if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){
        RollAlarmR = 1;
    }
    else{
        RollAlarmR = 0;
    }
}



int main(){ 
    Thread sd_thread(&SDprintf);//必ずmain内で
    Thread sonar_thread(&sonarCalc);
    Thread mpu_thread(&mpuProcessing);
    Thread soudaSerial_thread(&DataReceiveFromSouda);
    Thread cadence_thread(&cadenceDataReceive);
    init();
    while(1){
        pc.printf("test\n\r");
        RollAlarm();
//        DataReceiveFromSouda();
//        cadence.readData();
        WriteDatas();
//        mpuProcessing();
    }
}