File content as of revision 4:34a8e94c6fd5:

#include "mbed.h"
#include "mpu9250.h"
#include "SDFileSystem.h"
#include "soft_uart.h"
#include <string.h>
#include "ArduCAM2640.h"
#include "Servo/Servo.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;

Servo servo_pan(PB_0);
Servo servo_tilt(PB_1);
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 Current 1 pin PA6
AnalogIn   sp_2in(PA_5);//Solar Panel Current 2 pin PA5
uint8_t g_image_buffer[IMAGE_JPG_SIZE];

void get_image(void);

void init_serial(void)
{
    pc.baud(9600);
    xbee.baud(38400);
}

void Init_Inisat(void)
{
    activ_sensors = 0;        // 3.3V power sensors board Off
  
}
   
void test_imu(void)
{
    unsigned char stop;
    //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");
    /*--------------------------------------------------------------*/
    xbee.printf("\r\n/*----------------------------------------------*/\r\n");
    xbee.printf("\t IMU9250 test\r\n");
    xbee.printf("send \"s\" to stop\r\n");
    xbee.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");
    xbee.printf("I AM 0x%x\n\r", whoami); 
    xbee.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");
        xbee.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
        xbee.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);
        xbee.printf("Could not connect to MPU9250: \n\r");
        xbee.printf("%#x \n",  whoami);
        sprintf(buffer, "WHO_AM_I 0x%x", whoami);
        exit(0);
    }
    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 > 1000) 
        { // 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); 
            /*---------------------------------------------------*/
            xbee.printf("ax = %f", 1000*ax); 
            xbee.printf(" ay = %f", 1000*ay); 
            xbee.printf(" az = %f  mg\n\r", 1000*az); 
            xbee.printf("gx = %f", gx); 
            xbee.printf(" gy = %f", gy); 
            xbee.printf(" gz = %f  deg/s\n\r", gz); 
            xbee.printf("gx = %f", mx); 
            xbee.printf(" gy = %f", my); 
            xbee.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);
            xbee.printf(" temperature = %f  C\n\r", temperature); 
            xbee.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();
        if(xbee.readable())
            stop=xbee.getc();
    } while(stop!='s'&& (stop != 'S'));
    //activ_sensors = 0;        // 3.3V power sensors board Off
}

void get_image(void)
{ 
    /* 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 )
                {
                    pc.printf("Photo failure %d\r\n",image_size);
                    xbee.printf("Photo failure %d\r\n",image_size);
                    break;
                }
                else
                {
                    pc.printf("taille image%d",image_size);
                    xbee.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;
     if( g_arducam.Setup( OV_RESOLUTION_CIF/*OV_RESOLUTION_QQVGA*/,92, &cam_spi_cs, &camSPI, &camI2C) == false)
    {
        pc.printf("Arducam setup error \r\n");
        xbee.printf("Arducam setup error \r\n");
        exit(0);
    }
    else
        pc.printf("init_ok"); 
        xbee.printf("init_ok");
    get_image();

}

void test_gps(void)
{unsigned char tmp;
    unsigned char stop=0;
    //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");
    /*----------------------------------------------------------------*/
    xbee.printf("\r\n/*----------------------------------------------*/\r\n");
    xbee.printf("\tGPS module test\r\n");
    xbee.printf("send \"s\" to stop\r\n");
    xbee.printf("/*----------------------------------------------*/\r\n");
    wait(2);
  do
  { 
    do
    {
        tmp= _getchar();
        pc.printf("%c",tmp);
        xbee.printf("%c",tmp);
    }while(tmp!='\r');
    if(pc.readable())
           stop=pc.getc();
    if(xbee.readable())
           stop=xbee.getc();
    } while(stop!='s'&& (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"); 
    /*----------------------------------------------------------------*/
    xbee.printf("\r\n/*----------------------------------------------------*/\r\n");
    xbee.printf("\t SDCard module test\r\n");
    xbee.printf("/*----------------------------------------------------*/\r\n");
    xbee.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);
    xbee.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");
    /*-------------------------------------------------------------------*/
    xbee.printf("\r\n/*----------------------------------------------*/\r\n");
    xbee.printf("\t Battery level, temperature sensor test\r\n");
    xbee.printf("\t Solar Panel 1 and 2 Currents test\r\n");
    xbee.printf("send \"s\" to stop\r\n");
    xbee.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);
        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));
        /*--------------------------------------------------------------*/
        xbee.printf("battery: %.2f V\t\t",batin.read()*4.59f);        // 3.3 * 13.9/10
        xbee.printf("temperature: %.1f degC\r\n",(tempin.read()*3.3f-0.5f)/0.01f);
        xbee.printf("solar panel 1: %.1f mA\t",(sp_1in.read()*3.3f*50));
        xbee.printf("and 2: %.1f mA\r\n",(sp_2in.read()*3.3f*50));
        if(pc.readable())
            stop=pc.getc();
        if(xbee.readable())
            stop=xbee.getc();
     }while(stop!='s'&& (stop != 'S'));   
} 
/*void quit(void)
{
    //do nothing
    pc.printf("\r\nbye bye...");
    exit(0);
}*/

void test_pan_tilt(void)
{
    unsigned char stop=0;
    pc.printf("\r\n/*----------------------------------------------*/\r\n");
    pc.printf("\t Pan Tilt stage test\r\n");
    pc.printf("send \"s\" to stop\r\n");
    pc.printf("/*----------------------------------------------*/\r\n");
    xbee.printf("\r\n/*----------------------------------------------*/\r\n");
    xbee.printf("\t Pan Tilt stage test\r\n");
    xbee.printf("send \"s\" to stop\r\n");
    xbee.printf("/*----------------------------------------------*/\r\n");
    wait(1);    
    do
    {
        for(float p=0.2; p<0.7; p += 0.1) 
        {
            servo_pan = p;
            servo_tilt= p;
            wait(0.4);
        }
        for(float p=0.7; p>0.2; p -= 0.1) 
        {
            servo_pan = p;
            servo_tilt= p;
            wait(0.4);
        }
        if(pc.readable())
            stop=pc.getc();
        if(xbee.readable())
            stop=xbee.getc();
    }while(stop!='s'&& (stop != 'S'));
    servo_pan = 0.5;
    servo_tilt=0.5;
}

int menu(void)
{int choice=0;
// menu tests
    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 pan/tilt plates \r\n");
    pc.printf("/*-------------------------------------------*/\r\n");
    /*---------------------------------------------------------------------*/
    xbee.printf("\r\n/*-------------------------------------------*/\r\n");
    xbee.printf("\t1. Test IMU\r\n");
    xbee.printf("\t2. Test SDCARD\r\n");
    xbee.printf("\t3. Test GPS\r\n");
    xbee.printf("\t4. Test CAMERA\r\n");
    xbee.printf("\t5. Test Analog Sensors\r\n");
    xbee.printf("\t6. Test pan/tilt plates \r\n");
    xbee.printf("/*-------------------------------------------*/\r\n");
    do
    {
        pc.printf("Please enter your choice (1 to 6):"); 
        xbee.printf("Please enter your choice (1 to 6):");
        if (pc.readable())
            choice=pc.getc();
        if (xbee.readable())
            choice=xbee.getc(); 
    }while(choice<0x31||choice>0x36); 
    return choice;
}

int main()
{   
    float batest;
    unsigned char start;
    int status=0;
  //  NVIC_SystemReset();
  //initialise serial communication
    init_serial();
    //initialise inisat port
    Init_Inisat();
    batest=batin.read()*4.59f;
    pc.printf("battery: %.2f V\r\n",batest);
    xbee.printf("battery: %.2f V \r\n",batest);
    if (batest < 3.3f)
    {
        pc.printf("low battery, please recharge\r\n");
        xbee.printf("low battery, please recharge \r\n");
        exit(0);
    }
    pc.printf("Press \"S\" to Start\r\n");
    xbee.printf("Press \"S\" or \"s\" to Start\r\n");
    do
    {
        if(pc.readable())
            start=pc.getc();
        if(xbee.readable())
            start=xbee.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 '6' : test_pan_tilt();
                status=0;
                break;
            default : break;
        }
    }
}