AAU CANSAT - hw-290 imu board

//JDN  april 2022
//
#include <Wire.h>

// mpu6050
#include <MPU6050.h>

// uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated
// list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read,
// not so easy to parse, and slow(er) over UART.
#define OUTPUT_READABLE_ACCELGYRO

// uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit
// binary, one right after the other. This is very fast (as fast as possible
// without compression or data loss), and easy to parse, but impossible to read
// for a human.
//#define OUTPUT_BINARY_ACCELGYRO

// bmp085 tryk sensor
#include <bmp085.h>

// qmc5883
#include <DFRobot_QMC5883.h>



MPU6050 mpu; // variable for mpu6050
int16_t ax, ay, az;
int16_t gx, gy, gz;


DFRobot_QMC5883 compass; // and for qmc5883

void setup() {
  Serial.begin(115200);
  Wire.begin();

  // bmp085
  bmp085Init(0x77, 101300.0); //dagens luftryk

  // mpu6050
  initMPU6050();

  // hmc5883 / qmc5883
  initHMC5883QMC5883();
}

void loop() {
  bmp085Read();

  readCompass();

  readMPU6050();

  Serial.println();
  delay(10);
}

//////////////////////////////////////////

void initMPU6050()
{
  mpu.initialize();
  mpu.setI2CBypassEnabled(true); // bq  qmc5883 is behind mpu6050
  delay(200);
  mpu.initialize();
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  /*
   * du kan ændre range eks gyro til max 2000 gr/sek:

     setFullScaleGyroRange(MPU6050_GYRO_FS_2000);

     2000 degr/sek udlsæes som 32767 eller -32768 afh af rotationsretning
     hint: måling er et 16 bit heltal dvs +- 2^15 er max værdier


    void setFullScaleGyroRange(uint8_t range);

    #define MPU6050_GYRO_FS_250         0x00
    #define MPU6050_GYRO_FS_500         0x01
    #define MPU6050_GYRO_FS_1000        0x02
    #define MPU6050_GYRO_FS_2000        0x03

    Det samme for accelerometer:

    FS_2  er 2g osv
    void setFullScaleAccelRange(uint8_t range);

    #define MPU6050_ACCEL_FS_2          0x00
    #define MPU6050_ACCEL_FS_4          0x01
    #define MPU6050_ACCEL_FS_8          0x02
    #define MPU6050_ACCEL_FS_16         0x03
  */
}

void readMPU6050()
{
  // read raw accel/gyro measurements from device
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // these methods (and a few others) are also available
  //accelgyro.getAcceleration(&ax, &ay, &az);
  //accelgyro.getRotation(&gx, &gy, &gz);

#ifdef OUTPUT_READABLE_ACCELGYRO
  // display tab-separated accel/gyro x/y/z values
  Serial.print("a/g(+-32768 == max range)\t");
  Serial.print(ax); Serial.print("\t");
  Serial.print(ay); Serial.print("\t");
  Serial.print(az); Serial.print("\t");
  Serial.print(gx); Serial.print("\t");
  Serial.print(gy); Serial.print("\t");
  Serial.print(gz);
#endif

#ifdef OUTPUT_BINARY_ACCELGYRO
  Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF));
  Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF));
  Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF));
  Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF));
  Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF));
  Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF));
#endif

}

void initHMC5883QMC5883()
{
  while (!compass.begin())
  {
    Serial.println("Could not find a valid QMC5883 sensor, check wiring!");
    delay(500);
  }

  if (compass.isHMC()) {
    Serial.println("Initialize HMC5883");
    compass.setRange(HMC5883L_RANGE_1_3GA);
    compass.setMeasurementMode(HMC5883L_CONTINOUS);
    compass.setDataRate(HMC5883L_DATARATE_15HZ);
    compass.setSamples(HMC5883L_SAMPLES_8);
  }
  else if (compass.isQMC()) {
    Serial.println("Initialize QMC5883");
    compass.setRange(QMC5883_RANGE_2GA);
    compass.setMeasurementMode(QMC5883_CONTINOUS);
    compass.setDataRate(QMC5883_DATARATE_50HZ);
    compass.setSamples(QMC5883_SAMPLES_8);
  }
}

void bmp085Read()
{
  float tryk, temp, hojde;
  bmp085Measure(&temp, &tryk, &hojde);

  Serial.print(temp);  Serial.print("[C]\t");
  Serial.print(tryk); Serial.print("[Pascal]\t");
  Serial.print(hojde); Serial.print("[m]\t");
}

void readCompass()
{
  Vector norm = compass.readNormalize();

  // Calculate heading
  float heading = atan2(norm.YAxis, norm.XAxis);

  // Set declination angle on your location and fix heading
  // You can find your declination on: http://magnetic-declination.com/
  // (+) Positive or (-) for negative
  // For Bytom / Poland declination angle is 4'26E (positive)
  // Formula: (deg + (min / 60.0)) / (180 / M_PI);
  float declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / PI);
  heading += declinationAngle;

  // Correct for heading < 0deg and heading > 360deg
  if (heading < 0) {
    heading += 2 * PI;
  }

  if (heading > 2 * PI) {
    heading -= 2 * PI;
  }

  // Convert to degrees
  float headingDegrees = heading * 180 / M_PI;

  // Output
  Serial.print(" Heading = ");
  Serial.print(heading);
  Serial.print("[rad]\t ");
  Serial.print(headingDegrees);
  Serial.print("[degr]\t");
}