Nilli Martínez / Mbed 2 deprecated F-CubeSatKit4

super

Dependencies:   BMP180 INA219 JPEGCamera SDFileSystem ST7735_TFT Si7021 TSL2561 mbed

main.cpp@0:7fa3463671dd, 2017-03-25 (annotated)

Committer:
NilliM
Date:
Sat Mar 25 01:45:20 2017 +0000
Revision:
0:7fa3463671dd
super;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NilliM 0:7fa3463671dd 1 #include "mbed.h"
NilliM 0:7fa3463671dd 2 #include "BMP180.h"
NilliM 0:7fa3463671dd 3 #include "Si7021.h"
NilliM 0:7fa3463671dd 4 #include "TSL2561.h"
NilliM 0:7fa3463671dd 5 #include "MPU9250.h"
NilliM 0:7fa3463671dd 6 #include "INA219.hpp"
NilliM 0:7fa3463671dd 7 #include "JPEGCamera.h"
NilliM 0:7fa3463671dd 8 #include "stdio.h"
NilliM 0:7fa3463671dd 9 #include "stdlib.h"
NilliM 0:7fa3463671dd 10 #include "math.h"
NilliM 0:7fa3463671dd 11 #include "ST7735_TFT.h"
NilliM 0:7fa3463671dd 12 #include "string"
NilliM 0:7fa3463671dd 13 #include "Arial12x12.h"
NilliM 0:7fa3463671dd 14 #include "Arial24x23.h"
NilliM 0:7fa3463671dd 15 #include "Arial28x28.h"
NilliM 0:7fa3463671dd 16 #include "SDFileSystem.h"
NilliM 0:7fa3463671dd 17
NilliM 0:7fa3463671dd 18 #define NUMBER_OF_STARS 400
NilliM 0:7fa3463671dd 19 #define SCREEN_WIDTH 200
NilliM 0:7fa3463671dd 20 #define SCREEN_HEIGHT 250
NilliM 0:7fa3463671dd 21
NilliM 0:7fa3463671dd 22 /*star struct*/
NilliM 0:7fa3463671dd 23 typedef struct
NilliM 0:7fa3463671dd 24 {
NilliM 0:7fa3463671dd 25 float xpos, ypos;
NilliM 0:7fa3463671dd 26 short zpos, speed;
NilliM 0:7fa3463671dd 27 unsigned int color;
NilliM 0:7fa3463671dd 28 } STAR;
NilliM 0:7fa3463671dd 29
NilliM 0:7fa3463671dd 30 static STAR stars[NUMBER_OF_STARS];
NilliM 0:7fa3463671dd 31
NilliM 0:7fa3463671dd 32
NilliM 0:7fa3463671dd 33 void init_star(STAR* star, int id)
NilliM 0:7fa3463671dd 34 {
NilliM 0:7fa3463671dd 35 /* randomly init stars, generate them around the center of the screen */
NilliM 0:7fa3463671dd 36
NilliM 0:7fa3463671dd 37 star->xpos = -10.0 + (20.0 * (rand()/(RAND_MAX+1.0)));
NilliM 0:7fa3463671dd 38 star->ypos = -10.0 + (20.0 * (rand()/(RAND_MAX+1.0)));
NilliM 0:7fa3463671dd 39
NilliM 0:7fa3463671dd 40 star->xpos *= 3072.0; /*change viewpoint */
NilliM 0:7fa3463671dd 41 star->ypos *= 3072.0;
NilliM 0:7fa3463671dd 42
NilliM 0:7fa3463671dd 43 star->zpos = id;
NilliM 0:7fa3463671dd 44 star->speed = 2 + (int)(2.0 * (rand()/(RAND_MAX+1.0)));
NilliM 0:7fa3463671dd 45
NilliM 0:7fa3463671dd 46 star->color = id*Cyan >> 2; /*the closer to the viewer the brighter*/
NilliM 0:7fa3463671dd 47 }
NilliM 0:7fa3463671dd 48
NilliM 0:7fa3463671dd 49
NilliM 0:7fa3463671dd 50 void init()
NilliM 0:7fa3463671dd 51 {
NilliM 0:7fa3463671dd 52 int id;
NilliM 0:7fa3463671dd 53
NilliM 0:7fa3463671dd 54 for (id = 0; id < NUMBER_OF_STARS; id++)
NilliM 0:7fa3463671dd 55 {
NilliM 0:7fa3463671dd 56 init_star(stars + id, id + 1);
NilliM 0:7fa3463671dd 57 }
NilliM 0:7fa3463671dd 58 }
NilliM 0:7fa3463671dd 59
NilliM 0:7fa3463671dd 60
NilliM 0:7fa3463671dd 61 // the TFT is connected to SPI pin 5-7, CS is p8, RS is p11, reset is p15
NilliM 0:7fa3463671dd 62 ST7735_TFT TFT(p5, p6, p7, p12, p11, p15,"TFT"); // mosi, miso, sclk, cs, rs, reset
NilliM 0:7fa3463671dd 63 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
NilliM 0:7fa3463671dd 64 #define archivod "/sd/datos/dato%05d.txt";
NilliM 0:7fa3463671dd 65
NilliM 0:7fa3463671dd 66 int fru=0;
NilliM 0:7fa3463671dd 67 Timer td;
NilliM 0:7fa3463671dd 68
NilliM 0:7fa3463671dd 69 //Serial BT(p13, p14); // tx, rx
NilliM 0:7fa3463671dd 70
NilliM 0:7fa3463671dd 71 int i=0;
NilliM 0:7fa3463671dd 72
NilliM 0:7fa3463671dd 73 I2C myI2C(p9,p10);
NilliM 0:7fa3463671dd 74 Serial pc(USBTX,USBRX);
NilliM 0:7fa3463671dd 75
NilliM 0:7fa3463671dd 76 BMP180 bmp180(&myI2C); //Barometro
NilliM 0:7fa3463671dd 77 Si7021 sensor(p9,p10); //Sensor de humedad
NilliM 0:7fa3463671dd 78 TSL2561 tsl2561(p9,p10); //Sensor de luz
NilliM 0:7fa3463671dd 79 MPU9250 mpu9250; //Giroscopio
NilliM 0:7fa3463671dd 80 INA219 ina219(p9, p10, 0x41, 100000, RES_10BITS);
NilliM 0:7fa3463671dd 81
NilliM 0:7fa3463671dd 82 I2C RGBH(p9, p10); //RGB
NilliM 0:7fa3463671dd 83 DigitalOut green(p11);
NilliM 0:7fa3463671dd 84 int sensor_addr = 41 << 1;
NilliM 0:7fa3463671dd 85
NilliM 0:7fa3463671dd 86 Timer t;//Giroscopio
NilliM 0:7fa3463671dd 87 Timer setuptimer; //Luz
NilliM 0:7fa3463671dd 88 Timer executetimer;//Luz
NilliM 0:7fa3463671dd 89
NilliM 0:7fa3463671dd 90 //Barometro
NilliM 0:7fa3463671dd 91 int press;
NilliM 0:7fa3463671dd 92 float temp;
NilliM 0:7fa3463671dd 93 //Giroscopio
NilliM 0:7fa3463671dd 94 float sum = 0;
NilliM 0:7fa3463671dd 95 uint32_t sumCount = 0;
NilliM 0:7fa3463671dd 96 char buffer[14];
NilliM 0:7fa3463671dd 97
NilliM 0:7fa3463671dd 98 void setup(void){ //Luz y Giroscopio
NilliM 0:7fa3463671dd 99
NilliM 0:7fa3463671dd 100 if (tsl2561.begin()) pc.printf("TSL2561 Sensor Found\n\r");
NilliM 0:7fa3463671dd 101 else pc.printf("TSL2561 Sensor not Found\n\r");
NilliM 0:7fa3463671dd 102
NilliM 0:7fa3463671dd 103 tsl2561.setGain(TSL2561_GAIN_0X); // set no gain (for bright situtations)
NilliM 0:7fa3463671dd 104 tsl2561.setTiming(TSL2561_INTEGRATIONTIME_402MS); // longest integration time (dim tsl2561)
NilliM 0:7fa3463671dd 105
NilliM 0:7fa3463671dd 106 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
NilliM 0:7fa3463671dd 107 if (whoami == 0x71) pc.printf("MPU9250 is online...\n\r");
NilliM 0:7fa3463671dd 108 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
NilliM 0:7fa3463671dd 109 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
NilliM 0:7fa3463671dd 110 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
NilliM 0:7fa3463671dd 111 wait(2);
NilliM 0:7fa3463671dd 112 mpu9250.initMPU9250();
NilliM 0:7fa3463671dd 113 mpu9250.initAK8963(magCalibration);
NilliM 0:7fa3463671dd 114 wait(1);
NilliM 0:7fa3463671dd 115 }
NilliM 0:7fa3463671dd 116
NilliM 0:7fa3463671dd 117
NilliM 0:7fa3463671dd 118 int main(){
NilliM 0:7fa3463671dd 119
NilliM 0:7fa3463671dd 120 JPEGCamera camera(p13, p14); // TX, RX
NilliM 0:7fa3463671dd 121 LocalFileSystem local("local"); //save images on mbed
NilliM 0:7fa3463671dd 122 Timer timer;
NilliM 0:7fa3463671dd 123 timer.start();
NilliM 0:7fa3463671dd 124 camera.setPictureSize(JPEGCamera::SIZE320x240);
NilliM 0:7fa3463671dd 125
NilliM 0:7fa3463671dd 126 //BT.baud(38400);
NilliM 0:7fa3463671dd 127 //Sensor de luz
NilliM 0:7fa3463671dd 128 setuptimer.start();
NilliM 0:7fa3463671dd 129 setup();
NilliM 0:7fa3463671dd 130 setuptimer.stop();
NilliM 0:7fa3463671dd 131 setuptimer.reset();
NilliM 0:7fa3463671dd 132 uint16_t x,y,z;
NilliM 0:7fa3463671dd 133 t.start();
NilliM 0:7fa3463671dd 134
NilliM 0:7fa3463671dd 135 //RGB
NilliM 0:7fa3463671dd 136 RGBH.frequency(200000);
NilliM 0:7fa3463671dd 137 green = 1; // off
NilliM 0:7fa3463671dd 138
NilliM 0:7fa3463671dd 139 char id_regval[1] = {146};
NilliM 0:7fa3463671dd 140 char data[1] = {0};
NilliM 0:7fa3463671dd 141 RGBH.write(sensor_addr,id_regval,1, true);
NilliM 0:7fa3463671dd 142 RGBH.read(sensor_addr,data,1,false);
NilliM 0:7fa3463671dd 143
NilliM 0:7fa3463671dd 144 if (data[0]==68) {
NilliM 0:7fa3463671dd 145 green = 0;
NilliM 0:7fa3463671dd 146 wait (2);
NilliM 0:7fa3463671dd 147 green = 1;
NilliM 0:7fa3463671dd 148 } else {
NilliM 0:7fa3463671dd 149 green = 1;
NilliM 0:7fa3463671dd 150 }
NilliM 0:7fa3463671dd 151
NilliM 0:7fa3463671dd 152 // Initialize color sensor
NilliM 0:7fa3463671dd 153
NilliM 0:7fa3463671dd 154 char timing_register[2] = {129,0};
NilliM 0:7fa3463671dd 155 RGBH.write(sensor_addr,timing_register,2,false);
NilliM 0:7fa3463671dd 156
NilliM 0:7fa3463671dd 157 char control_register[2] = {143,0};
NilliM 0:7fa3463671dd 158 RGBH.write(sensor_addr,control_register,2,false);
NilliM 0:7fa3463671dd 159
NilliM 0:7fa3463671dd 160 char enable_register[2] = {128,3};
NilliM 0:7fa3463671dd 161 RGBH.write(sensor_addr,enable_register,2,false);
NilliM 0:7fa3463671dd 162
NilliM 0:7fa3463671dd 163 mkdir("/sd/datos", 0777);
NilliM 0:7fa3463671dd 164
NilliM 0:7fa3463671dd 165 unsigned int centerx, centery;
NilliM 0:7fa3463671dd 166 int id, j, tempx, tempy;
NilliM 0:7fa3463671dd 167 init();
NilliM 0:7fa3463671dd 168 TFT.set_orientation(2);
NilliM 0:7fa3463671dd 169 centerx = TFT.width() >> 1;
NilliM 0:7fa3463671dd 170 centery = TFT.height() >> 1;
NilliM 0:7fa3463671dd 171
NilliM 0:7fa3463671dd 172
NilliM 0:7fa3463671dd 173 TFT.claim(stdout); // send stdout to the TFT display
NilliM 0:7fa3463671dd 174 //TFT.claim(stderr); // send stderr to the TFT display
NilliM 0:7fa3463671dd 175
NilliM 0:7fa3463671dd 176 TFT.background(Black); // set background to black
NilliM 0:7fa3463671dd 177 TFT.foreground(White); // set chars to white
NilliM 0:7fa3463671dd 178
NilliM 0:7fa3463671dd 179 TFT.cls();
NilliM 0:7fa3463671dd 180 TFT.set_font((unsigned char*) Arial12x12); // select the font
NilliM 0:7fa3463671dd 181
NilliM 0:7fa3463671dd 182 td.start();
NilliM 0:7fa3463671dd 183
NilliM 0:7fa3463671dd 184 ////// demo start
NilliM 0:7fa3463671dd 185
NilliM 0:7fa3463671dd 186 for ( j = 0 ; j < 5000; j++ )
NilliM 0:7fa3463671dd 187 {
NilliM 0:7fa3463671dd 188
NilliM 0:7fa3463671dd 189 /* move and draw stars */
NilliM 0:7fa3463671dd 190
NilliM 0:7fa3463671dd 191 for (id = 0; i < NUMBER_OF_STARS; id++)
NilliM 0:7fa3463671dd 192 {
NilliM 0:7fa3463671dd 193 tempx = (stars[id].xpos / stars[id].zpos) + centerx;
NilliM 0:7fa3463671dd 194 tempy = (stars[id].ypos / stars[id].zpos) + centery;
NilliM 0:7fa3463671dd 195 TFT.pixel(tempx,tempy,Black);
NilliM 0:7fa3463671dd 196
NilliM 0:7fa3463671dd 197
NilliM 0:7fa3463671dd 198 stars[id].zpos -= stars[id].speed;
NilliM 0:7fa3463671dd 199
NilliM 0:7fa3463671dd 200 if (stars[id].zpos <= 0)
NilliM 0:7fa3463671dd 201 {
NilliM 0:7fa3463671dd 202 init_star(stars + id, id + 1);
NilliM 0:7fa3463671dd 203 }
NilliM 0:7fa3463671dd 204
NilliM 0:7fa3463671dd 205 //compute 3D position
NilliM 0:7fa3463671dd 206 tempx = (stars[id].xpos / stars[id].zpos) + centerx;
NilliM 0:7fa3463671dd 207 tempy = (stars[id].ypos / stars[id].zpos) + centery;
NilliM 0:7fa3463671dd 208
NilliM 0:7fa3463671dd 209 if (tempx < 0 || tempx > TFT.width() - 1 || tempy < 0 || tempy > TFT.height() - 1) //check if a star leaves the screen
NilliM 0:7fa3463671dd 210 {
NilliM 0:7fa3463671dd 211 init_star(stars + id, id + 1);
NilliM 0:7fa3463671dd 212
NilliM 0:7fa3463671dd 213 continue;
NilliM 0:7fa3463671dd 214 }
NilliM 0:7fa3463671dd 215
NilliM 0:7fa3463671dd 216 TFT.pixel(tempx,tempy,stars[id].color);
NilliM 0:7fa3463671dd 217
NilliM 0:7fa3463671dd 218
NilliM 0:7fa3463671dd 219 }
NilliM 0:7fa3463671dd 220 //TFT.Bitmap(centerx-60,centery-19,120,38,p1);
NilliM 0:7fa3463671dd 221
NilliM 0:7fa3463671dd 222 printf("N y U %i\n", fru);
NilliM 0:7fa3463671dd 223 /*fru++;
NilliM 0:7fa3463671dd 224 pc.printf("%i", fru);
NilliM 0:7fa3463671dd 225 char fil[32];
NilliM 0:7fa3463671dd 226 sprintf(fil, "/sd/datos/dato%d.txt", fru);
NilliM 0:7fa3463671dd 227 FILE *fp = fopen(fil, "w");
NilliM 0:7fa3463671dd 228 fprintf(fp,"Swag\n\r");
NilliM 0:7fa3463671dd 229 pc.printf("Escribiendo");
NilliM 0:7fa3463671dd 230 wait(0.3);
NilliM 0:7fa3463671dd 231 fclose(fp);
NilliM 0:7fa3463671dd 232 pc.printf("Cerrado");*/
NilliM 0:7fa3463671dd 233 }
NilliM 0:7fa3463671dd 234
NilliM 0:7fa3463671dd 235
NilliM 0:7fa3463671dd 236
NilliM 0:7fa3463671dd 237 ///// demo stop
NilliM 0:7fa3463671dd 238
NilliM 0:7fa3463671dd 239 td.stop();
NilliM 0:7fa3463671dd 240 TFT.locate(0,10);
NilliM 0:7fa3463671dd 241 TFT.set_font((unsigned char*) Arial12x12); // select the font
NilliM 0:7fa3463671dd 242 printf("Time %f s\n", td.read());
NilliM 0:7fa3463671dd 243
NilliM 0:7fa3463671dd 244 while(1)
NilliM 0:7fa3463671dd 245 {
NilliM 0:7fa3463671dd 246 //Sensor de luz
NilliM 0:7fa3463671dd 247 executetimer.start();
NilliM 0:7fa3463671dd 248 x = tsl2561.getLuminosity(TSL2561_VISIBLE);
NilliM 0:7fa3463671dd 249 y = tsl2561.getLuminosity(TSL2561_FULLSPECTRUM);
NilliM 0:7fa3463671dd 250 z = tsl2561.getLuminosity(TSL2561_INFRARED);
NilliM 0:7fa3463671dd 251 executetimer.stop();
NilliM 0:7fa3463671dd 252
NilliM 0:7fa3463671dd 253
NilliM 0:7fa3463671dd 254 bmp180.init(); //Barometro
NilliM 0:7fa3463671dd 255 sensor.measure();//Sensor de humedad
NilliM 0:7fa3463671dd 256
NilliM 0:7fa3463671dd 257 //Barometro
NilliM 0:7fa3463671dd 258 float altitud;
NilliM 0:7fa3463671dd 259 bmp180.startTemperature();
NilliM 0:7fa3463671dd 260 wait_ms(5); //
NilliM 0:7fa3463671dd 261 if(bmp180.getTemperature(&temp) != 0) {
NilliM 0:7fa3463671dd 262 printf("Error getting temperature\n");
NilliM 0:7fa3463671dd 263 continue;
NilliM 0:7fa3463671dd 264 }
NilliM 0:7fa3463671dd 265 bmp180.startPressure(BMP180::ULTRA_LOW_POWER);
NilliM 0:7fa3463671dd 266 wait_ms(10); // Wait for conversion to complete
NilliM 0:7fa3463671dd 267 if(bmp180.getPressure(&press) != 0) {
NilliM 0:7fa3463671dd 268 printf("Error getting pressure\n");
NilliM 0:7fa3463671dd 269 continue;
NilliM 0:7fa3463671dd 270 }
NilliM 0:7fa3463671dd 271 altitud= 44330.0*(1-pow(press*.01/1020.0,(1/5.225)));
NilliM 0:7fa3463671dd 272 //Sensor de humedad
NilliM 0:7fa3463671dd 273 float temps=sensor.get_temperature()/1000.0;
NilliM 0:7fa3463671dd 274 float hum=sensor.get_humidity()/1000.0;
NilliM 0:7fa3463671dd 275
NilliM 0:7fa3463671dd 276 //Sensor de voltaje
NilliM 0:7fa3463671dd 277 float current_ma = ina219.read_current_mA();
NilliM 0:7fa3463671dd 278 float voltageb = ina219.read_bus_voltage();
NilliM 0:7fa3463671dd 279
NilliM 0:7fa3463671dd 280 //Giroscopio
NilliM 0:7fa3463671dd 281 mpu9250.getAres(); // Get accelerometer sensitivity
NilliM 0:7fa3463671dd 282 mpu9250.getGres(); // Get gyro sensitivity
NilliM 0:7fa3463671dd 283 mpu9250.getMres(); // Get magnetometer sensitivity
NilliM 0:7fa3463671dd 284 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
NilliM 0:7fa3463671dd 285 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
NilliM 0:7fa3463671dd 286 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
NilliM 0:7fa3463671dd 287 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
NilliM 0:7fa3463671dd 288 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
NilliM 0:7fa3463671dd 289 // Now we'll calculate the accleration value into actual g's
NilliM 0:7fa3463671dd 290 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
NilliM 0:7fa3463671dd 291 ay = (float)accelCount[1]*aRes - accelBias[1];
NilliM 0:7fa3463671dd 292 az = (float)accelCount[2]*aRes - accelBias[2];
NilliM 0:7fa3463671dd 293
NilliM 0:7fa3463671dd 294 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
NilliM 0:7fa3463671dd 295 // Calculate the gyro value into actual degrees per second
NilliM 0:7fa3463671dd 296 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
NilliM 0:7fa3463671dd 297 gy = (float)gyroCount[1]*gRes - gyroBias[1];
NilliM 0:7fa3463671dd 298 gz = (float)gyroCount[2]*gRes - gyroBias[2];
NilliM 0:7fa3463671dd 299
NilliM 0:7fa3463671dd 300 mpu9250.readMagData(magCount); // Read the x/y/z adc values
NilliM 0:7fa3463671dd 301 // Calculate the magnetometer values in milliGauss
NilliM 0:7fa3463671dd 302 // Include factory calibration per data sheet and user environmental corrections
NilliM 0:7fa3463671dd 303 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
NilliM 0:7fa3463671dd 304 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
NilliM 0:7fa3463671dd 305 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
NilliM 0:7fa3463671dd 306 }
NilliM 0:7fa3463671dd 307 Now = t.read_us();
NilliM 0:7fa3463671dd 308 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
NilliM 0:7fa3463671dd 309 lastUpdate = Now;
NilliM 0:7fa3463671dd 310 sum += deltat;
NilliM 0:7fa3463671dd 311 sumCount++;
NilliM 0:7fa3463671dd 312
NilliM 0:7fa3463671dd 313 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
NilliM 0:7fa3463671dd 314 tempCount = mpu9250.readTempData(); // Read the adc values
NilliM 0:7fa3463671dd 315 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
NilliM 0:7fa3463671dd 316 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]);
NilliM 0:7fa3463671dd 317 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
NilliM 0:7fa3463671dd 318 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]);
NilliM 0:7fa3463671dd 319 pitch *= 180.0f / PI;
NilliM 0:7fa3463671dd 320 yaw *= 180.0f / PI;
NilliM 0:7fa3463671dd 321 yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
NilliM 0:7fa3463671dd 322 roll *= 180.0f / PI;
NilliM 0:7fa3463671dd 323
NilliM 0:7fa3463671dd 324 //RGB
NilliM 0:7fa3463671dd 325 char clear_reg[1] = {148};
NilliM 0:7fa3463671dd 326 char clear_data[2] = {0,0};
NilliM 0:7fa3463671dd 327 RGBH.write(sensor_addr,clear_reg,1, true);
NilliM 0:7fa3463671dd 328 RGBH.read(sensor_addr,clear_data,2, false);
NilliM 0:7fa3463671dd 329
NilliM 0:7fa3463671dd 330 int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
NilliM 0:7fa3463671dd 331
NilliM 0:7fa3463671dd 332 char red_reg[1] = {150};
NilliM 0:7fa3463671dd 333 char red_data[2] = {0,0};
NilliM 0:7fa3463671dd 334 RGBH.write(sensor_addr,red_reg,1, true);
NilliM 0:7fa3463671dd 335 RGBH.read(sensor_addr,red_data,2, false);
NilliM 0:7fa3463671dd 336
NilliM 0:7fa3463671dd 337 int red_value = ((int)red_data[1] << 8) | red_data[0];
NilliM 0:7fa3463671dd 338
NilliM 0:7fa3463671dd 339 char green_reg[1] = {152};
NilliM 0:7fa3463671dd 340 char green_data[2] = {0,0};
NilliM 0:7fa3463671dd 341 RGBH.write(sensor_addr,green_reg,1, true);
NilliM 0:7fa3463671dd 342 RGBH.read(sensor_addr,green_data,2, false);
NilliM 0:7fa3463671dd 343
NilliM 0:7fa3463671dd 344 int green_value = ((int)green_data[1] << 8) | green_data[0];
NilliM 0:7fa3463671dd 345
NilliM 0:7fa3463671dd 346 char blue_reg[1] = {154};
NilliM 0:7fa3463671dd 347 char blue_data[2] = {0,0};
NilliM 0:7fa3463671dd 348 RGBH.write(sensor_addr,blue_reg,1, true);
NilliM 0:7fa3463671dd 349 RGBH.read(sensor_addr,blue_data,2, false);
NilliM 0:7fa3463671dd 350
NilliM 0:7fa3463671dd 351 int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
NilliM 0:7fa3463671dd 352
NilliM 0:7fa3463671dd 353
NilliM 0:7fa3463671dd 354 pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\r\n", clear_value, red_value, green_value, blue_value);
NilliM 0:7fa3463671dd 355 pc.printf("P=%d Pa\n\rT=%.2fC\n\rAltitud= %.2f m\n\r", press, temp, altitud); //Barometro
NilliM 0:7fa3463671dd 356 pc.printf("Temp: %0.2f\n\r",temps); //Humedad
NilliM 0:7fa3463671dd 357 pc.printf("Humidity: %0.2f % \n\r",hum);//Humedad
NilliM 0:7fa3463671dd 358 pc.printf("Visible: %d \n\r",x); //Luz
NilliM 0:7fa3463671dd 359 pc.printf("Full Spectrum: %d\n\r",y); //Luz
NilliM 0:7fa3463671dd 360 pc.printf("Infrared: %d\n\r",z);//Luz
NilliM 0:7fa3463671dd 361 pc.printf(" temperature = %f C\n\r", temperature); //Giroscopio
NilliM 0:7fa3463671dd 362 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);
NilliM 0:7fa3463671dd 363 pc.printf("%f mA\r\n", current_ma);
NilliM 0:7fa3463671dd 364 pc.printf("%f V\r\n", voltageb);
NilliM 0:7fa3463671dd 365
NilliM 0:7fa3463671dd 366 /*
NilliM 0:7fa3463671dd 367 BT.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\r\n", clear_value, red_value, green_value, blue_value);
NilliM 0:7fa3463671dd 368 BT.printf("P=%d Pa\n\rT=%.2fC\n\r", press, temp); //Barometro
NilliM 0:7fa3463671dd 369 BT.printf("Temp: %0.2f\n\r",temps); //Humedad
NilliM 0:7fa3463671dd 370 BT.printf("Humidity: %0.2f % \n\r",hum);//Humedad
NilliM 0:7fa3463671dd 371 BT.printf("Visible: %d \n\r",x); //Luz
NilliM 0:7fa3463671dd 372 BT.printf("Full Spectrum: %d\n\r",y); //Luz
NilliM 0:7fa3463671dd 373 BT.printf("Infrared: %d\n\r",z);//Luz
NilliM 0:7fa3463671dd 374 BT.printf(" temperature = %f C\n\r", temperature); //Giroscopio
NilliM 0:7fa3463671dd 375 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);
NilliM 0:7fa3463671dd 376 BT.printf("%f mA\r\n", current_ma);
NilliM 0:7fa3463671dd 377 BT.printf("%f V\r\n", voltageb);*/
NilliM 0:7fa3463671dd 378
NilliM 0:7fa3463671dd 379
NilliM 0:7fa3463671dd 380 if (camera.isReady()) {
NilliM 0:7fa3463671dd 381 char filename[32];
NilliM 0:7fa3463671dd 382 sprintf(filename, "/local/pict%03d.jpg", i);
NilliM 0:7fa3463671dd 383 //printf("Picture: %s ", filename);
NilliM 0:7fa3463671dd 384 if (camera.takePicture(filename)) {
NilliM 0:7fa3463671dd 385 while (camera.isProcessing()) {
NilliM 0:7fa3463671dd 386 camera.processPicture();}}}
NilliM 0:7fa3463671dd 387 i++;
NilliM 0:7fa3463671dd 388 pc.printf("Picture");
NilliM 0:7fa3463671dd 389
NilliM 0:7fa3463671dd 390 wait(1);
NilliM 0:7fa3463671dd 391
NilliM 0:7fa3463671dd 392
NilliM 0:7fa3463671dd 393 }}