File content as of revision 78:e272e65f5a0e:

//翼端can program
/*****************************************
* -----
* | 1 | Is R pin
* | 2 | In set mode pin
* | 3 | on:Elevon, off:Drug
* | 4 | on:Trim, off:Max deg
* -----
*
******************************************
**magic character of debug**
*
*(0)s:sending datas: mpu, servoV
*(1)g:getting datas: eruronfloat, drugInput, servoOff
*(2)c:servo config:eruronTrim,drugTrim,eMD,dMD
*(3)j:data to debug what you want(joker)
******************************************
*/
#include "mbed.h"
#include "MPU6050.h"
#include "INA226.hpp"
#include "rtos.h"
#include "XBusServo.h"
#include "math.h"

#define INIT_SERVO_PERIOD_US 10000
#define WAIT_LOOP_TIME 0.02
#define YOKUTAN_DATAS_NUM 7
#define INPUT_DATAS_NUM 4 //ここは8バイトまでしかCANでは一度に送れないため、8以下。そして、操舵コードと数字を合わせる必要あり。
#define ERURON_DATAS_NUM 3 //送られてくるエルロンインプットの文字数
#define TO_SEND_CAN_ID 0x701 //0x0>>0x7ff
#define SEND_DATAS_LOOP_TIME 0.1
#define RECEIVE_DATAS_LOOP_TIME 0.1

#define MPU_LOOP_TIME 0.01
#define MPU_DELT_MIN 250

//値を大きくするとサーボに向かって反時計回りになる
//Rエレボンは頭上げ==値大きく
#define ERURON_MOVE_DEG_INI_R 0.3
#define ERURON_TRIM_INI_R 0.5
//Rラダーは右回り(時計回り)が開く
#define DRUG_MOVE_DEG_INI_R -1.41297
#define DRUG_TRIM_INI_R  1.014408
//Lエレボンは頭上げ==値小さく
#define ERURON_MOVE_DEG_INI_L -0.32
#define ERURON_TRIM_INI_L 0.51567
//Lラダーは左回りが開く
#define DRUG_MOVE_DEG_INI_L 0.983027
#define DRUG_TRIM_INI_L -0.1

/*ドラッグラダー
初期値 0.65
最大角0.99
*/

#define print2pc(flag,fmt,...) do{if(flag){pc.printf(fmt,__VA_ARGS__);}}while(0)
#define SENDING_DATA_DEBUG_FLAG debugflag[0].flag
#define GETTING_DATA_DEBUG_FLAG debugflag[1].flag
#define SERVO_CONFIG_DEBUG_FLAG debugflag[2].flag
#define DEBUG_FLAG debugflag[3].flag
#define DRUG_OPEN_FLAG debugflag[4].flag

struct flaglist{
    char key;
    bool flag;
};
struct flaglist debugflag[]={
    {'s',0},
    {'g',0},
    {'c',0},
    {'j',0},
    {'d',0},
    {'0',0}
};

const char *configfilename = "/local/CONFIG.csv";

CAN can(p30,p29);
CANMessage recmsg;
Serial pc(USBTX,USBRX);
I2C ina226_i2c(p28,p27);
INA226 VCmonitor(ina226_i2c);
PwmOut drugServo(p23);
PwmOut eruronServo(p25);
DigitalOut led1(LED1);
AnalogIn setDeg10(p20);
AnalogIn setDeg1(p19);
DigitalIn IsRPin(p16,PullDown);
DigitalIn InSetModePin(p15,PullDown);
DigitalIn EDstatePin(p14,PullDown);
DigitalIn TrimMaxDegPin(p13,PullDown);
DigitalOut servoOff(p17);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
Ticker sendDatasTicker;
//Ticker toStringTicker;
Ticker receiveDatasTicker;
MPU6050 mpu6050;
Timer t;

LocalFileSystem local("local");

char toSendDatas[YOKUTAN_DATAS_NUM];
char intValues[ERURON_DATAS_NUM];
float eruronTrim;
float drugTrim;
float eruronMoveDeg;
float drugMoveDeg;
float eruronfloat = 0.0;
unsigned short ina_val;
double V,C;
bool SERVO_FLAG;
bool INA_FLAG;
bool MPU_FLAG;

int gyroX;
int gyroY;
int gyroZ;
float sum = 0;
int drugInput = 0;
int servoOffVer = 0;
uint32_t sumCount = 0;

char gyro_c[6] = {0,0,0,0,0,0};

void toString();
void receiveDatas();
void WriteServo();
void MpuInit();
int calcPulse_us(float analog);
void mpuProcessing(void const *arg);

Ticker gTimer;



void receiveFromPc(){
    while(pc.readable()){
        char c = pc.getc();
        for(int i = 0; debugflag[i].key != '0'; i++){
                if(debugflag[i].key == c)
                    debugflag[i].flag = !(debugflag[i].flag);
        }
    }
}

bool servoInit()
{
    drugServo.period_us(INIT_SERVO_PERIOD_US);
    return true;
}

void sendDatas()
{
    if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, YOKUTAN_DATAS_NUM))) {
    }
}

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(100);
        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
        Thread::wait(100);
        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(200);
        } else {
        }
    } else {
        //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
    }
}


void mpuProcessing(void const *arg)
{
    MpuInit();
    while(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;
        }
        gyro_c[0]=(char)((int)pitch);
        gyro_c[1]=(char)(int)((pitch - (int)pitch)*100);
        gyro_c[2]=(char)((int)roll);
        gyro_c[3]=(char)(int)((roll - (int)roll)*100);
        gyro_c[4]=(char)((int)yaw);
        gyro_c[5]=(char)(int)((yaw - (int)yaw)*100);
        
        Thread::wait(1);
    }//while(1)
}


bool inaInit()
{
    if(!VCmonitor.isExist()) {
        pc.printf("VCmonitor NOT FOUND\n");
        return false;
    }
    ina_val = 0;
    if(VCmonitor.rawRead(0x00,&ina_val) != 0) {
        pc.printf("VCmonitor READ ERROR\n");
        return false;
    }
    VCmonitor.setCurrentCalibration();
    return true;
}

//動かしたいエレボンの角度から、動かしたいサーボホーンの角度を得る。
double ConvertDeg(double servo)
{
    return 0.0011 * pow(servo,3) + 0.017 * pow(servo,2) + 2.3019 * servo - 0.2269;
    //return 0.0003*pow(servo,3)+0.0039*pow(servo,2)+1.746*servo - 0.0105;
}

//ホーンを動かしたい角度から、変化させるアナログ値の幅を得る。3度変化
double GetValueByHorn(double deg)
{
    return deg*(0.10/25.7);
}

double GetFloatByErebon(double erebonDeg)
{
    double servoDeg = ConvertDeg(erebonDeg);
    return GetValueByHorn(servoDeg);
}
// configファイル作成
int makeConfigFile() {
    FILE *fp;
    if((fp = fopen(configfilename, "w")) == NULL) {
        pc.printf("can't open %s\n", configfilename);
        return -1;
    }
    fprintf(fp, "// This is Yokutan %s.\n", IsRPin ? "R" : "L");
    fprintf(fp, "eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg\n");
    fclose(fp);
    return 0;
}

// config内容をファイルに追記
int writeConfig() {
    FILE *fp;
    if((fp = fopen(configfilename, "a")) == NULL) {
        pc.printf("can't open %s\n", configfilename);
        return -1;
    }
    fprintf(fp, "%f,%f,%f,%f\n", eruronTrim, drugTrim, eruronMoveDeg, drugMoveDeg);
    fclose(fp);
    return 0;
}
// configの最新情報を読み込み
int readConfig() {
    FILE *fp;
    char s[256];
    if((fp = fopen(configfilename, "r")) == NULL) {
        return -1;
    }
    while(fgets(s, 255, fp) != NULL) {
        sscanf(s, "%f,%f,%f,%f\n", &eruronTrim, &drugTrim, &eruronMoveDeg, &drugMoveDeg);
    }
    fclose(fp);
    //debug("eruronTrim:%f,drugtrim:%f,eMD:%f,dMD:%f\n\r",eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg);
    return 0;
}

void init()
{
    if(readConfig() == -1) {
        makeConfigFile();
        if(IsRPin) {
            eruronTrim = ERURON_TRIM_INI_R;
            drugTrim = DRUG_TRIM_INI_R;
//            eruronMoveDeg =GetFloatByErebon(ERURON_MOVE_DEG_INI_R);
            eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
            drugMoveDeg =DRUG_MOVE_DEG_INI_R;
        } else {
            eruronTrim = ERURON_TRIM_INI_L;
            drugTrim = DRUG_TRIM_INI_L;
//            eruronMoveDeg = GetFloatByErebon(ERURON_MOVE_DEG_INI_L);
            eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
            drugMoveDeg = DRUG_MOVE_DEG_INI_L;
        }
        writeConfig();
    }
    SERVO_FLAG = servoInit();
    INA_FLAG = inaInit();
    sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
    // toStringTicker.attach(&toString,0.5);
    receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);
}

void updateDatas()
{
    if(INA_FLAG) {
        int tmp = VCmonitor.getVoltage(&V);
        tmp = VCmonitor.getCurrent(&C);
    }
    uint8_t r[4];
    uint8_t y[4];
    int rl,yw;
    rl = (int)(roll*10000);
    yw = (int)(yaw*10000);
    r[2] = (rl&0x00ff0000)>>16;  y[2] = (yw&0x00ff0000)>>16;
    r[1] = (rl&0x0000ff00)>>8;   y[1] = (yw&0x0000ff00)>>8;
    r[0] = (rl&0x000000ff);      y[0] = (yw&0x000000ff);
    r[3] = (r[2]>>7 == 0)?0:0xff;y[3] = (y[2]>>7 == 0)?0:0xff;//r[3]can make from r[2]
    int i = 0;
    for(;i<3;i++)
        toSendDatas[i] = r[i];
    for(;i<6;i++)
        toSendDatas[i] = y[i-3];
    toSendDatas[i] = (char)V;
    print2pc(SENDING_DATA_DEBUG_FLAG,"p:%12f,r:%12f,y:%12f,servoV%12f    r[]:%d,y[]:%d\n\r"
                                        ,pitch,roll,yaw,V,*(int*)r,*(int*)y);
}

void receiveDatas()
{
    if(can.read(recmsg)) {              //ここの中でpc.printfすると固まるので注意
        for(int i = 0; i < ERURON_DATAS_NUM; i++) {
            intValues[i] = recmsg.data[i];
        }
        drugInput = (recmsg.data[3]-'0')/2;
        servoOffVer = ((recmsg.data[3]-'0')%2);
        eruronfloat = atoi(intValues)/100.0;
        led1 = !led1;
    }
    servoOff = servoOffVer;
}

//double calcPulse(float analog)
//{
//    return (0.0006 + (analog)*(0.00240-0.00060) );
//}
int calcPulse_us(float analog)
{
    return (1000 + 1000*analog);
}

float getPinDeg()
{
    float setPin10 = -setDeg10.read() + 1.0;
    return (int)(setPin10*9)/10.0 + setDeg1.read()/10;
}

void WriteServo()
{
    // for(int i = 0; i< 10; i++) {
//        pc.printf("%c",floatValues[i]);
//    }
//    pc.printf(" : %f",eruronfloat);
//    pc.printf("\n\r");
    eruronServo.pulsewidth_us(calcPulse_us( eruronTrim + eruronMoveDeg * eruronfloat));
    if(DRUG_OPEN_FLAG)drugServo.pulsewidth_us(calcPulse_us(drugTrim + drugMoveDeg));
    else drugServo.pulsewidth_us(calcPulse_us(drugTrim + drugMoveDeg * drugInput));
    print2pc(GETTING_DATA_DEBUG_FLAG,"ef:%5.2f di:%d so:%d\n\r",eruronfloat,drugInput,int(servoOff));
    print2pc(SERVO_CONFIG_DEBUG_FLAG,"eTr:%f   dTr:%f   eMD:%f   dMD:%f\n\r",eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg);
    print2pc(DEBUG_FLAG,"servoOffVer:%d\n\r",servoOffVer);
}

void setTrim(PwmOut servo, float* servoTr, float initServoTrim, float initPinState)
{
    led2 = 1;
    led4 = 0;
    if(*servoTr<0)led2 = 0;
    if(*servoTr>1.1) led1 = 1;
    if(int(*servoTr * 10) == 0) led4 = 1;
//    if(EDstatePin) {
//        eruronTrim = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initEruronState;
//        eruronServo.pulsewidth_us(calcPusle_us(eruronTrim));
//    } else {
//        drugTrim = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initDrugState;
//        drugServo.pulsewidth_us(calcPulse_us(drugTrim));
//    }
    *servoTr = getPinDeg() - initPinState + initServoTrim;
    servo.pulsewidth_us(calcPulse_us(*servoTr));
    
    pc.printf("eTr:%f   dTr:%f   ",eruronTrim,drugTrim);
    pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
}

void setMaxDeg(PwmOut servo, float* servoMD, float initServoTr, float initServoMD, float initPinState)
{
    led4 = 1;
    led2 = 0;
//    float eruronTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 - initPinState + initEruronState;
//    float drugTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 - initPinState + initDrugState;
    float temp = getPinDeg() - initPinState + initServoTr + initServoMD;
    
//    if(EDstatePin) {
//        eruronMoveDeg = eruronTemp-eruronTrim;
//        if(eruronTemp < 0)led4 = 0;
//        else eruronServo.pulsewidth_us(calcPusle_us(eruronTemp));
//    } else {
//        drugMoveDeg = drugTemp-drugTrim;
//        if(drugTemp < 0)led4 = 0;
//        else drugServo.pulsewidth_us(calcPusle_us(drugTemp));
//    }
    *servoMD = temp-initServoTr;
    if(temp<0)led4 = 0;
    if(temp>1.1) led1 = 1;
    if(int(temp * 10) == 0) led2 = 1;
    else servo.pulsewidth_us(calcPulse_us(temp));
    
    pc.printf("eTr:%f   dTr:%f   ",eruronTrim,drugTrim);
    pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
    
    wait_us(10);
}

int main()
{
    init();
    Thread mpu_thread(&mpuProcessing);

//    start motion
//    gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);

    while(1) {
        if(InSetModePin){
            float initEruronTrim = eruronTrim;
            float initDrugTrim = drugTrim;
            float initEruronMD = eruronMoveDeg;
            float initDrugMD = drugMoveDeg;
            float initPinState = getPinDeg();
            do{
                if(TrimMaxDegPin){
                    if(EDstatePin)
                        setTrim(eruronServo, &eruronTrim, initEruronTrim, initPinState);
                    else
                        setTrim(drugServo, &drugTrim, initDrugTrim, initPinState);
                }
                else{
                    if(EDstatePin)
                        setMaxDeg(eruronServo, &eruronMoveDeg, initEruronTrim, initEruronMD, initPinState);
                    else
                        setMaxDeg(drugServo, &drugMoveDeg, initDrugTrim, initDrugMD, initPinState);
                }
                led1 = 0;
            }while(InSetModePin);
            writeConfig();
        }
        led4 = 0;
        led2 = 0;
        receiveDatas();
        sendDatas();
        receiveFromPc();
        WriteServo();
        updateDatas();
        led3 = !led3;
        wait(WAIT_LOOP_TIME);
    }
}