File content as of revision 1:d8964d9ff503:

#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_12); //
// Analog pin sensors : battery and temperature
AnalogIn   batin(PA_0);//battery pin pA0
AnalogIn   tempin(PA_1);//temperature pin pa1
AnalogIn   sp_1in(PA_6);//Solar Panel 1 pin pa1
AnalogIn   sp_2in(PA_5);//Solar Panel 2 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, temperature sensor test\r\n");
    pc.printf("\t Solar Panel 1 and 2 Currents 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+0.32f);        // 3.3 * 13.9/10 + 0.32 (Vd)
        pc.printf("temperature: %.1f degC\t\t",(tempin.read()*3.3f-0.5f)/0.01f);
        pc.printf("solar panel 1: %.1f mA\t",(sp_1in.read()*3.3f*50));
        pc.printf("and 2: %.1f mA\r\n",(sp_2in.read()*3.3f*50));
        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 Analog Sensors\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;
        }
    }
}