Diff: main.cpp
- Revision:
- 0:b8bade04f24f
- Child:
- 1:d8964d9ff503
- Child:
- 2:065d789dbcbd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Sep 03 21:45:56 2020 +0000
@@ -0,0 +1,398 @@
+#include "mbed.h"
+#include "mpu9250.h"
+#include "SDFileSystem.h"
+#include "soft_uart.h"
+#include <string.h>
+#include "ArduCAM2640.h"
+#define IMAGE_JPG_SIZE 16*1024
+#define MIN(x,y) x < y ? x : y
+float sum = 0;
+uint32_t sumCount = 0;
+char buffer[14];
+char buf[100];
+unsigned int framepos=0;
+ArduCAM2640 g_arducam;
+I2C camI2C(PB_7, PB_6);
+//I2C camI2C(PB_7,PB_6); //SDA/SCL
+SPI camSPI(PB_5,PB_4,PB_3);//MOSI/MISO/SCL
+DigitalOut cam_spi_cs(PA_8);// cs _camera PA_8
+DigitalOut sdcard_spi_cs(PA_11);// cs _sdcard PA_11
+MPU9250 mpu9250;
+Timer t;
+// serial port for USB, camera, GPS and XBEE
+Serial pc(USBTX, USBRX); // usb serial tx, rx
+
+//Serial ser(pa3, pa4);//GPS softserial TX, RX
+Serial xbee(PA_9, PA_10); //xbee serial pin TX,RX
+//sdcard
+SDFileSystem sd(PB_5, PB_4, PB_3, PA_11, "sd"); //MOSI,MISO,SCK,CS the pinout on the mbed Cool Components workshop board
+
+// activation 3.3V power supply on sensors board
+DigitalOut activ_sensors(PA_7); //
+// Analog pin sensors : battery and temperature
+AnalogIn batin(PA_0);//battery pin pA0
+AnalogIn tempin(PA_1);//temperature pin pa1
+uint8_t g_image_buffer[IMAGE_JPG_SIZE];
+
+void telegram_bot();
+void init_serial(void)
+{
+ pc.baud(9600);
+ xbee.baud(38400);
+ // came.baud(38400);
+}
+
+void Init_Inisat(void)
+{
+ activ_sensors = 0; // 3.3V power sensors board Off
+
+}
+
+void test_imu(void)
+{
+ unsigned char stop;
+ //cam_spi_cs=0;
+ // sdcard_spi_cs=0;
+ //test mpu9250-IMU
+ pc.printf("\r\n/*----------------------------------------------*/\r\n");
+ pc.printf("\t IMU9250 test\r\n");
+ pc.printf("send \"s\" to stop\r\n");
+ pc.printf("/*----------------------------------------------*/\r\n");
+ activ_sensors = 1; // 3.3V power sensors board On
+ wait(2);
+
+ //Set up I2C
+ i2c.frequency(400000); // use fast (400 kHz) I2C
+ t.start();
+ // Read the WHO_AM_I register, this is a good test of communication
+ uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
+ pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
+ if (whoami == 0x71) // WHO_AM_I should always be 0x68
+ {
+ pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
+ pc.printf("MPU9250 is online...\n\r");
+ sprintf(buffer, "0x%x", whoami);
+ wait(1);
+ 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();
+ pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+ mpu9250.initAK8963(magCalibration);
+ pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+ pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
+ pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
+ if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
+ if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
+ if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
+ if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
+ wait(1);
+ }
+ else
+ {
+ pc.printf("Could not connect to MPU9250: \n\r");
+ pc.printf("%#x \n", whoami);
+ sprintf(buffer, "WHO_AM_I 0x%x", whoami);
+ }
+ mpu9250.getAres(); // Get accelerometer sensitivity
+ mpu9250.getGres(); // Get gyro sensitivity
+ mpu9250.getMres(); // Get magnetometer sensitivity
+ pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
+ pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
+ pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
+ 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
+ do
+ {
+ // If intPin goes high, all data registers have new data
+ 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++;
+// Pass gyro rate as rad/s
+// mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+ mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+ // Serial print and/or display at 0.5 s rate independent of data rates
+ delt_t = t.read_ms() - countA;
+ if (delt_t > 500)
+ { // update LCD once per half-second independent of read rate
+ pc.printf("ax = %f", 1000*ax);
+ pc.printf(" ay = %f", 1000*ay);
+ pc.printf(" az = %f mg\n\r", 1000*az);
+ pc.printf("gx = %f", gx);
+ pc.printf(" gy = %f", gy);
+ pc.printf(" gz = %f deg/s\n\r", gz);
+ pc.printf("gx = %f", mx);
+ pc.printf(" gy = %f", my);
+ pc.printf(" gz = %f mG\n\r", mz);
+ tempCount = mpu9250.readTempData(); // Read the adc values
+ temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
+ pc.printf(" temperature = %f C\n\r", temperature);
+ pc.printf("average rate = %f\n\r", (float) sumCount/sum);
+ countA = t.read_ms();
+ if(countA > 1<<21)
+ {
+ t.start(); // start the timer over again if ~30 minutes has passed
+ countA = 0;
+ deltat= 0;
+ lastUpdate = t.read_us();
+ }
+ sum = 0;
+ sumCount = 0;
+ }
+ if(pc.readable())
+ stop=pc.getc();
+ } while(stop!='s');
+ //activ_sensors = 0; // 3.3V power sensors board Off
+}
+
+void telegram_bot()
+{
+
+
+
+ /* main loop */
+ // while (1)
+ {sdcard_spi_cs=1;//deactivate sdcard cs
+ cam_spi_cs=0;
+
+
+ uint32_t image_size;
+ uint32_t idx;
+
+
+ for(int i=0; i<3; i++)
+ {
+ image_size = g_arducam.CaptureImage(g_image_buffer,IMAGE_JPG_SIZE,&idx);
+ if( image_size == 0 )
+ {
+ printf("Photo failure %d\r\n",image_size);
+ break;
+ }
+ else
+ pc.printf("taille image%d",image_size);
+ }
+ sdcard_spi_cs=0;//activate sdcard cs, deactivate cam
+ cam_spi_cs=1;
+ FILE *fp = fopen("/sd/mydir/image.jpg", "w");
+ if(fp == NULL) {
+ error("Could not open file for write\n");
+ }
+
+
+ for (uint32_t l=1;l<image_size+1;l++)
+ fprintf(fp, "%c",g_image_buffer[l]);
+ //pc.printf("%c",g_image_buffer[l]);
+ fclose(fp);
+ }
+}
+void test_camera(void)
+{
+ sdcard_spi_cs=1;//deactivate sdcard cs
+ cam_spi_cs=0;
+ // pc.baud(9600);
+ if( g_arducam.Setup( OV_RESOLUTION_CIF/*OV_RESOLUTION_QQVGA*/,92, &cam_spi_cs, &camSPI, &camI2C) == false)
+ {
+ pc.printf("Arducam setup error \r\n");
+ exit(0);
+ }
+ else
+ pc.printf("init_ok");
+ /* start chatbot */
+ telegram_bot();
+
+}
+
+void test_gps(void)
+{unsigned char tmp;
+ unsigned char stop=0;
+ // ser.baud(9600);
+ //GPS selection
+ activ_sensors = 1; // 3.3V power sensors board On
+
+ init_uart();
+ pc.printf("\r\n/*----------------------------------------------*/\r\n");
+ pc.printf("\tGPS module test\r\n");
+ pc.printf("send \"s\" to stop\r\n");
+ pc.printf("/*----------------------------------------------*/\r\n");
+ wait(2);
+ do
+ {
+ do
+ {
+ tmp= _getchar();
+ pc.printf("%c",tmp);
+ }while(tmp!='\r');
+ if(pc.readable())
+ stop=pc.getc();
+ } while(stop!='s');
+ }
+
+void test_sdcard(void)
+{
+ char buf[50];
+ cam_spi_cs=1; //deactivate CS camera
+
+ pc.printf("\r\n/*----------------------------------------------------*/\r\n");
+ pc.printf("\t SDCard module test\r\n");
+ pc.printf("/*----------------------------------------------------*/\r\n");
+ pc.printf("Write some words in sdtest.txt on sd/mydir directory\r\n");
+ mkdir("/sd/mydir", 0777);
+ FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
+ if(fp == NULL) {
+ error("Could not open file for write\n");
+ }
+ fprintf(fp, "Hello World, greats from Inisat!");
+ fclose(fp);
+ FILE *fp1 = fopen("/sd/mydir/sdtest.txt", "r");
+ if(fp1 == NULL) {
+ error("Could not open file for write\n");
+ }
+ //fscanf(fp,"%s",buf);
+ fgets(buf,50,fp1);
+ wait(2);
+ pc.printf("read from sd/mydir directory : %s\r\n",buf);
+ fclose(fp1);
+}
+
+void test_xbee_comm()
+{
+ char buf[250];
+ pc.printf("\r\n/*----------------------------------------------*/\r\n");
+ pc.printf("\t xbee module test\r\n");
+ pc.printf("/*----------------------------------------------*/\r\n");
+
+ xbee.printf("Hello from inisat\r\nTest receiving data from Ground Station : type a sentence and press enter to loopback\r\n");
+ while(pc.getc()!='s')
+ {
+ xbee.printf("%s\r\n",pc.gets(buf,250)); // Echo
+
+ }
+ while(xbee.getc()!='s')
+ pc.printf("%c\r\n",xbee.getc());
+}
+
+void test_analog_sensor(void)
+{
+ unsigned char stop=0;
+ activ_sensors = 1;
+ pc.printf("\r\n/*----------------------------------------------*/\r\n");
+ pc.printf("\t Battery level and temperature sensor test\r\n");
+ pc.printf("send \"s\" to stop\r\n");
+ pc.printf("/*----------------------------------------------*/\r\n");
+ wait(2);
+ do
+ {
+ wait_ms(300);
+ pc.printf("battery: %.2f V\t\t",batin.read()*4.59f); // 3.3 * 13.9/10
+ pc.printf("temperature: %.1f degC\r\n",(tempin.read()*3.3f-0.5f)/0.01f);
+ if(pc.readable())
+ stop=pc.getc();
+ }while(stop!='s');
+}
+void quit(void)
+{
+ //do nothing
+ pc.printf("\r\nbye bye...");
+ exit(0);
+}
+int menu(void)
+{int choice=0;
+// menu tests
+ //pc.putc(27);//to clear terminal screen
+ //pc.printf("[2J");
+ pc.printf("\r\n/*-------------------------------------------*/\r\n");
+ pc.printf("\t1. Test IMU\r\n");
+ pc.printf("\t2. Test SDCARD\r\n");
+ pc.printf("\t3. Test GPS\r\n");
+ pc.printf("\t4. Test CAMERA\r\n");
+ pc.printf("\t5. Test temperature sensor & battery level \r\n");
+ pc.printf("\t6. Test XBEE communication\r\n");
+ pc.printf("\t7. Quit\r\n");
+ pc.printf("/*-------------------------------------------*/\r\n");
+ do
+ {
+ pc.printf("Please enter your choice (1 to 7):");
+ choice=pc.getc();
+ }while(choice<0x31||choice>0x37);
+ return choice;
+}
+
+int main()
+{
+float batest;
+unsigned char start;
+
+ int status=0;
+ //initialise serial communication
+ init_serial();
+ //initialise inisat port
+ Init_Inisat();
+ batest=batin.read()*4.59f;
+ pc.printf("battery: %.2f V\r\n",batest);
+
+ /* if (batest < 3.3f)
+ {
+ pc.printf("low battery, please recharge\r\n");
+ exit(0);
+ }*/
+ pc.printf("Press \"S\" to Start\r\n");
+ do
+ {
+ if(pc.readable())
+ start=pc.getc();
+ }while((start!='S') && (start != 's'));
+
+ while(1)
+ {
+ switch(status)
+ {
+ case 0 : status=menu();
+ break;
+ case '1' : test_imu();
+ status=0;
+ break;
+ case '2' : test_sdcard();
+ status=0;
+ break;
+ case '3' : test_gps();
+ status=0;
+ break;
+ case '4' : test_camera();
+ status=0;
+ break;
+ case '5' : test_analog_sensor();
+ status=0;
+ break;
+ case '6' : test_xbee_comm();
+ status=0;
+ break;
+ //case 7: all_test();
+ // break;
+ case '7' : quit();
+ break;
+ }
+ }
+}
\ No newline at end of file