Arduino for STM32

Code: Select all

void getAbosluteAltitudeFromBaro()
{
  /* Description : This functions Gets one altitude data from baro*/

  if (barometer_status != ACTIVE)
    return;

  Wire2.beginTransmission(0x76);
  Wire2.write(0x08);
  if (Wire2.endTransmission() != 0)
  {
    barometer_status = NOTPRESENT;
    return;
  }

  Wire2.requestFrom(0x76, 1);
  uint8_t meas_cfg;
  while (Wire2.available() > 0)
  {
    meas_cfg = Wire2.read();
  }

  if ((meas_cfg & 0b10000000) == 0b10000000)
  {
    // read cofficients
    Wire2.beginTransmission(0x76);
    Wire2.write(0x10);
    Wire2.endTransmission();
    Wire2.requestFrom(0x76, 18); // request pressure and temp data
    byte i = 0;

    while (Wire2.available())
    {
      if (i >= 18)
      {
        // error
        Serial.println("ERRor in getting baro reading");
        barometer_status = NOTPRESENT;
        break;
      }
      buffer_cofficients[i++] = Wire2.read();
    }

    c0 = (buffer_cofficients[0] & 0x80 ? 0xFF : 0) << 12 | buffer_cofficients[0] << 4 | buffer_cofficients[1] >> 4; // was 12 bit so had to extend msb manually
    c1 = (buffer_cofficients[1] & 0x08 ? 0xFF : 0) << 12 | (buffer_cofficients[1] & 0x0f) << 8 | buffer_cofficients[2];
    c00 = (buffer_cofficients[3] & 0x80 ? 0xFFFF : 0) << 20 | buffer_cofficients[3] << 12 | buffer_cofficients[4] << 4 | buffer_cofficients[5] >> 4; // was 20 bit so had to extend msb manually
    c10 = (buffer_cofficients[5] & 0x08 ? 0xFFFF : 0) << 20 | (buffer_cofficients[5] & 0x0f) << 16 | buffer_cofficients[6] << 8 | buffer_cofficients[7];

    c01 = buffer_cofficients[8] << 8 | buffer_cofficients[9];
    c11 = buffer_cofficients[10] << 8 | buffer_cofficients[11];
    c20 = buffer_cofficients[12] << 8 | buffer_cofficients[13];
    c21 = buffer_cofficients[14] << 8 | buffer_cofficients[15];
    c30 = buffer_cofficients[16] << 8 | buffer_cofficients[17];
  }

  if ((meas_cfg & 0b00010000) == 0b00010000)
  {
    // new pressure data available;
    // read temperature then only

    Wire2.beginTransmission(0x76);
    Wire2.write(0x00);
    Wire2.endTransmission();
    Wire2.requestFrom(0x76, 6); // request pressure and temp data
    byte i = 0;

    while (Wire2.available())
    {
      if (i >= 6)
      {
        // error
        Serial.println("ERRor in getting baro reading");
        barometer_status = NOTPRESENT;
        break;
      }
      buffer_cofficients[i++] = Wire2.read();
    }

    pressure = int32_t((buffer_cofficients[0] & 0x80 ? 0xFF : 0) << 24 | (buffer_cofficients[0] << 16 | buffer_cofficients[1] << 8 | buffer_cofficients[2])); // was 24 bit so had to add msb // also it is unsigned so before converting to float have to convert to int
    temperature = int32_t((buffer_cofficients[3] & 0x80 ? 0xFF : 0) << 24 | (buffer_cofficients[3] << 16 | buffer_cofficients[4] << 8 | buffer_cofficients[5]));
    pressure = pressure / float(K_pressure);
    temperature = temperature / float(K_temperature);
    pressure = double(c00) + pressure * (double(c10) + pressure * (double(c20) + pressure * double(c30))) + temperature * double(c01) + temperature * pressure * (double(c11) + pressure * double(c21));
    temperature = double(c0) * 0.5 + double(c1) * temperature;
  }

  altitude_from_baro = 44330 * (1.0 - pow(pressure / 100.0 / 1013.25, 0.1903));
  // get data from baro
}