mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 07:28:29 -04:00
AP_EFI: added fuel pressure
This commit is contained in:
parent
13f033ac92
commit
8c335be045
@ -260,6 +260,15 @@ void AP_EFI::send_mavlink_status(mavlink_channel_t chan)
|
|||||||
ignition_voltage = state.ignition_voltage;
|
ignition_voltage = state.ignition_voltage;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// If fuel pressure is supported, but is exactly zero, shift it to 0.0001
|
||||||
|
// to indicate that it is supported.
|
||||||
|
float fuel_pressure = state.fuel_pressure;
|
||||||
|
#pragma GCC diagnostic push
|
||||||
|
#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as we only care about exact zero here
|
||||||
|
if (fuel_pressure == 0.0 && state.fuel_pressure_status != Fuel_Pressure_Status::NOT_SUPPORTED) {
|
||||||
|
fuel_pressure = 0.0001;
|
||||||
|
}
|
||||||
|
#pragma GCC diagnostic pop
|
||||||
mavlink_msg_efi_status_send(
|
mavlink_msg_efi_status_send(
|
||||||
chan,
|
chan,
|
||||||
AP_EFI::is_healthy(),
|
AP_EFI::is_healthy(),
|
||||||
@ -279,7 +288,8 @@ void AP_EFI::send_mavlink_status(mavlink_channel_t chan)
|
|||||||
KELVIN_TO_C(state.cylinder_status.exhaust_gas_temperature),
|
KELVIN_TO_C(state.cylinder_status.exhaust_gas_temperature),
|
||||||
state.throttle_out,
|
state.throttle_out,
|
||||||
state.pt_compensation,
|
state.pt_compensation,
|
||||||
ignition_voltage
|
ignition_voltage,
|
||||||
|
fuel_pressure
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user