06_compass_RTC.ino

// SETUP MPU9250 ACCELLEROMETER AND MAGNETOMETER
bool MPU9250_setup(){
    if (!i2c_init()){                                               // i2c init failed
      i2c_write_byte(MPU9250_ADDRESS,29,0x06);                      // need to do something for some reason
      return 0;                                                     // exit function with error code
    }
  
    // Set accelerometers low pass filter at 5Hz
    if (!i2c_write_byte(MPU9250_ADDRESS,29,0x06)) return 0;         // set, exit with error code upon failure

    // Set gyroscope low pass filter at 5Hz
    if (!i2c_write_byte(MPU9250_ADDRESS,26,0x06)) return 0;
     
    // Configure accelerometers range 0x00 2G, 0x08 4G, 0x10 8G, 0x18 16G
    if (!i2c_write_byte(MPU9250_ADDRESS,28,0x00)) return 0;

    // Set bypass mode for the magnetometers
    if (!i2c_write_byte(MPU9250_ADDRESS,0x37,0x02)) return 0;       // there are two chips in the IC. This setting affects, how they interact.
     
    // Request continuous magnetometer measurements in 16 bits
    if (!i2c_write_byte(MAG_ADDRESS,0x0A,0x16)) return 0;
    return 1;                                                       // initialization successfull
 //  }
}

// READ ACCELEROMETER AND CALCULATE ELEVATION
void acceleration()
{ 

  i2c_start_read_burst(MPU9250_ADDRESS,0x3B);                                                               //start reading in burst mode
   
  // Accelerometer
  /*old orientation of the compass chip
  int16_t ax=i2c_read(0) << 8 | i2c_read(0);
  i2c_read(0);i2c_read(0); //discard ay
  int16_t az=i2c_read(0) << 8 | i2c_read(0);
  */ 
  i2c_read(0);i2c_read(0); //discard ay (actually now ax in the present orientation of the chip)

  /* for controlbox in the housing
  int16_t ax=-(i2c_read(0) << 8 | i2c_read(0));                                  U                           // acceleration in x-direction. Create 2 byte integer from stored bytes
  int16_t az=-(i2c_read(0) << 8 | i2c_read(0));                                                             // acceleration in z-direction
  */

  // for test setup
  int16_t ax=(i2c_read(0) << 8 | i2c_read(0));                                                             // acceleration in x-direction. Create 2 byte integer from stored bytes
  int16_t az=(i2c_read(0) << 8 | i2c_read(0));                                                             // acceleration in z-direction
  
  temperature = i2c_read(0) << 8 | i2c_read(1);                                                             // temperature reading from temperature correction, presently not used
  i2c_stop();                                                                                               // burst finished
  
  elevation_compass=(atan((float)az/ax)-PI/2)+PI/2;                                                         // calculate tilt (elevation)
  if (elevation_compass<-0.3) elevation_compass += PI;                                                      // make sure that the elevation is in the 0..Pi range
  elevation_compass *= 1800 / PI;                                                                           // calculates in 0.1 degree steps to be compatible with integer 
  elevation_compass -= ((ERI(EA_el_up_limit)-elevation_compass)*ERI(EA_el_down_corr) + (elevation_compass-ERI(EA_el_down_limit))*ERI(EA_el_up_corr))/(ERI(EA_el_up_limit)-ERI(EA_el_down_limit));
    // apply corrections which are set in the calibration routine



//  Serial.print(F(" El=")); Serial.print(elevation_compass);
}

// READ MAGNETOMETER AND CALCULATE COMPASS READING ( AZIMUTH )
void magnetometer()
{
  int mx_old,my_old,mz_old,mx_new,my_new,mz_new;
  mx_raw=0; my_raw=0; mz_raw=0;
  for (int i=0; i<20; i++)                                                                                  // 20x oversampling
  {

    while (!(i2c_read_byte(MAG_ADDRESS,0x02) &0x01  )){};                                                   // &0x01 masks last bit in ST1. Last bit is data ready. 
      
    i2c_start_read_burst(MAG_ADDRESS,0x03);                                                                 //read seven bytes from 0x03 register
     
    
     
    // Magnetometer
    /* old arrangement, change to y=-x, x=-y, z=-z
    my_raw+=-(i2c_read(0) | i2c_read(0) << 8);
    mx_raw+=-(i2c_read(0) | i2c_read(0) << 8);  
    mz_raw+=(i2c_read(0) | i2c_read(0) << 8);   
    */

    /* for controlbox in the housing. 
    mx_raw+=(i2c_read(0) | i2c_read(0) << 8);                                                               // read 3 axis magnetometer values
    my_raw+=(i2c_read(0) | i2c_read(0) << 8);                                                               // create 16 bits integers from 8 bits data  
    mz_raw-=(i2c_read(0) | i2c_read(0) << 8);                                                               // change sign for z-axis
    */
    
    // for test setup, y-arrow on MPU9250 points up in morning position. xyz readable from top. Negate x and z for mounting on tracker.
    mx_new=-(i2c_read(0) | i2c_read(0) << 8);                                                               // read 3 axis magnetometer values
    my_new=(i2c_read(0) | i2c_read(0) << 8);                                                               // create 16 bits integers from 8 bits data  
    mz_new=(i2c_read(0) | i2c_read(0) << 8);                                                               // change sign for z-axis
    
    i2c_read(1);                                                                                            // read last byte
    i2c_stop(); 

    if ( (i % 2 == 1) & ((abs(mx_new-mx_old)>100) | (abs(my_new-my_old)>100) | (abs(mz_new-mz_old)>100))){ // check for unrealistic differences and discard last two values if necessary
      i-=2; // Serial.println("bad"); 
    }
    else {
      mx_raw += mx_old + mx_new;
      my_raw += my_old + my_new;
      mz_raw += mz_old + mz_new;                                                                                               
    }
    mx_old = mx_new;
    my_old = my_new;
    mz_old = mz_new;
  }
  mx_raw /= 10;    // divide by 10 to make sure that a drift of the zero positions will not result in an integer overflow at +/-32k
  my_raw /= 10;
  mz_raw /= 10;
      
  if ((calibration_step>8)&(calibration_step<21)){
    mx_raw += temporary_mx_neutral;
    mz_raw += temporary_mz_neutral; // corrections used in the calibration routine. Will be saved to EEPROM at the end of the calibration routine.
  }
  else {
    mx_raw += ERI(EA_mx_neutral);                                                                             // user defined shift of neutral positions to correct for high offset values
    mz_raw += ERI(EA_mz_neutral);
  }
  compass_calculations();                                                                                   // calculate the compass reading (AZIMUTH)
}

// MAIN ROUTINE FOR COMPASS CALCULATIONS AND CORRECTIONS
void compass_calculations(){

  elevation1_position_float = (elevation_compass - float(ERI(EA_el_down_limit)))/(float(ERI(EA_el_up_limit)-ERI(EA_el_down_limit)))*(2*ERI(EA_el_steps));  //calculate an elevation position
  if (elevation1_position_float < 0) {elevation1_position_float = 0.01;    }      // slightly above (2*ERI(EA_el_steps)) or below 0 results in reading undesired positions in EEPROM --> correct
  if (elevation1_position_float > (2*ERI(EA_el_steps))) {elevation1_position_float = (2*ERI(EA_el_steps))-0.01;  }
  elevation1_position = int(elevation1_position_float);                           // int cuts off anything after the decimal point
  elevation1_position_float -= float(elevation1_position);                        // what has been cut off. This is important for the interpolation between defined elevation positions

  // 
  /*
   * DISABLED but may be helpful in some special cases: load west readings for present elevation from EEPROM, interpolate
   * HOW TO CORRECT THE WEST POSITION FOR KNOWN ANGULAR DEVIATIONS FROM CALIBRATION
   * we have stored the west readings of the magnetic components my and mx_horizontal in EEPROM during the multiple passes in calibration_step == 9. 
   * In calibration_step == 20 angular corrections in west directions were calculated and stored to EEPROM-positions 752ff
   * These are restored now in order to calculate an angular correction of the Azimuth for the present elevation position. 
   * West should be at 270° = 2700*0.1°. Therefore we calculate the difference of the calculated compass position to 2700.
   * We want to correct, so we take the negative of this value and store it into the the variable azimuth compass. The actual reading will be added to this value.
  
  azimuth_compass = 0 -(ERI(752+2*elevation1_position)*elevation1_position_float + ERI(750+2*elevation1_position)*(1-elevation1_position_float));     

  */

  azimuth_compass = 0;                                          // preset                                                               
  
  // present readings for first pass
  mx = mx_raw;
  my = my_raw;
  mz = mz_raw;
  calc_mx_horizontal();                                         // calculate horizontal component in x-direction with all errors

  azimuth_compass += compass_calculation_2();                   // get compass angle (azimuth) with present magnetometer readings
  azimuth_compass -= ERI(EA_north_corr);                        // user defined compass correction angle
  correct_azimuth_compass_range_to_0_3600();                    // make the compass reading unambiguous

  while (azimuth_compass - prev_azimuth_compass < -1799){azimuth_compass += 3600;}  // supress jumps, count up or down continuously
  while (azimuth_compass - prev_azimuth_compass > 1799){azimuth_compass -= 3600;}   
  
  if ((azimuth_compass < 700)&(ERI(EA_azimuth_left_of_east) < 1)) {EWI(EA_azimuth_left_of_east,1);Serial.println(F("to1"));}  // remember left of east azimuth values in case of power failure
  if ((azimuth_compass > 1100)&(ERI(EA_azimuth_left_of_east) > 0)) {EWI(EA_azimuth_left_of_east,0);Serial.println(F("to0"));}
  
  if(print_compass) {Serial.print(F("AzC=")); Serial.print(azimuth_compass); Serial.print(F(" "));}                           // user enabled printing of compass values 
  if (averaged_azimuth_compass == 10000){averaged_azimuth_compass = azimuth_compass;}                                         // handle first pass through this routine by a preset of 10000 in the header

  // Smooth azimuth compass readings. Add or subtract fullspeed Azimuth movements. Use the Azimuth speed 1/(ms per degree) measured during the calibration
  else if (fabs(azimuth_compass-averaged_azimuth_compass)<100)                                                                // smooth if the deviations are smaller than 10 degree, use moving average
       {averaged_azimuth_compass = 0.1*azimuth_compass +0.9*(averaged_azimuth_compass + float((millis()-prevmil_[0]))/(float(ERI(EA_azimuth_rotation_msdeg))/10)*(!(move_pwm_state == 0))*(move_step == 0)*((move_direction==3)-(move_direction==2)));}
  else {averaged_azimuth_compass = azimuth_compass;}                                                                          // use azimuth compass for deviations >10 degree
  azimuth_compass = averaged_azimuth_compass;
  prevmil_[0] = millis();                                                                                                     // remember system time to calculate expected angular step size in next pass for averaging
 
  if (first_pass == 0) prev_azimuth_compass = azimuth_compass;                                                                // workaround for wrong Azimuth readings after upload of code
  if(print_compass) {Serial.print(F("\tAzC2=")); Serial.print(azimuth_compass);}
  first_pass = 0;
}

// CALCULATE THE COMPASS AZIMUTH ANGLE
float compass_calculation_2(){
  
  // correct my and mx_horizontal values with calibration results
  my -= ERI(2*elevation1_position + 350 + 2)*elevation1_position_float + ERI(2*elevation1_position + 350 )*(1.0-elevation1_position_float);             // subtract interpolated neutral position
  my /= ERI(2*elevation1_position + 400 + 2)*elevation1_position_float + ERI(2*elevation1_position + 400 )*(1.0-elevation1_position_float);             // divide by interpolated range
  mx_horizontal -=  ERI(2*elevation1_position + 250 + 2)*elevation1_position_float + ERI(2*elevation1_position + 250 )*(1.0-elevation1_position_float); // subtract interpolated neutral position
  mx_horizontal /=  ERI(2*elevation1_position + 300 + 2)*elevation1_position_float + ERI(2*elevation1_position + 300 )*(1.0-elevation1_position_float); // divide by interpolated range

  
  float azimuth_compass_2 = atan(my/mx_horizontal);         // calculate the Azimuth compass reading
  
  if (mx_horizontal<0) {azimuth_compass_2 +=PI;}            // make the reading unambiguous, otherwise chaning signs on both components would yield the same result. -0.5Pi..´1.5Pi range
  
  azimuth_compass_2 *= 1800 / PI;                           // calculates in 0.1 degree steps to be compatible with integer
  return azimuth_compass_2;
}

// CORRECT THE COMPASS RANGE TO 0..3600 degree
void correct_azimuth_compass_range_to_0_3600() {
  while ((azimuth_compass > 3600)|(azimuth_compass < 0)){
    if (azimuth_compass > 3600){azimuth_compass -= 3600;}
    if (azimuth_compass < 0){azimuth_compass += 3600;}
  }
}

// PROJECT THE MAGNETIC VECTOR ONTO A HORIZONTAL PLANE (my is allready horizontal)
void calc_mx_horizontal(){
  elevation_compass *= PI/1800;                                             // we need elevation_compass in the correct format
  mx_horizontal=(mx*sin(elevation_compass) - mz*cos(elevation_compass));    // This is a simple projection making use of the elevation angle
  elevation_compass *= 1800/PI;
}







// RECEIVE TIME FROM RTC AND CALCULATE FRACTIONAL DAY AND FRACTIONAL YEAR
// The first 3 bytes seconds, minutes, hours are used to  calculate a fractional day in the range [0..1]
// The third byte is the day of the week and discarded
// The following 3 bytes are used to estimate the number that have passed in the present year and calculate a fractional year [0..1]


void RTC_receive(){
  
  i2c_start_read_burst(RTC_ADDRESS,0x00);  
  received = BCD2DEC(i2c_read(0));        //  seconds = received;
  
  frac_day = received/86400;  

  received = BCD2DEC(i2c_read(0));        //  minutes = received;
  frac_day += received/1440;  
  
  received = BCD2DEC(i2c_read(0));        //  hours = received;
  if (prev_hour - received == 23){        // detect new day 
    EWI(EA_operation_days,ERI(EA_operation_days)+1);                                                              // increase operation day counter
    EWI(EA_days_since_cal,ERI(EA_days_since_cal)+1);                                                              // increase operation days since last calibration
 
    Serial.print(F("cal_days")); Serial.print(ERI(EA_days_since_cal));
  }              

  prev_hour=received;
  frac_day += received/24;  

  i2c_read(0); // discard day of week

  received = BCD2DEC(i2c_read(0));                              //  days = received;
  frac_year = received - 0.5;    

  
  received = BCD2DEC(i2c_read(0));                              //  months = received;
  i2c_read(1);
  i2c_stop();
  frac_year += 30.55*(received - 1) - 2 * (received > 2.5);     // correcting for the short February. We can live with the different lengths of the other months.
  frac_year /= 365; //!!Saheli delete, replace frac_year by days_in_year
}


// CONVERT BCD TO DECIMAL FORMAT
byte BCD2DEC(byte received_){
  return byte((received_) & B00001111) + byte(10 * ((received_) & B01110000)>>4); 
}

// CONVERT DECIMAL FORMAT TO BINARY CODE DECIMAL BCD
byte DEC2BCD(byte dec_){
  return (((dec_ / 10) << 4) + (dec_ % 10));
}

// START READING A BURST FROM I2C BUS
void i2c_start_read_burst(byte I2C_7BITADDR_, byte MEMLOC_){
  if (!i2c_start((I2C_7BITADDR_<<1)|I2C_WRITE)) { // start transfer
    Serial.print(I2C_7BITADDR_);  Serial.println(F(" busy"));
    return;
  }
  i2c_write(MEMLOC_);
  i2c_rep_start((I2C_7BITADDR_<<1)|I2C_READ); // restart for reading
}

// WRITE A SINGLE BYTE TO I2C BUS
bool i2c_write_byte(byte I2C_7BITADDR__, byte MEMLOC__, int byte2write__){
  i2c_start(I2C_7BITADDR__ << 1 | I2C_WRITE);
  i2c_write(MEMLOC__);
  bool success__ = i2c_write(byte2write__);
  i2c_stop();
  return success__;
}

// READ A SINGLE BYTE FROM I2C BUS
byte i2c_read_byte(byte I2C_7BITADDR__, byte MEMLOC__){
  i2c_start_read_burst(I2C_7BITADDR__, MEMLOC__);
  byte received_byte = i2c_read(1); // send NAK to stop transmission
  i2c_stop();
  return received_byte;
}