AP_EFI: make C_TO_KELVIN a function macro; create KELVIN_TO_C

These are in celsius
This commit is contained in:
Peter Barker 2022-01-12 23:03:24 +11:00 committed by Peter Barker
parent d028a6efc3
commit c86ebc7ffb
2 changed files with 6 additions and 6 deletions

View File

@ -225,8 +225,8 @@ void AP_EFI::send_mavlink_status(mavlink_channel_t chan)
state.spark_dwell_time_ms,
state.atmospheric_pressure_kpa,
state.intake_manifold_pressure_kpa,
(state.intake_manifold_temperature - C_TO_KELVIN),
(state.cylinder_status[0].cylinder_head_temperature - C_TO_KELVIN),
KELVIN_TO_C(state.intake_manifold_temperature),
KELVIN_TO_C(state.cylinder_status[0].cylinder_head_temperature),
state.cylinder_status[0].ignition_timing_deg,
state.cylinder_status[0].injection_time_ms,
0, 0, 0);

View File

@ -86,12 +86,12 @@ void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame)
memcpy(&data, frame.data, sizeof(data));
switch((NWPMU_TEMPERATURE_TYPE)data.temp_type) {
case NWPMU_TEMPERATURE_TYPE::C:
internal_state.coolant_temperature = data.coolant_temp * 0.1f + C_TO_KELVIN;
internal_state.cylinder_status[0].cylinder_head_temperature = data.coolant_temp * 0.1f + C_TO_KELVIN;
internal_state.coolant_temperature = C_TO_KELVIN(data.coolant_temp * 0.1f);
internal_state.cylinder_status[0].cylinder_head_temperature = C_TO_KELVIN(data.coolant_temp * 0.1f);
break;
case NWPMU_TEMPERATURE_TYPE::F:
internal_state.coolant_temperature = ((data.coolant_temp * 0.1f) - 32 * 5/9) + C_TO_KELVIN;
internal_state.cylinder_status[0].cylinder_head_temperature = ((data.coolant_temp * 0.1f) - 32 * 5/9) + C_TO_KELVIN;
internal_state.coolant_temperature = C_TO_KELVIN(((data.coolant_temp * 0.1f) - 32 * 5/9));
internal_state.cylinder_status[0].cylinder_head_temperature = C_TO_KELVIN(((data.coolant_temp * 0.1f) - 32 * 5/9));
break;
default:
break;