File content as of revision 2:a79a9c9f83a4:

/* mbed Microcontroller Library
 * Copyright (c) 2018 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include "ak8963.h"
#include <stddef.h>
#include <stdio.h>                // This ert_main.c example uses printf/fflush
#include "LAAP.h"                      // Model's header file
#include "rtwtypes.h"

static LAAPModelClass rtObj;           // Instance of model class

void rt_OneStep(void);
void rt_OneStep(void)
{
  static boolean_T OverrunFlag = false;

  // Disable interrupts here

  // Check for overrun
  if (OverrunFlag) {
    rtmSetErrorStatus(rtObj.getRTM(), "Overrun");
    return;
  }

  OverrunFlag = true;

  // Save FPU context here (if necessary)
  // Re-enable timer or interrupt here
  // Set model inputs here

  // Step the model
  rtObj.step();

  // Get model outputs here

  // Indicate task complete
  OverrunFlag = false;

  // Disable interrupts here
  // Restore FPU context here (if necessary)
  // Enable interrupts here
}
/*
#define AK8963_ADDRESS   0x0C
#define WHO_AM_I_AK8963  0x00 // should return 0x48
#define INFO             0x01
#define AK8963_ST1       0x02  // data ready status bit 0
#define AK8963_XOUT_L    0x03  // data
#define AK8963_XOUT_H    0x04
#define AK8963_YOUT_L    0x05
#define AK8963_YOUT_H    0x06
#define AK8963_ZOUT_L    0x07
#define AK8963_ZOUT_H    0x08
#define AK8963_ST2       0x09  // Data overflow bit 3 and data read error status bit 2
#define AK8963_CNTL      0x0A  // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
#define AK8963_ASTC      0x0C  // Self test control
#define AK8963_I2CDIS    0x0F  // I2C disable
#define AK8963_ASAX      0x10  // Fuse ROM x-axis sensitivity adjustment value
#define AK8963_ASAY      0x11  // Fuse ROM y-axis sensitivity adjustment value
#define AK8963_ASAZ      0x12  // Fuse ROM z-axis sensitivity adjustment value
*/
#define USER_CTRL        0x6A;
#define I2C_MST_EN       0x20;

#define MPU6050_ADDR         0xD0
#define MPU6050_SMPLRT_DIV   0x19
#define MPU6050_CONFIG       0x1a
#define MPU6050_GYRO_CONFIG  0x1b
#define MPU6050_ACCEL_CONFIG 0x1c
#define MPU6050_WHO_AM_I     0x75
#define MPU6050_PWR_MGMT_1   0x6b
#define ACCEL_XOUT_H_REG 0x3B
#define Alfa_comp 0.96

    double  Ax, Ay, Az, Gx, Gy, Gz,Mx,My,Mz;
    
    double   H_senx,H_seny,H_senz;
    char   name[1];
     
    int16_t Mag_X_RAW = 0;
    int16_t Mag_Y_RAW = 0;
    int16_t Mag_Z_RAW = 0;

    double avgx,avgy,avgz;
    double rate,bias=0; 
    double angle;
    
    double pitch_acc=0, roll_acc=0;
    double pitch_gyro=0, roll_gyro=0;
    double pitch=0, roll=0,yaw=0;
    double dtx,dtx1,dtx2,dt=0,dt1,dt2;
    //global variables for meaured pwm values
    uint16_t Rip_chnl_n[6],Rip_chnl_p[6];
    
    int distx,disty,distz;
    int distxo,distyo,distzo;
    int velocityx,velocityy,velocityz;
    
//////////////////////To measure pwm using interrupts and timer///////////////////////////////
InterruptIn ch1(PG_2);
InterruptIn ch2(PG_3);
InterruptIn ch3(A4);
InterruptIn ch4(A5);
InterruptIn ch5(PG_0);
InterruptIn ch6(PG_1);
///////////////////////////////////////////////////////////////////////////////////////////
//////////////////// pwm out for ESC inputs////////////////////////////////////////////////
PwmOut u1(PE_9);
PwmOut u2(PE_11);
PwmOut u3(PE_12);
PwmOut u4(PE_14);
///////////////////////////I2C/////////////////////////////////////////////////////////////
I2C i2c(PF_15,PF_14);
//////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////LIDAR serial ports/////////////////////////////////////////
RawSerial rawz(PD_5,PD_6);
RawSerial rawx(PC_10,PC_11);
RawSerial rawy(PC_12,PD_2); 
//////////////////////////////////////////////////////////////////////////////////////////
//////////blue light indicator for error in i2c///////////////////////////////////////////
DigitalOut myled(LED2); 
//////////////////////////////////////////////////////////////////////////////////////////
Serial pc(SERIAL_TX, SERIAL_RX);

Timeout onestep;

Timer timer1,timer2;
////////////////////////////////function initializations/////////////////////////////////


int main()
{  
 void stop_counter();
void start_counter();
void counters_init();

void dist_x();
void dist_y();
void dist_z();

void mpu_init();
void mpu_read();
void get_angle();
void mpu_calibrate();
void Mag_init();
void Mag_read();

onestep.attach_us(&rt_OneStep,100);
    counters_init();
    
    pc.baud(115200);
    
    rawx.baud(115200);
    rawy.baud(115200);
    rawz.baud(115200);
    
    rawx.attach(&dist_x);
    rawy.attach(&dist_y);
    rawz.attach(&dist_z);
    
    mpu_init();
    //Mag_init();  
    /*AK8963 mag(&i2c, AK8963::SLAVE_ADDR_1);
    if(mag.checkConnection() != AK8963::SUCCESS){
        pc.printf("check connection");
        
        }
    if(mag.setOperationMode(AK8963::MODE_CONTINUOUS_1) != AK8963::SUCCESS){
        while(1){pc.printf("failed continious mode");}
        }*/
    timer1.start(); //////////////timer for channel  
    timer2.start(); /////////////timer for dt to calculate dt
   
    //mpu_calibrate();
    
    
    u1.period_us(20000);
    u2.period_us(20000);
    u3.period_us(20000);
    u4.period_us(20000);
      
    
    while(timer2.read_ms()<5000)
    {    u1.write((float)rtObj.getu1_());
        u2.write((float)rtObj.getu2_());
        u3.write((float)rtObj.getu3 ());
        u4.write((float)rtObj.getu4 ());
        
        }
     
    


    while (true) 
    {   int i;
        //int statusAK8963= AK8963::NOT_DATA_READY;
    
         
        get_angle();
        
       /* if(statusAK8963 == AK8963::DATA_READY){
            AK8963::MagneticVector mag1;
            mag.getMagneticVector(&mag1);
            pc.printf("%5.1f,%5.1f,%5.1f\n",mag1.mx,mag1.my,mag1.mz);
            statusAK8963 = AK8963::NOT_DATA_READY;
            } else if (statusAK8963 == AK8963::NOT_DATA_READY){
                }
        
        
        for(int i=1000;i<2000;i++){
         u1.write((float)i/20000.0);
        u2.write((float)i/20000.0);
        u3.write((float)i/20000.0);
        u4.write((float)i/20000.0);
        wait(0.0001);}
        for(int i=2000;i>1000;i--){
         u1.write((float)i/20000.0);
        u2.write((float)i/20000.0);
        u3.write((float)i/20000.0);
        u4.write((float)i/20000.0);
        wait(0.0001);}*/
       // Mag_read();
       //for(i=0;i<6;i++)
        //pc.printf("%d\t",Rip_chnl_n[i]);
      // pc.printf("\n");
     /*  pc.printf("pitch =  %f\t",pitch);
       pc.printf("roll =  %f\t",roll);
       pc.printf("yaw =  %f\t",yaw);
       */
       pc.printf("x =  %d\t",distx);
       pc.printf("y =  %d\t",disty);
       pc.printf("z =  %d\n",distz);
       
       
       
       //pc.printf("magx =  %d\t",name[0]);
       //pc.printf("raw =  %d\t",Mag_X_RAW);
       //pc.printf("mx =  %f\t",Mx);
       //wait(0.01);
      // 
       }
}


///////////////////////////////////////////////////////////////functions//////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////********************************functions for interrupts*****************************////////////////
void start_counter_1()
{
     Rip_chnl_p[0]=timer1.read_us();
    }
void start_counter_2(){
    Rip_chnl_p[1]=timer1.read_us();
    }
void start_counter_3(){
    Rip_chnl_p[2]=timer1.read_us();
    }
void start_counter_4(){
   Rip_chnl_p[3]=timer1.read_us();
    }
void start_counter_5(){
    Rip_chnl_p[4]=timer1.read_us();
    }
void start_counter_6(){
    Rip_chnl_p[5]=timer1.read_us();
    }
void stop_counter_1(){
   uint32_t x;
    x=timer1.read_us();
   Rip_chnl_n[0]=x-Rip_chnl_p[0];
    }
void stop_counter_2(){
    uint32_t x;
    x=timer1.read_us();
   Rip_chnl_n[1]=x-Rip_chnl_p[1];
    }
void stop_counter_3(){
   uint32_t x;
    x=timer1.read_us();
   Rip_chnl_n[2]=x-Rip_chnl_p[2];
    }
void stop_counter_4(){
    uint32_t x;
    x=timer1.read_us();
   Rip_chnl_n[3]=x-Rip_chnl_p[3];
    }
void stop_counter_5(){
    uint32_t x;
    x=timer1.read_us();
   Rip_chnl_n[4]=x-Rip_chnl_p[4];
    }
void stop_counter_6(){
    uint32_t x;
    x=timer1.read_us();
   Rip_chnl_n[5]=x-Rip_chnl_p[5];
    }

void counters_init(){
    ch1.rise(&start_counter_1);
    ch2.rise(&start_counter_2);
    ch3.rise(&start_counter_3);
    ch4.rise(&start_counter_4);
    ch5.rise(&start_counter_5);
    ch6.rise(&start_counter_6);
    ch1.fall(&stop_counter_1);
    ch2.fall(&stop_counter_2);
    ch3.fall(&stop_counter_3);
    ch4.fall(&stop_counter_4);
    ch5.fall(&stop_counter_5);
    ch6.fall(&stop_counter_6);
    }
    

//////////////////////////////***********************************LIDAR********************************************////////////////
void dist_x() ///////////*******use this function to get distance it has object of rawserial as argument*******////////////
    { //int distance;//actual distance measurements of LiDAR
    int strength;//signal strength of LiDAR
    int check;//save check value
    int i;
    int uartx[9];//save data measured by LiDAR
    
        if (rawx.readable())//check if serial port has data input
    {       
        if(rawx.getc()==0x59)//assess data package frame header 0x59
        {
            uartx[0]=0x59;
            if(rawx.getc()==0x59)//assess data package frame header 0x59
                {
                    uartx[1]=0x59;
                    for(i=2;i<9;i++)//save data in array
                        {
                        uartx[i]=rawx.getc();
                        }
                    check=uartx[0]+uartx[1]+uartx[2]+uartx[3]+uartx[4]+uartx[5]+uartx[6]+uartx[7];
                    if(uartx[8]==(check&0xff))//verify the received data as per protocol
                        {
                            
                            
                        strength=uartx[4]+uartx[5]*256;//calculate signal strength value
                        distx=uartx[2]+uartx[3]*256;//calculate distance value in cm
                        //velocityx = distx - distxo;  ////velocity in meters
                          //distxo = distx;
                        }
                }
        }
    } 
    
  }
void dist_y() ///////////*******use this function to get distance it has object of rawserial as argument*******////////////
    { 
    int strength;//signal strength of LiDAR
    int check;//save check value
    int i;
    int uarty[9];//save data measured by LiDAR
    
        if (rawy.readable())//check if serial port has data input
    {
        if(rawy.getc()==0x59)//assess data package frame header 0x59
        {
            uarty[0]=0x59;
            if(rawy.getc()==0x59)//assess data package frame header 0x59
                {
                    uarty[1]=0x59;
                    for(i=2;i<9;i++)//save data in array
                        {
                        uarty[i]=rawy.getc();
                        }
                    check=uarty[0]+uarty[1]+uarty[2]+uarty[3]+uarty[4]+uarty[5]+uarty[6]+uarty[7];
                    if(uarty[8]==(check&0xff))//verify the received data as per protocol
                        {
                            
                        strength=uarty[4]+uarty[5]*256;//calculate signal strength value
                        disty=uarty[2]+uarty[3]*256;//calculate distance value
                         //velocityy = disty - distyo;  ////velocity in meters
                          //distyo = disty;
                        }
                        
                }
        }
    } 
    
  }
  void dist_z() ///////////*******use this function to get distance it has object of rawserial as argument*******////////////
    { 
    int strength;//signal strength of LiDAR
    int check;//save check value
    int i;
    int uartz[9];//save data measured by LiDAR
    
        if (rawz.readable())//check if serial port has data input
    {
        if(rawz.getc()==0x59)//assess data package frame header 0x59
        {
            uartz[0]=0x59;
            if(rawz.getc()==0x59)//assess data package frame header 0x59
                {
                    uartz[1]=0x59;
                    for(i=2;i<9;i++)//save data in array
                        {
                        uartz[i]=rawz.getc();
                        }
                    check=uartz[0]+uartz[1]+uartz[2]+uartz[3]+uartz[4]+uartz[5]+uartz[6]+uartz[7];
                    if(uartz[8]==(check&0xff))//verify the received data as per protocol
                        { 
                        strength=uartz[4]+uartz[5]*256;//calculate signal strength value
                        distz=uartz[2]+uartz[3]*256;//calculate distance value
                         //velocityz = distz - distzo;  ////velocity in meters
                          //distzo = distz;
                        }
                }
        }
    } 
    
  }
  //////////////////////////////////**********velocity**************//////////////////////////////////////
 

/////////////////////////////////***********************mpu****************************///////////////////////////////

 void mpu_init()
 {
     i2c.frequency(100000);
    char data_write[2];
    
    data_write[0] = MPU6050_PWR_MGMT_1;
    data_write[1] = 0x00;
    
    int status = i2c.write(MPU6050_ADDR, data_write, 2, 0);
    
    if (status != 0)
     { // Error
             while (1)
          {
            myled = !myled;
            wait(0.2);
           }
    }
    data_write[0] = USER_CTRL;
    data_write[1] = I2C_MST_EN;
    i2c.write(MPU6050_ADDR, data_write, 2, 0);
    
     data_write[0] = 0x37;//INT_PIN_CFG;
    data_write[1] = 0x02;
    i2c.write(MPU6050_ADDR, data_write, 2, 0);
    
    data_write[0] = MPU6050_SMPLRT_DIV;
    data_write[1] = 0x07;
    i2c.write(MPU6050_ADDR, data_write, 2, 0);
    
    data_write[0] = MPU6050_ACCEL_CONFIG;
    data_write[1] = 0x00;
    i2c.write(MPU6050_ADDR, data_write, 2, 0);
    data_write[0] = 0x1d;
    data_write[1] = 0x06;
    i2c.write(MPU6050_ADDR, data_write, 2, 0);
    data_write[0] = MPU6050_CONFIG;
    data_write[1] = 0x00;
    i2c.write(MPU6050_ADDR, data_write, 2, 0);
    
    data_write[0] = MPU6050_GYRO_CONFIG;
    data_write[1] = 0x00;
    i2c.write(MPU6050_ADDR, data_write, 2, 0);
    wait(0.2);
 }

void mpu_read(){
    char Rec_Data[14];
    int16_t Accel_X_RAW = 0;
    int16_t Accel_Y_RAW = 0;
    int16_t Accel_Z_RAW = 0;
    int16_t Temp_raw = 0;
    int16_t Gyro_X_RAW = 0;
    int16_t Gyro_Y_RAW = 0;
    int16_t Gyro_Z_RAW = 0;
    char data_write[1];
    
    // Read 1 BYTES of data starting from ACCEL_XOUT_H register
    data_write[0] = ACCEL_XOUT_H_REG;
        i2c.write(MPU6050_ADDR, data_write, 1, 1); // no stop
        i2c.read(MPU6050_ADDR, Rec_Data, 14);
    Accel_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data [1]);
    Accel_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data [3]);
    Accel_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data [5]);
    
    Temp_raw = (int16_t)(Rec_Data[6] << 8 | Rec_Data [7]);
    
    Gyro_X_RAW = (int16_t)(Rec_Data[8] << 8 | Rec_Data [9]);
    Gyro_Y_RAW = (int16_t)(Rec_Data[10] << 8 | Rec_Data [11]);
    Gyro_Z_RAW = (int16_t)(Rec_Data[12] << 8 | Rec_Data [13]);
    /*** convert the RAW values into acceleration in 'g'
         we have to divide according to the Full scale value set in FS_SEL
         I have configured FS_SEL = 0. So I am dividing by 16384.0
         for more details check ACCEL_CONFIG Register              ****/

    Ax = Accel_X_RAW/16384.0;
    Ay = Accel_Y_RAW/16384.0;
    Az = Accel_Z_RAW/16384.0;
    
    Gx = Gyro_X_RAW/131.0;
    Gy = Gyro_Y_RAW/131.0;
    Gz = Gyro_Z_RAW/131.0;
    
    }
   
     
 void get_angle(){
    
      mpu_read();
        dt2=timer2.read_ms();                 ////////****very imp self note check the order of dt1 and dt2*********/////////// 
        dt=dt2-dt1;
        dt=dt/1000;
        
            roll_acc = (atan2((Ay),sqrt(pow((Ax),2) + pow((Az),2))))*(90*7/11);
            pitch_acc = ((atan2((-Ax),sqrt(pow((Ay),2) + pow((Az),2))))*(90*7/11));
       
              /* COMPLIMENTARY FILTER*/
            pitch = Alfa_comp*(pitch + Gy*dt) + (1-Alfa_comp)*pitch_acc ;
            
            roll = Alfa_comp*(roll + Gx*dt) + (1-Alfa_comp)*roll_acc ;
            if(abs(Gz)>1)
           yaw += Gz*dt;
       dt1=timer2.read_ms();
         
         }
 void mpu_calibrate(void){
        int i;
        for(i=0;i<=3000;i++)
    {
        //dt2=timer2.read_ms();
        if(i>0){
        get_angle();}
       
       // dt1=timer2.read_ms();
        //dt=dt1-dt2;
        
        avgx+=roll;
        avgy+=pitch;
        //avgz+=yaw;
     }
        avgx=avgx/3000;
        avgy=avgy/3000;
       // avgz=avgz/2000;
        }
 /*void Mag_init(){
     char raw[3];
     char data_write[2];

///////////////////////////////////////// Power down magnetometer///////////////////////////////////////////////////////  
    data_write[0] = AK8963_CNTL;
    data_write[1] = 0x00;
    
      int status = i2c.write(AK8963_ADDRESS, data_write, 2, 0);
       if (status != 0)
     { // Error
             while (1)
          {
            myled = !myled;
            wait(0.2);
           }
    }
    
    wait(0.01);
    data_write[0] = 0x6B;                   //////////////////rest magnetometer
    data_write[1] = 0x80;
    
     i2c.write(MPU6050_ADDR, data_write, 2, 0);
    
    
    wait(0.01);
///////////////////////////////////////// Enter Fuse ROM access mode///////////////////////////////////////////////////  
  data_write[0] = AK8963_CNTL;
    data_write[1] = 0x0F;
    i2c.write(AK8963_ADDRESS, data_write, 2, 0);
  
   wait(0.01);
///////////////////////////////////////// Read the x-, y-, and z-axis calibration values//////////////////////////////   
   data_write[0] = AK8963_ASAX;
    i2c.write(AK8963_ADDRESS, data_write, 1, 0);
    i2c.read(AK8963_ADDRESS, raw, 3);
// Return x-axis sensitivity adjustment values, etc.    
  H_senx =  (float)(raw[0] - 128)/256.0f + 1.0f;   
  H_seny =  (float)(raw[1] - 128)/256.0f + 1.0f;  
  H_senz =  (float)(raw[2] - 128)/256.0f + 1.0f; 
   
    data_write[0] = AK8963_CNTL;
    data_write[1] = 0x16;
    i2c.write(AK8963_ADDRESS, data_write, 2, 0);
    wait(0.01);
    data_write[0] = 0x0B;
    data_write[1] = 0x01;
    i2c.write(AK8963_ADDRESS, data_write, 2, 0);
    wait(0.01);
  // Configure the magnetometer for continuous read and highest resolution
  // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
  // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
 
     }
void Mag_read(){
    
    char data_write[1];
    char x[1];
    char rawData[7]; 
 /////////////read name///////////////////////////////////     
data_write[0] = WHO_AM_I_AK8963;
    i2c.write(AK8963_ADDRESS, data_write, 1, 0);
   i2c.read(AK8963_ADDRESS, name, 1);
   
    //////////////////////////////////////
    data_write[0] = AK8963_ST1;
    i2c.write(AK8963_ADDRESS, data_write, 1, 0);
    i2c.read(AK8963_ADDRESS, x, 1);
    
   if(x[0] & 0x01) { // wait for magnetometer data ready bit to be set
    data_write[0] = AK8963_XOUT_L;// Read the six raw data and ST2 registers sequentially into data array
    i2c.write(AK8963_ADDRESS, data_write, 1, 1);
    i2c.read(AK8963_ADDRESS, rawData, 7);
 
  uint8_t c = rawData[6]; // End data read by reading ST2 register
    if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
    Mag_X_RAW = (int16_t)((rawData[1] << 8) | rawData[0]);  // Turn the MSB and LSB into a signed 16-bit value
    Mag_Y_RAW = (int16_t)((rawData[3] << 8) | rawData[2]) ;  // Data stored as little Endian
    Mag_Z_RAW = (int16_t)((rawData[5] << 8) | rawData[4]) ; 
                   }
          }
    Mx = Mag_X_RAW*H_senx;
    My = Mag_Y_RAW*H_seny;
    Mz = Mag_Z_RAW*H_senz;
 }*/