File content as of revision 0:05cb0f94324f:

#include "mbed.h"
#include "HMC5883L.h"
#include "MPU6050.h"
#include "ms5611.h"
#include "MadgwickAHRS.h"
#include "ESC.h"
#include "MemLog.h"
#include "defines.h"
#include "utils.h"

#define DEBUG // Debug to COM
//#define DEBUG_F // Debug to flash

#define FLY_MODE_X

#ifdef DEBUG
Serial pc(USBTX, USBRX);
#define DBG_DELAY 0.1
#endif

#ifdef DEBUG_F
MemLog flog(2, 2000);
#endif

// x1, x2, y1, y2
ESC esc( p21, p22, p23, p24, LED4 );

// sda, scl
HMC5883L mag_src(p28, p27);
ms5611 bar_src(p28, p27);
MPU6050 mpu_src(p28, p27);
I2CSlave i2c(p9, p10);

DigitalOut led_1(LED1);
DigitalOut led_2(LED2);
DigitalOut led_3(LED3);
DigitalOut buzz(p17);

DigitalIn cal_mode(p16);

Timer tmr;

typedef float TVector3[3];
typedef float TVector4[4];

struct TThrust {
    float x1,x2,y1,y2;
};

struct TPID{
    short p,i,d;
};

int mode = 0;

int16_t mag_raw[3], acc_raw[3];
//gyr_raw[3]
float spd_yaw, spd_alt, thr_base, thr_avg, thr_to, altz, acc_1g;
float bar_table[BAR_TABLE_SIZE], bar_alt, bar_lpf, bar_base, bar_table_sum;
uint8_t bar_table_idx;
TVector3 gyr, acc_lpf, mag, grv, grv_old, grv_err;
TVector4 pos, pos_base, pos_offset, imu, pid, pid_ist, pid_dst, pid_err, quat;
TThrust thr;
TPID pidk[4];

//DEBUG
float debug_data_1;

// hs battop
const float mag_cal_def[3][3] = MAG_CAL;
const float mag_off_def[3] = MAG_OFF;
const float pidk_def[4][3] = MAG_CAL;

void get_mag();

// Calibrate accel offsets and set it up to adxl345 registers
void calibrate_acc_offsets(){
    float biasa[3], biasg[3];
    mpu_src.calibrate(biasa,biasg);
}

void calibrate_acc_grv(){
    int16_t data[3];
    int32_t avg[3] = {0,0,0};
    for(uint16_t i=0; i<100; i++ ){
        mpu_src.getAccelRaw(data);
        avg[X] += data[X];
        avg[Y] += data[Y];
        avg[Z] += data[Z];
        wait_ms(10);
    }
    
    // calc averages and set default values for lpf
    for(uint8_t i=0; i<3; i++ ){
        avg[i] = avg[i]/100;
        acc_lpf[i] = float(avg[i]);
    }
    acc_1g = 1.0f/invSqrt(avg[X]*avg[X] + avg[Y]*avg[Y] + avg[Z]*avg[Z]);
    
    grv_err[2] = avg[Z] - acc_1g;
}

// Calibrate barometer level
void calibrate_bar(){
    float bar_sum = 0.0f;
    blink(2000, &led_1);
    
    // 1 measure per 3 iterations
    for(uint16_t i=0; i<300; i++){
        if( bar_src.calcPT() ){
            bar_sum += bar_src.getAltitude();
        }
        wait_ms(10);
    }

    bar_base = bar_sum/100.0f;
    
    //fill default bar table
    bar_table_sum = 0.0f;
    for(uint8_t i=0; i<BAR_TABLE_SIZE; i++){
        bar_table[i] = bar_base;
        bar_table_sum += bar_base;
    }
}

// Get compass heading. Add current magnetic declination magnitude and correct angle of copter nose.
int get_heading(){
    float yaw = imu[YAW] * TO_DEG - MAG_DECL + 90;
    if( yaw > 180.0 )
        yaw -= 360.0;
    else if( yaw < -180.0 )
        yaw += 360.0;
    return yaw;    
}
    
// Calc Z-acceleration vector without gravitational part
void get_grv(float g[3]){
    g[X] = 2 * (quat[1] * quat[3] - quat[0] * quat[2]);
    g[Y] = 2 * (quat[0] * quat[1] + quat[2] * quat[3]);
    g[Z] = (quat[0] * quat[0] - quat[1] * quat[1] - quat[2] * quat[2] + quat[3] * quat[3]);
}

// Calc barometer altitude
void get_bar(){
    float bar_raw;
    if( bar_src.calcPT() ){
        bar_raw = bar_src.getAltitude(); // meters
        // LPF over baro values
        uint8_t bar_table_idx1 = (bar_table_idx + 1);
        if (bar_table_idx1 == BAR_TABLE_SIZE) 
            bar_table_idx1 = 0;
        bar_table[bar_table_idx] = bar_raw;
        bar_table_sum += bar_table[bar_table_idx];
        bar_table_sum -= bar_table[bar_table_idx1];
        bar_table_idx = bar_table_idx1;        

        bar_lpf = bar_lpf*(1-LPFC_BAR) + bar_table_sum*(LPFC_BAR)/21.0f;
        
        // relative bar
        //bar_alt = clamp_bot(bar_base - bar_lpf, 0);
        bar_alt = bar_lpf - bar_base;
    }    
}

// Calc Z-displacement after 2-stage integration
void get_alt( float tdelta ){
    float vdelta;
    float g[3];
    float alt_err = 0.0f;
    float tmp = 1.0f;
    
    // don't fuse baro in takeoff mode
    if( !(mode & MODE_TAKEOFF) && !(mode & MODE_ON) ){
        alt_err = clamp((imu[ALT] - bar_alt), -0.5, 0.5) * ALT_P;
        tmp = alt_err*ALT_ACC_I;
        grv_err[Z]+= acc_lpf[Z] * tmp;

        // don't take into account X and Y if UAV position is almost vertical
        if(abs(imu[ROLL]) > GRAD5 || abs(imu[PITCH]) > GRAD5) {
            grv_err[X]+= acc_lpf[X] * 2 * tmp; 
            grv_err[Y]+= acc_lpf[Y] * 2 * tmp; 
        }
    }    
    
    // fetch gravitational vector from position quaternion
    get_grv(&g[0]);
    
    tmp = (acc_lpf[X]-grv_err[0]) * g[X] + (acc_lpf[Y]-grv_err[1]) * g[Y];

    grv_old[0] = grv[0];
    grv[0] = ((tmp + (acc_lpf[Z] - grv_err[2]) * g[Z]) * invSqrt(g[X]*g[X] + g[Y]*g[Y] + g[Z]*g[Z]) - acc_1g)* ACC_GS;
    
    // calc velocity in meters per seconds by median integration
    grv_old[1] = grv[1];
    grv[1] = grv[1] + deadband((grv[0]+grv_old[0])/2.0f, ACC_DEADBAND) * tdelta;

    // calc displacement in meters by median integration
    vdelta = (grv[1]+grv_old[1]) * tdelta / 2.0f;
    
    if( !(mode & MODE_TAKEOFF) && !(mode & MODE_ON) ){
        vdelta -= alt_err*ALT_VEL_P;
    }    
    imu[ALT] += vdelta - alt_err;
  
    // pressure and gravitational altitude fusing
    //imu[ALT] = imu[ALT]*(1-LPFC_ALT) + bar_alt*LPFC_ALT;
}

// Calc accel
void get_acc(){
    mpu_src.getAccelRaw(acc_raw);
    
    // LPF over accel values
    for(uint8_t i=0; i<3; i++ ){
        acc_lpf[i] = acc_lpf[i]*(1-LPFC_ACC) + acc_raw[i]*LPFC_ACC;
    }
}

// Calc gyro
void get_gyr(){
    mpu_src.getGyro(gyr);
    for(uint8_t i=0; i<3; i++ ){
        gyr[i] = TO_RAD*gyr[i];
    }
}

// Calc magnitude
void get_mag(){
    mag_src.getXYZ(mag_raw);

    for(uint8_t i=0; i<3; i++ ){
        mag[i] = mag_raw[i] - mag_off_def[i];
    }

    mag[X] = mag_cal_def[0][0]*mag[X] + mag_cal_def[0][1]*mag[Y] + mag_cal_def[0][2]*mag[Z];
    mag[Y] = mag_cal_def[1][0]*mag[X] + mag_cal_def[1][1]*mag[Y] + mag_cal_def[1][2]*mag[Z];
    mag[Z] = mag_cal_def[2][0]*mag[X] + mag_cal_def[2][1]*mag[Y] + mag_cal_def[2][2]*mag[Z];
}

// Calc pitch, roll, yaw angles
void get_imu( float tdelta ) {
    MadgwickAHRSupdate( tdelta, gyr[X], gyr[Y], gyr[Z], 
                                float(acc_raw[X]), float(acc_raw[Y]), float(acc_raw[Z]), 
                                mag[X], mag[Z], mag[Y]);

    quat[0] = q0; quat[1] = q1; quat[2] = q2; quat[3] = q3;
    quat_2_euler(&quat[0], &imu[0]); // check imu size (4 actually)
}    

void takeoff_init(){
    // Setup base yaw
    pos_base[YAW] = pos[YAW] = imu[YAW];

    // Setup base alt
    pos_base[ALT] = pos[ALT] = imu[ALT];

    // reset velocity
    grv[1] = 0.0f;
}    

void takeoff_process(){
    thr_to += TAKEOFF_SPEED;
    if( imu[ALT] > TAKEOFF_LEV_ALT ){
        // remove takeoff flag, add altitude stab flag and set target altitude
        mode = mode ^ MODE_TAKEOFF;
        mode = mode | MODE_STAB_ALT;
        pos[ALT] += TAKEOFF_CLIMB_ALT;
    }
}

void calc_thrust( float tdelta ){
    float p, i, d, error;
    float thr_a, thr_base;
    float mpos;
    TThrust thr_pry;

    // x (roll)
    #ifdef FLY_MODE_X
    mpos = rotx( imu[ROLL], imu[PITCH] );
    error = rotx(pos[ROLL] + pos_offset[ROLL], pos[PITCH]) - mpos;
    #else
    mpos = imu[ROLL]; // moment pos
    error = pos[ROLL] + pos_offset[ROLL] - mpos;
    #endif
    p = pidk[ROLL].p * error;
    pid_ist[ROLL] = clamp( pid_ist[ROLL] + error * tdelta, -1, 1 );
    i = pidk[ROLL].i * pid_ist[ROLL];
    d = pidk[ROLL].d * (pid_dst[ROLL] - mpos);
    pid_dst[ROLL] = mpos;

    pid[ROLL] = p + i + d;

    // y (pitch)
    mpos = roty( imu[ROLL], imu[PITCH] );
    #ifdef FLY_MODE_X
    error = roty(pos[ROLL], pos[PITCH] + pos_offset[PITCH]) - mpos;
    #else
    error = pos[PITCH] + pos_offset[PITCH] - mpos;
    #endif
    p = pidk[PITCH].p * error;
    pid_ist[PITCH] = clamp( pid_ist[PITCH] + error * tdelta, -1, 1 );
    i = pidk[PITCH].i * pid_ist[PITCH];
    d = pidk[PITCH].d * (pid_dst[PITCH] - mpos);
    pid_dst[PITCH] = mpos;

    pid[PITCH] = p + i + d;
    
    pos[YAW] = pos[YAW]+spd_yaw*tdelta;
    // z (yaw)
    mpos = imu[YAW];
    error = pos[YAW] - mpos;
    if( error > 180.0*TO_RAD )
        error -= 360.0*TO_RAD;
    else if( error < -180.0*TO_RAD )
        error += 360.0*TO_RAD;
    
    p = pidk[YAW].p * error;
    pid_ist[YAW] = clamp( pid_ist[YAW] + error * tdelta, -3, 3 );
    i = pidk[YAW].i * pid_ist[YAW];
    d = pidk[YAW].d * (pid_dst[YAW] - mpos);
    pid_dst[YAW] = mpos;

    pid[YAW] = p + i + d;

    // Thrust only for PRY stabilization
    thr_pry.x1 = -pid[ROLL] * ROLL_SCALE - YAW_PID_DIR * pid[YAW] * YAW_SCALE;
    thr_pry.x2 = +pid[ROLL] * ROLL_SCALE - YAW_PID_DIR * pid[YAW] * YAW_SCALE;
    thr_pry.y1 = +pid[PITCH] * PITCH_SCALE + YAW_PID_DIR * pid[YAW] * YAW_SCALE;
    thr_pry.y2 = -pid[PITCH] * PITCH_SCALE + YAW_PID_DIR * pid[YAW] * YAW_SCALE;

    // do alt stabilization only if automatic miode is on
    if( mode & MODE_STAB_ALT && !(mode & MODE_MANUAL ) ){
        // increase target altitude according to current speed
        pos[ALT] = pos[ALT] + spd_alt*tdelta;
        mpos = imu[ALT];
        error = pos[ALT] - mpos;
        p = pidk[ALT].p * error;
        pid_ist[ALT] = clamp( pid_ist[ALT] + error * tdelta, -1, 1 );
        i = pidk[ALT].i * pid_ist[ALT];
        d = pidk[ALT].d * (pid_dst[ALT] - mpos);
        pid_dst[ALT] = mpos;

        pid[ALT] = p + i + d;
        
        thr_a = pid[ALT] * ALT_SCALE;
    } else {
        pos[ALT] = imu[ALT];
        thr_a = 0.0;
    }        

    if( mode & MODE_MANUAL )
        thr_base = clamp(spd_alt*4, 0.0, 1.0);
    else
        thr_base = thr_to;

    // Summary thrust for PRY + ALT stabilization
    thr.x1 = clamp( thr_base + thr_pry.x1 + thr_a, 0.0, 1.0 );
    thr.x2 = clamp( thr_base + thr_pry.x2 + thr_a, 0.0, 1.0 );
    thr.y1 = clamp( thr_base + thr_pry.y1 + thr_a, 0.0, 1.0 );
    thr.y2 = clamp( thr_base + thr_pry.y2 + thr_a, 0.0, 1.0 );
    thr_avg = (thr.x1 + thr.x2 + thr.y1 + thr.y2) / 4.0f;
}

void init(){
    // reset pid values, pid coeffs, and pos offsets
    for(uint8_t i=0; i<4; i++){
        pos_offset[i] = 0.0;

        pid_ist[i] = 0.0;
        pid_dst[i] = 0.0;

        pidk[i].p = pidk_def[i][0];
        pidk[i].i = pidk_def[i][1];
        pidk[i].d = pidk_def[i][2];

        pos[i] = 0.0;
    }

    // desire xyz
    imu[ALT] = 0.0;

    spd_alt = 0;
    thr_to = 0.0f;
    thr_base = 0.0f;
    thr_avg = 0.0f;

    for(uint8_t i=0; i<3; i++){
        grv[i] = 0.0f;
        grv_err[i] = 0.0f;
    }
    altz = 0;

    bar_lpf = 0.0f;
    bar_table_idx = 0;

    acc_1g = 0.0f;

}

void stop(){
    esc.stop();
    #ifdef DEBUG_F
    flog.save();
    #endif
}

void checkMode(){
    if( mode & MODE_READY ){
        mode = mode ^ MODE_READY;
        mode = mode | MODE_STAB_PRY;
        if( !(mode & MODE_MANUAL) ){
            mode = mode | MODE_TAKEOFF;
        }
        takeoff_init();
    }
    else if ( mode & MODE_MANUAL){
        //pass
    }
    else {
    }
}

void obey(){
    char cbuf[32]; 
    short sbuf[16]; 
    char cbufn = 15;

    int i = i2c.receive();
    switch (i) {
        case I2CSlave::ReadAddressed:
            sbuf[0] = (short)(debug_data_1*10000);//(thr_avg*100);
            sbuf[1] = (short)(imu[ALT]*100);
            sbuf[2] = (short)(pos[ALT]*100);
            sbuf[3] = (short)(get_heading());
            sbuf[4] = (short)(imu[ROLL]*TO_DEG);
            sbuf[5] = (short)(imu[PITCH]*TO_DEG);
            sbuf[6] = (short)(mode);
            short2char( &sbuf[0], &cbuf[0], 0, 7 );
            i2c.write( cbuf, 14 );
            break;

        case I2CSlave::WriteAddressed:
            //receive on/off command
            i2c.read(cbuf, 32);
            if( cbuf[0]==CMD_I2C_OFF ){
                if( mode & MODE_ON ){
                    stop();
                    mode = 0; // remove MODE_ON flag
                    beep(5, 50, &buzz);
                    beep(1, 200, &buzz);
                }
            }
            else if( cbuf[0]==CMD_I2C_ON ){
                if( !(mode & MODE_ON) ){
                    beep(1, 200, &buzz);
                    beep(5, 50, &buzz);
                    mode = mode | MODE_READY | MODE_ON; // set default flags
                }
            }
            // receive manual/auto command
            else if( cbuf[0]==CMD_I2C_AUTO ){
                if( mode & MODE_MANUAL ){
                    beep(6, 50, &buzz);
                    beep(1, 200, &buzz);
                    mode = mode ^ MODE_MANUAL; // remove MODE_MANUAL flag
                }
            }
            else if( cbuf[0]==CMD_I2C_MANUAL ){
                if( !(mode & MODE_MANUAL) ){
                    beep(1, 200, &buzz);
                    beep(6, 50, &buzz);
                    mode = mode | MODE_MANUAL; // add MODE_MANUAL flag
                }
            }
            // receive control values
            else if( cbuf[0]==CMD_I2C_CTRL ){
                char2short( &cbuf[0], &sbuf[0], 1, cbufn*2 );
                spd_alt = sbuf[0]*ALT_MAX/CTRL_DIA;
                pos[PITCH] = (sbuf[1]*PITCH_MAX/CTRL_DIA)*TO_RAD;
                pos[ROLL] = (sbuf[2]*ROLL_MAX/CTRL_DIA)*TO_RAD;
                spd_yaw = (sbuf[3]*YAW_MAX/CTRL_DIA)*TO_RAD;
                pidk[ROLL].p = sbuf[4];
                pidk[PITCH].p = sbuf[4];
                pidk[ROLL].i = sbuf[5];
                pidk[PITCH].i = sbuf[5];
                pidk[ROLL].d = sbuf[6];
                pidk[PITCH].d = sbuf[6];
                pidk[YAW].p = sbuf[7];
                pidk[YAW].i = sbuf[8];
                pidk[YAW].d = sbuf[9];
                pidk[ALT].p = sbuf[10];
                pidk[ALT].i = sbuf[11];
                pidk[ALT].d = sbuf[12];
                pos_offset[ROLL] = sbuf[13]*TO_RAD/10.0;
                pos_offset[PITCH] = sbuf[14]*TO_RAD/10.0;
            }    
            break;
    }
}

int main() {
    double tc, td, incl_time, ctrl_time, mag_time, bar_time;
    incl_time = ctrl_time = mag_time = bar_time = 0;

    // Variables init
    init();
    
    blink(1000, &led_1);

    // set i2c address
    i2c.address(0xA0);

    #ifdef DEBUG
    float dbg_time = 0;
    pc.baud(19200);
    #endif

    // ESC init
    esc.initialize_();

    // Accel and Gyro init
    mpu_src.init();
    mpu_src.getAres();
    mpu_src.getGres();

    // Barometer init
    bar_src.cmd_reset();
    calibrate_bar();
    
    beep(2, 100, &buzz);
    beep(1, 500, &buzz);
    
    // Accel optional calibration
    if( cal_mode ){
        calibrate_acc_offsets();
        beep(3, 100, &buzz);
        beep(1, 500, &buzz);
    };
    calibrate_acc_grv();
    beep(4, 100, &buzz);
    beep(1, 500, &buzz);
    
    // Magnitometer init
    mag_src.init();

    blink(1000, &led_1);
    beep(1, 1000, &buzz);
    
    tmr.start();
    while(1){
        obey();

        tc = tmr.read();
        if( mag_time + MAG_DELAY < tc ){
            mag_time = tmr.read();
            get_mag();
        }

        tc = tmr.read();
        if( bar_time + BAR_DELAY < tc ){
            bar_time = tc;
            get_bar();
        }

        tc = tmr.read();
        if( incl_time + INCL_DELAY < tc ){
            td = tc - incl_time;
            incl_time = tc;
            
            get_acc(); // collect accel data
            get_gyr(); // collect gyro data
            get_imu(td); // calc angles, after acc & gyr calcs
            get_alt(td); // calc gravity altitude, after grv calc
            debug_data_1 = td;
        }

        tc = tmr.read();
        if( ctrl_time + CTRL_DELAY < tc ){
            td = tc - ctrl_time;
            ctrl_time = tc;

            #ifdef DEBUG_F
            flog.log(imu[X]*TO_DEG);
            flog.log(gyr[X]);
            #endif

            if( mode & MODE_TAKEOFF ){
                takeoff_process();
            }

            if( mode & MODE_ON ){
                calc_thrust( td );
                esc.set(thr.x1, thr.x2, thr.y1, thr.y2);
            }    
        }

        #ifdef DEBUG
        tc = tmr.read();
        if( dbg_time + DBG_DELAY < tc ){
            dbg_time = tc;
            pc.printf("imu %+7.2f %+7.2f %+7.2f %+7.2f %+7.2f \r\n", TO_DEG*imu[X], TO_DEG*imu[Y], TO_DEG*imu[Z], bar_alt, imu[ALT]);
            //pc.printf("imu %d %d %d %+7.2f %+7.2f %+7.2f \r\n", acc_raw[0], acc_raw[1], acc_raw[2], gyr[0], gyr[1], gyr[2]);
        }
        #endif

        if( mode & MODE_ON )
            checkMode();
    }        
}