File content as of revision 0:7fa3463671dd:

#include "mbed.h"
#include "BMP180.h"
#include "Si7021.h"
#include "TSL2561.h"
#include "MPU9250.h"
#include "INA219.hpp"
#include "JPEGCamera.h"
#include "stdio.h"
#include "stdlib.h"
#include "math.h"
#include "ST7735_TFT.h"
#include "string"
#include "Arial12x12.h"
#include "Arial24x23.h"
#include "Arial28x28.h"
#include "SDFileSystem.h" 
 
  #define NUMBER_OF_STARS 400
 #define SCREEN_WIDTH 200
 #define SCREEN_HEIGHT 250
 
 /*star struct*/
typedef struct 
{
  float xpos, ypos;
  short zpos, speed;
  unsigned int color;
} STAR;

static STAR stars[NUMBER_OF_STARS];


void init_star(STAR* star, int id)
{
  /* randomly init stars, generate them around the center of the screen */
  
  star->xpos =  -10.0 + (20.0 * (rand()/(RAND_MAX+1.0)));
  star->ypos =  -10.0 + (20.0 * (rand()/(RAND_MAX+1.0)));
  
  star->xpos *= 3072.0; /*change viewpoint */
  star->ypos *= 3072.0;

  star->zpos =  id;
  star->speed =  2 + (int)(2.0 * (rand()/(RAND_MAX+1.0)));

  star->color = id*Cyan >> 2; /*the closer to the viewer the brighter*/
}


void init()
{
  int id;

  for (id = 0; id < NUMBER_OF_STARS; id++)
    {
      init_star(stars + id, id + 1);
    }
}
 
  
 // the TFT is connected to SPI pin 5-7, CS is p8, RS is p11, reset is p15 
 ST7735_TFT TFT(p5, p6, p7, p12, p11, p15,"TFT"); // mosi, miso, sclk, cs, rs, reset
 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
 #define archivod "/sd/datos/dato%05d.txt";

int fru=0;
Timer td;
 
//Serial BT(p13, p14); // tx, rx

int i=0;

I2C myI2C(p9,p10);
Serial pc(USBTX,USBRX);

BMP180 bmp180(&myI2C); //Barometro
Si7021 sensor(p9,p10); //Sensor de humedad
TSL2561 tsl2561(p9,p10); //Sensor de luz
MPU9250 mpu9250; //Giroscopio
INA219 ina219(p9, p10, 0x41, 100000, RES_10BITS);

I2C RGBH(p9, p10); //RGB
DigitalOut green(p11);
int sensor_addr = 41 << 1;
   
Timer t;//Giroscopio
Timer setuptimer; //Luz
Timer executetimer;//Luz

//Barometro
int press;
float temp;
//Giroscopio
float sum = 0;
uint32_t sumCount = 0;
char buffer[14];

void setup(void){ //Luz y Giroscopio

    if (tsl2561.begin()) pc.printf("TSL2561 Sensor Found\n\r");         
    else  pc.printf("TSL2561 Sensor not Found\n\r");   
    
    tsl2561.setGain(TSL2561_GAIN_0X);         // set no gain (for bright situtations) 
    tsl2561.setTiming(TSL2561_INTEGRATIONTIME_402MS);  // longest integration time (dim tsl2561)
    
    uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250 
    if (whoami == 0x71) pc.printf("MPU9250 is online...\n\r");
    mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
    mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
    mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
    wait(2);  
    mpu9250.initMPU9250();
    mpu9250.initAK8963(magCalibration);
    wait(1);
}


int main(){
    
    JPEGCamera camera(p13, p14); // TX, RX
    LocalFileSystem local("local"); //save images on mbed
    Timer timer;
    timer.start();
    camera.setPictureSize(JPEGCamera::SIZE320x240);
    
    //BT.baud(38400);
    //Sensor de luz  
    setuptimer.start();
    setup();    
    setuptimer.stop();
    setuptimer.reset();
    uint16_t x,y,z;
    t.start();
    
    //RGB
    RGBH.frequency(200000);
    green = 1; // off
    
    char id_regval[1] = {146};
    char data[1] = {0};
    RGBH.write(sensor_addr,id_regval,1, true);
    RGBH.read(sensor_addr,data,1,false);
    
    if (data[0]==68) {
        green = 0;
        wait (2); 
        green = 1;
        } else {
        green = 1; 
    }
    
    // Initialize color sensor
    
    char timing_register[2] = {129,0};
    RGBH.write(sensor_addr,timing_register,2,false);
    
    char control_register[2] = {143,0};
    RGBH.write(sensor_addr,control_register,2,false);
    
    char enable_register[2] = {128,3};
    RGBH.write(sensor_addr,enable_register,2,false);
    
    mkdir("/sd/datos", 0777);
    
    unsigned int centerx, centery;
    int id, j, tempx, tempy;  
    init();
    TFT.set_orientation(2);
    centerx = TFT.width() >> 1;
    centery = TFT.height() >> 1; 
    
     
    TFT.claim(stdout);      // send stdout to the TFT display 
    //TFT.claim(stderr);      // send stderr to the TFT display

    TFT.background(Black);    // set background to black
    TFT.foreground(White);    // set chars to white
    
    TFT.cls();
    TFT.set_font((unsigned char*) Arial12x12);  // select the font
        
    td.start();

    ////// demo start
    
    for ( j = 0 ; j < 5000; j++ ) 
    {
     
      /* move and draw stars */
       
      for (id = 0; i < NUMBER_OF_STARS; id++)
    {
      tempx = (stars[id].xpos / stars[id].zpos) + centerx;
      tempy = (stars[id].ypos / stars[id].zpos) + centery;
      TFT.pixel(tempx,tempy,Black);
      
        
      stars[id].zpos -= stars[id].speed;
      
      if (stars[id].zpos <= 0)
        {
          init_star(stars + id, id + 1);
        }

      //compute 3D position
      tempx = (stars[id].xpos / stars[id].zpos) + centerx;
      tempy = (stars[id].ypos / stars[id].zpos) + centery;

      if (tempx < 0 || tempx > TFT.width() - 1 || tempy < 0 || tempy > TFT.height() - 1) //check if a star leaves the screen
        {
          init_star(stars + id, id + 1);
          
          continue;
        }
      
      TFT.pixel(tempx,tempy,stars[id].color);
      
        
    }
     //TFT.Bitmap(centerx-60,centery-19,120,38,p1);
     
    printf("N y U %i\n", fru);
    /*fru++;
    pc.printf("%i", fru);
    char fil[32];
    sprintf(fil, "/sd/datos/dato%d.txt", fru); 
    FILE *fp = fopen(fil, "w");
    fprintf(fp,"Swag\n\r");
    pc.printf("Escribiendo");
    wait(0.3);
    fclose(fp);
    pc.printf("Cerrado");*/
    }
    
    

    ///// demo stop

    td.stop();
    TFT.locate(0,10);
    TFT.set_font((unsigned char*) Arial12x12);  // select the font
    printf("Time %f s\n", td.read());
    
    while(1)
    {   
        //Sensor de luz  
        executetimer.start();
        x = tsl2561.getLuminosity(TSL2561_VISIBLE);     
        y = tsl2561.getLuminosity(TSL2561_FULLSPECTRUM);
        z = tsl2561.getLuminosity(TSL2561_INFRARED);
        executetimer.stop();
        
        
        bmp180.init(); //Barometro
        sensor.measure();//Sensor de humedad
        
        //Barometro
        float altitud;
        bmp180.startTemperature(); 
        wait_ms(5);     // 
        if(bmp180.getTemperature(&temp) != 0) {
            printf("Error getting temperature\n");
            continue;
            }    
        bmp180.startPressure(BMP180::ULTRA_LOW_POWER);
        wait_ms(10);    // Wait for conversion to complete
        if(bmp180.getPressure(&press) != 0) {
            printf("Error getting pressure\n");
            continue;
            }
        altitud= 44330.0*(1-pow(press*.01/1020.0,(1/5.225)));   
        //Sensor de humedad    
        float temps=sensor.get_temperature()/1000.0;
        float hum=sensor.get_humidity()/1000.0;
        
        //Sensor de voltaje
        float current_ma = ina219.read_current_mA();
        float voltageb = ina219.read_bus_voltage();
        
        //Giroscopio
        mpu9250.getAres(); // Get accelerometer sensitivity
        mpu9250.getGres(); // Get gyro sensitivity
        mpu9250.getMres(); // Get magnetometer sensitivity
        magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
        magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
        magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
        if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
            mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
            // Now we'll calculate the accleration value into actual g's
            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];  
           
            mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
            // Calculate the gyro value into actual degrees per second
            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];   
          
            mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
            // Calculate the magnetometer values in milliGauss
            // Include factory calibration per data sheet and user environmental corrections
            mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
            my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
            mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
            }
        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++;
    
        mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
        tempCount = mpu9250.readTempData();  // Read the adc values
        temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
        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; 
        yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
        roll  *= 180.0f / PI;
        
        //RGB
        char clear_reg[1] = {148};
        char clear_data[2] = {0,0};
        RGBH.write(sensor_addr,clear_reg,1, true);
        RGBH.read(sensor_addr,clear_data,2, false);
        
        int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
        
        char red_reg[1] = {150};
        char red_data[2] = {0,0};
        RGBH.write(sensor_addr,red_reg,1, true);
        RGBH.read(sensor_addr,red_data,2, false);
        
        int red_value = ((int)red_data[1] << 8) | red_data[0];
        
        char green_reg[1] = {152};
        char green_data[2] = {0,0};
        RGBH.write(sensor_addr,green_reg,1, true);
        RGBH.read(sensor_addr,green_data,2, false);
        
        int green_value = ((int)green_data[1] << 8) | green_data[0];
        
        char blue_reg[1] = {154};
        char blue_data[2] = {0,0};
        RGBH.write(sensor_addr,blue_reg,1, true);
        RGBH.read(sensor_addr,blue_data,2, false);
    
        int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
        
        
        pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\r\n", clear_value, red_value, green_value, blue_value);
        pc.printf("P=%d Pa\n\rT=%.2fC\n\rAltitud= %.2f m\n\r", press, temp, altitud); //Barometro
        pc.printf("Temp: %0.2f\n\r",temps); //Humedad
        pc.printf("Humidity: %0.2f % \n\r",hum);//Humedad
        pc.printf("Visible: %d \n\r",x); //Luz
        pc.printf("Full Spectrum: %d\n\r",y); //Luz
        pc.printf("Infrared: %d\n\r",z);//Luz
        pc.printf(" temperature = %f  C\n\r", temperature);    //Giroscopio
        pc.printf("Giroscopio: %2.2f,%2.2f,%2.2f,%2.2f,%2.2f,%2.2f,Y:%3.1f,P:%3.1f,R:%3.1f\n\r",ax,ay,az,gx,gy,gz,yaw,pitch,roll);
        pc.printf("%f mA\r\n", current_ma);
        pc.printf("%f V\r\n", voltageb);
        
        /*
        BT.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\r\n", clear_value, red_value, green_value, blue_value);
        BT.printf("P=%d Pa\n\rT=%.2fC\n\r", press, temp); //Barometro
        BT.printf("Temp: %0.2f\n\r",temps); //Humedad
        BT.printf("Humidity: %0.2f % \n\r",hum);//Humedad
        BT.printf("Visible: %d \n\r",x); //Luz
        BT.printf("Full Spectrum: %d\n\r",y); //Luz
        BT.printf("Infrared: %d\n\r",z);//Luz
        BT.printf(" temperature = %f  C\n\r", temperature);    //Giroscopio
        BT.printf("Giroscopio: %2.2f,%2.2f,%2.2f,%2.2f,%2.2f,%2.2f,Y:%3.1f,P:%3.1f,R:%3.1f\n\r",ax,ay,az,gx,gy,gz,yaw,pitch,roll);
        BT.printf("%f mA\r\n", current_ma);
        BT.printf("%f V\r\n", voltageb);*/
        
        
        if (camera.isReady()) {
        char filename[32];
        sprintf(filename, "/local/pict%03d.jpg", i);
        //printf("Picture: %s ", filename);
            if (camera.takePicture(filename)) {
                while (camera.isProcessing()) {
                    camera.processPicture();}}}
        i++;
        pc.printf("Picture");

        wait(1);
         
 
 }}