diff --git a/libraries/SITL/SIM_Airspeed_DLVR.cpp b/libraries/SITL/SIM_Airspeed_DLVR.cpp index 555d39de65..4cae7047a4 100644 --- a/libraries/SITL/SIM_Airspeed_DLVR.cpp +++ b/libraries/SITL/SIM_Airspeed_DLVR.cpp @@ -69,5 +69,5 @@ void SITL::Airspeed_DLVR::update(const class Aircraft &aircraft) AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta); // To Do: Add a sensor board temperature offset parameter - temperature = (SSL_AIR_TEMPERATURE * theta - C_TO_KELVIN) + 25.0; + temperature = (KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta)) + 25.0; } diff --git a/libraries/SITL/SIM_BattMonitor_SMBus.cpp b/libraries/SITL/SIM_BattMonitor_SMBus.cpp index 24d4b527ef..418318f672 100644 --- a/libraries/SITL/SIM_BattMonitor_SMBus.cpp +++ b/libraries/SITL/SIM_BattMonitor_SMBus.cpp @@ -17,7 +17,7 @@ SITL::SIM_BattMonitor_SMBus::SIM_BattMonitor_SMBus() : add_block("Device Name", SMBusBattDevReg::DEVICE_NAME, SITL::I2CRegisters::RegMode::RDONLY); add_register("Manufacture Data", SMBusBattDevReg::MANUFACTURE_DATA, SITL::I2CRegisters::RegMode::RDONLY); - set_register(SMBusBattDevReg::TEMP, (int16_t)((15 + C_TO_KELVIN)*10)); + set_register(SMBusBattDevReg::TEMP, (int16_t)((C_TO_KELVIN(15))*10)); // see update for voltage // see update for current // TODO: remaining capacity diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index d8df693f21..45b736fb97 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -319,7 +319,7 @@ float Frame::get_air_density(float alt_amsl) const AP_Baro::SimpleAtmosphere(alt_amsl * 0.001f, sigma, delta, theta); const float air_pressure = SSL_AIR_PRESSURE * delta; - return air_pressure / (ISA_GAS_CONSTANT * (C_TO_KELVIN + model.refTempC)); + return air_pressure / (ISA_GAS_CONSTANT * (C_TO_KELVIN(model.refTempC))); } /* diff --git a/libraries/SITL/SIM_MS5525.cpp b/libraries/SITL/SIM_MS5525.cpp index c0178577e0..2dd4cb46fb 100644 --- a/libraries/SITL/SIM_MS5525.cpp +++ b/libraries/SITL/SIM_MS5525.cpp @@ -72,6 +72,6 @@ void MS5525::get_pressure_temperature_readings(float &P_Pa, float &Temp_C) AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta); // To Do: Add a sensor board temperature offset parameter - Temp_C = (SSL_AIR_TEMPERATURE * theta - C_TO_KELVIN) + 25.0; + Temp_C = (KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta)) + 25.0; P_Pa = AP::sitl()->state.airspeed_raw_pressure[0]; } diff --git a/libraries/SITL/SIM_MS5611.cpp b/libraries/SITL/SIM_MS5611.cpp index 3a56217584..a1957df17d 100644 --- a/libraries/SITL/SIM_MS5611.cpp +++ b/libraries/SITL/SIM_MS5611.cpp @@ -114,7 +114,7 @@ void MS5611::get_pressure_temperature_readings(float &P_Pa, float &Temp_C) AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta); P_Pa = SSL_AIR_PRESSURE * delta; - Temp_C = (SSL_AIR_TEMPERATURE * theta - C_TO_KELVIN) + AP::sitl()->temp_board_offset; + Temp_C = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta) + AP::sitl()->temp_board_offset; // TO DO add in temperature adjustment by inheritting from AP_Baro_SITL_Generic? // AP_Baro_SITL::temperature_adjustment(P_Pa, Temp_C); diff --git a/libraries/SITL/SIM_SerialRangeFinder.cpp b/libraries/SITL/SIM_SerialRangeFinder.cpp index 7111550d58..9441657fec 100644 --- a/libraries/SITL/SIM_SerialRangeFinder.cpp +++ b/libraries/SITL/SIM_SerialRangeFinder.cpp @@ -48,7 +48,7 @@ void SerialRangeFinder::send_temperature() // Use the simple underwater model to get temperature float rho, delta, theta; AP_Baro::SimpleUnderWaterAtmosphere(-0.5 * 0.001, rho, delta, theta); // get simulated temperature for 0.5m depth - const float temperature = Aircraft::rand_normal(SSL_AIR_TEMPERATURE * theta - C_TO_KELVIN, 1); // FIXME pick a stddev based on data sheet + const float temperature = Aircraft::rand_normal(KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta), 1); // FIXME pick a stddev based on data sheet uint8_t data[255]; const uint32_t packetlen = packet_for_temperature(temperature,