File content as of revision 3:9244869157bd:

//3D Scanner
//2017 Angelo Mottolese mottoleseangelo@gmail.com
#include "mbed.h"
#include "OV7670.h"
#include "ST7735_TFT.h"
#include "ADXL345_I2C.h"
#include "HMC5843.h"
#include "SDFileSystem.h"
#include "Servo.h"

#define SIZEX 320
#define SIZEY 240
#define SIZE SIZEX*SIZEY
#define LABVIEW_MODE 1


#define TRANSFER_SIZE   4
#define N_VAR 14

//LocalFileSystem local("local");               // Create the local filesystem under the name "local"


OV7670 cam(
    p28,p27,       // SDA,SCL(I2C / SCCB)
    p23,NC,p25,   // VSYNC,HREF,WEN(FIFO)
    Port0,0x07878000,//PortIn(p18,p17,p16,p15,p11,p12,p14,p13) is D7-D0
    p26,p29,p30) ; // RRST,OE,RCLK
ST7735_TFT TFT(p5, p6, p7, p8, p9, p10,"TFT"); // mosi, miso, sclk, cs, rs, reset
// SDA, nc, SCK,CS,A0,RESET = mosi, miso, sclk, cs, rs, reset

HMC5843 compass(p28, p27);
ADXL345_I2C accelerometer(p28, p27);
//SDFileSystem sd(p5, p6, p7, p21, "sd"); // the pinout on the mbed Cool Components workshop board
//nRF24L01P my_nrf24l01p(p5, p6, p7, p24, p21, p20);    // mosi, miso, sck, csn, ce, irq
Servo yaw(p21);
Serial pc(USBTX,USBRX);
Timer tempo;
AnalogIn pot(p20);

float Complementary(float newAngle, int looptime);
float Complementary2(float newAngle, int looptime);
double kalman_update(int index,double measurement);
float kalmanCalculate(float newAngle, int looptime);
void accMagInit(void);
void ahrs(void);
void kalman_init(void);
void showImage(void);
void image_minmax(void);
void processImage(void);
void sdInit(void);
void nrfpInit(void);
void countdown(void);

extern "C" void mbed_reset();

double q[N_VAR]; //process noise covariance
double rk[N_VAR]; //measurement noise covariance
double value[N_VAR]; //value
double p[N_VAR]; //estimation error covariance
double k[N_VAR]; //kalman gain

int i=0, loopN=0, l=0;
float mv=0.0,servoHead=0.0;

int thr=5;

float xrot=0.0,yrot=0.0,zrot=0.0, vx=0.0,vy=0.0,vz=0.0;
int ax=0,ay=0,az=0,ax0,ay0,az0;
int mx=0,my=0,mz=0,mx0,my0,mz0;
double xh=0.0, yh=0.0, head=0.0;
double ca=0.0, cb=0.0, cc=0.0;
double sa=0.0, sb=0.0,sc=0.0;
    double fx,fy,fz;
uint16_t thri=3,thrr=3,thrg=4;

uint16_t max_red,max_blue, max_green,min_blue,min_green,min_red;
uint16_t colour=0,r,g,b;
int x=0,y=0, lineacc,cnt=0,x0[SIZEY];
uint16_t bank0,bank1;
int ok=0;
    uint16_t mr=0,mb=0,mg=0,id=0;

//  FILE *out=fopen("/local/world.txt","w");

int main()
{
    //  if ( out == NULL )   error("Could not open file.\r\n");
    //fprintf(out,"v 0.0 0.0 0.0\n");
    pc.baud(115200);
        yaw=0.0;
    //////////////////////Camera and TFT init
    TFT.set_orientation(3);
    //TFT.background(Black);
    TFT.cls();
    cam.Reset() ;
    cam.InitQQVGA(SIZEX) ;

    pc.baud(115200);
    kalman_init();
    accMagInit();
//nrfpInit();
//sdInit();

    /////////////////////////////////// Loop control////////////////////////////////////////////////////
    int j=0;

    for(j=1; j>0; j--) {
       if(LABVIEW_MODE==0) pc.printf("Posizionamento %d s\n", j);
        tempo.start();
        while(tempo.read()<0.5) showImage();
        tempo.stop();
        tempo.reset();
    }
    //countdown();

    image_minmax();

    // while(1) {
    //tempo.start();
    ahrs();
    ///////////Image Acquisition and Processing
    //      if((hd[0]+0.22)<hd[1] || (hd[0]-0.22)>hd[1]){   //Heading noise gate
    ok=0;
    
    //if(loopN%20==0) 
    TFT.cls();
    //Misura intensità max e min
    if(loopN%100==0 || loopN==0) image_minmax(); //Se il numero di pixel validi è piccolo

   // printf("    SCANNING.\n");
   while(1) {
   
        ahrs();
        image_minmax();
        processImage();
        loopN++;
            yaw=kalman_update(12,mv);
        servoHead=mv*1.5768863175;
           //   wait_ms(20);
    }
   /* for(mv=1.0; mv>0.0; mv-=0.01) {
        
        ahrs();
        image_minmax();
        processImage();
        loopN++;
            yaw=kalman_update(12,mv);
        servoHead=mv*1.5768863175;
        wait_ms(20);
    }
    yaw=kalman_update(12,0.0);*/
    //fclose(out);
  //  printf("            END\n");
 //   tempo.stop();
  //  printf("Time taken= %f seconds, OK=%d\n", tempo.read(),ok);
 //   tempo.reset();
    // }//Fine while(1)
    //     mbed_reset();
}


double kalman_update(int index,double measurement)
{
    //prediction update
    //omit x = x
    p[index]+= q[index];
    //measurement update
    k[index] = p[index] / (p[index] + rk[index]);
    value[index] += k[index] * (measurement - value[index]);
    p[index] *= (1 - k[index]);
    //fprintf(out,"%f %f \r\n",p,k);
    return value[index];
}

void kalman_init(void)
{
    for(l=0; l<N_VAR; l++) {
        q[l]=0.001; //process noise covariance
        rk[l]=0.1; //measurement noise covariance
        p[l]=0.0; //estimation error covariance
        k[l]=0.0; //kalman gain
        value[l]=0.0;
    }
}

///////////////////////////////////////////SD Card init
void sdInit(void)
{
    /*
       mkdir("/sd/", 0777);
       FILE *fp = fopen("/sd/worldSD.obj", "w");
       if(fp == NULL) error("Could not open file for write\n");
       fprintf(fp, "World mapping");
       fclose(fp);
    */
}

////////////////////////////AHRS//////////////////
void accMagInit(void)
{

    int breadings[3] = {0, 0, 0};
    int bhmcreadings[3];
    char buffer[3];
   // if(LABVIEW_MODE==0) pc.printf("Starting ADXL345 test...\n");
  //  if(LABVIEW_MODE==0) pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
    accelerometer.setPowerControl(0x00);
    accelerometer.setDataFormatControl(0x0B);
    accelerometer.setDataRate(ADXL345_3200HZ);
    accelerometer.setPowerControl(0x08);
    compass.getAddress(buffer);
  //  if(LABVIEW_MODE==0) pc.printf("Magnetic Compass  Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]);
    compass.setDefault();
    wait(.1);
    for(i=0; i<50; i++) {
        accelerometer.getOutput(breadings);
        //13-bit, sign extended values.
        compass.readData(bhmcreadings);
        wait(0.1);
        ax+=(int16_t)(breadings[0]);
        ay+=(int16_t)(breadings[1]);
        az+=(int16_t)(breadings[2]);
        mx+=(int16_t)(bhmcreadings[0]);
        my+=(int16_t)(bhmcreadings[1]);
        mz+=(int16_t)(bhmcreadings[2]);
    }
    ax0=ax/50;
    ay0=ay/50;
    az0=az/50;
    mx0=mx/50;
    my0=my/50;
    mz0=mz/50;
    ax=0;
    ay=0;
    az=0;
    mx=0;
    my=0;
    mz=0;
}

void ahrs(void)
{
    cnt=0;
    int accelero=0, deadr=0;

    if(accelero==1) {

float x1=0.0,y1=0.0,z1=0.0,x2=0.0,y2=0.0,z2=0.0,xd=0.0,yd=0.0,zd=0.0;
float aax=0.0,aay=0.0,aaz=0.0;
        int readings[3] = {0, 0, 0};
        int hmcreadings[3];
        accelerometer.getOutput(readings);
        compass.readData(hmcreadings);
        //13-bit, sign extended values.
        cnt++;
        ax+=(int16_t)(readings[0]);
        ay+=(int16_t)(readings[1]);
        az+=(int16_t)(readings[2]);
        ax-=ax0;
        ay-=ay0;
        az-=az0;
        mx+=(int16_t)(hmcreadings[0]);
        my+=(int16_t)(hmcreadings[1]);
        mz+=(int16_t)(hmcreadings[2]);
        mx-=mx0;
        my-=my0;
        mz-=mz0;
        wait(0.1);

        xd=(double)(ax);
        yd=(double)(ay);
        zd=(double)(az);
        aax=kalman_update(9,xd); //Kalman to accelerometer
        aay=kalman_update(10,yd);
        aaz=kalman_update(11,-zd);

        if(deadr==1) {
            x1+=kalman_update(3,aax);
            y1+= kalman_update(4,aay);
            z1+= kalman_update(5,aaz);

            if(aax<=1.6420 && aax>=-1.6420 ) x1=0.0;
            if(aay<=1.2315 && aay>=-1.1325 ) y1=0.0;
            if(aaz<=8.374 && aaz>=-8.374 ) z1=0.0;
            x2+=kalman_update(6,x1);
            y2+=kalman_update(7,y1);
            z2+=kalman_update(8,z1);
        }
        double xAngle=-0.00609*aay;
        double yAngle=0.00609*aax;

        double mmx=(double)(mx);
        double mmy=(double)(my);
        double mmz=(double)(mz);

        fx=kalman_update(0,xAngle);
        fy=kalman_update(1,yAngle);

        ca = cos(fx);//theta
        cb = cos(fy);//phi
        sa = sin(fx);
        sb = sin(fy);

        double yh=mmz*sb-mmy*cb;
        double xh=+mmx*ca+mmy*sa*sb+mmz*sa*cb;
        double head=atan2(yh,xh);
        fz=kalman_update(2,head);
    }


    //if(accelero==0) {
    fz=servoHead;
fz=pot.read();
        fz-=0.1;
        fz*=6.2831168863175;
        fz=3.14-fz;
        
    cc = cos(fz);
    sc = sin(fz);
    //if(LABVIEW_MODE==0) pc.printf("%f %f %f %f %f %f %f %f %f\n",fx,fy,fz,x2,y2,z2,kalman_update(9,x2),kalman_update(10,y2),kalman_update(11,z2));
    cnt=0;
    ax=0;
    ay=0;
    az=0;


}

/////////////////////CAMERA FUNCTIONS/////////////////////
void image_minmax(void)
{
    for(id=0;id<SIZEY;id++) x0[id]=0;
    max_red=0;max_blue=0; max_green=0;min_blue=200;min_green=200;min_red=200;
    
    cam.CaptureNext() ;
    while (cam.CaptureDone()==0) ;
    cam.ReadStart();
    
    for (y = 0; y < SIZEY; y++) {
        for ( x= 0; x < SIZEX; x++) {
            bank0 =  cam.ReadOneByte();
            bank1 =  cam.ReadOneByte();
            r=((bank1 & 0x1f)) ;
           g= (((bank0<<3)&0x38 )|((bank1>>5)&0x7)) & 0x3f;
            b=((bank0 & 0xf8)>>3 );

           /* if(b>max_blue) {
                max_blue=b;
                if(max_green<g) max_green=g;
                if(max_red<r) max_red=r;
                if(min_blue>b) min_blue=b;
                if(min_green>g) min_green=g;
                if(min_red>r) min_red=r;
                mr+=r;
                mb+=b;
                mg+=g;
                id++;
            }*/    
            if(r>max_red) {
                max_red=r;
                if(max_green<g) max_green=g;
                if(max_blue<b) max_blue=b;
                if(min_blue>b) min_blue=b;
                if(min_green>g) min_green=g;
                if(min_red>r) min_red=r;
                mr+=r;
                mb+=b;
                mg+=g;
                id++;
            }
        }
    }
    cam.ReadStop() ;
}



void processImage(void)
{
    cnt=0;
    double xrot,yrot,zrot;
    cam.CaptureNext() ;
    while (cam.CaptureDone()==0) ;
    cam.ReadStart();
    for (y = 0; y < SIZEY; y++) {
        for ( x= 0; x < SIZEX; x++) {
            bank0 =  cam.ReadOneByte();
            bank1 =  cam.ReadOneByte();
            r=((bank1 & 0x1f)) ;
           g= (((bank0<<3)&0x38 )|((bank1>>5)&0x7)) & 0x3f;
            b=((bank0 & 0xf8)>>3 );
           //if(b>=(mb-thri) && r<=(mr+thrr) )  {
        if(r>=(max_red-thrr) && b>=(max_blue-thri))  {
                lineacc+=x;
                cnt++;
            }
        }
        if(cnt>1) {
            x0[y]=lineacc/cnt;
        
            ok++;
        }
        lineacc=0;
        cnt=0;
    }
    cam.ReadStop() ;
    
    for(y=0;y<SIZEY;y++){
            vy=(double)(SIZEX)-(double)x0[y];//vy*sc
        vx=(double)(SIZEX)-(double)(SIZEX)*cc;//vy*cc
        vx=vy*cc;//vy*cc
        yrot=vy*sc;
        zrot=y*0.6;
            TFT.pixel(x0[y],y,x0[y]*10);
            
//           if(LABVIEW_MODE==0)  pc.printf("v %f. %f. %f\n",xrot,yrot,zrot);
           if(LABVIEW_MODE==0)  pc.printf("%f %f %f %f ",vx,vy,vz,fz);
            if(LABVIEW_MODE==1) pc.printf("%.3f,%.3f,%.3f\n",vx,yrot,zrot);
        }
}


void showImage(void)
{
    cam.CaptureNext() ;
    while (cam.CaptureDone()==0) ;
    cam.ReadStart();
    for (y = 0; y < SIZEY; y++) {
        for ( x= 0; x < SIZEX; x++) {
            bank0 =  cam.ReadOneByte();
            bank1 =  cam.ReadOneByte();
            r=((bank1 & 0x1f)) ;
           g= (((bank0<<3)&0x38 )|((bank1>>5)&0x7)) & 0x3f;
            b=((bank0 & 0xf8)>>3 );
            colour = (((r<<11)&0xF800)|((g<<5)&0x7E0)| (b&0x1f));
            TFT.pixel(x,y,colour);
        }
    }
    cam.ReadStop() ;
}

void countdown(void)
{
    int i;
    for(i=3; i>0; i--) {
        showImage();
        wait(1);
        if(LABVIEW_MODE==0) pc.printf("Acquisisco fra %d s\n",i);
    }
}