You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
127 lines
4.5 KiB
127 lines
4.5 KiB
/* |
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
*/ |
|
|
|
#include <AP_HAL/AP_HAL.h> |
|
|
|
#include <AP_Common/AP_Common.h> |
|
#include <AP_HAL/utility/sparse-endian.h> |
|
#include <AP_Math/AP_Math.h> |
|
|
|
#include "AP_EFI_NWPMU.h" |
|
|
|
#if HAL_EFI_NWPWU_ENABLED |
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
AP_EFI_NWPMU::AP_EFI_NWPMU(AP_EFI &_frontend) : |
|
CANSensor("NWPMU", AP_CANManager::Driver_Type_EFI_NWPMU), |
|
AP_EFI_Backend(_frontend) |
|
{ |
|
} |
|
|
|
void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame) |
|
{ |
|
const uint32_t id = frame.id & AP_HAL::CANFrame::MaskExtID; |
|
|
|
WITH_SEMAPHORE(sem); |
|
|
|
switch ((NWPMU_ID)id) { |
|
case NWPMU_ID::ECU_1: { |
|
internal_state.last_updated_ms = AP_HAL::millis(); |
|
struct ecu_1 *data = (struct ecu_1 *)frame.data; |
|
internal_state.engine_speed_rpm = data->rpm; |
|
internal_state.throttle_position_percent = data->tps * 0.1f; |
|
internal_state.cylinder_status[0].ignition_timing_deg = data->ignition_angle * 0.1f; |
|
break; |
|
} |
|
|
|
case NWPMU_ID::ECU_2: { |
|
internal_state.last_updated_ms = AP_HAL::millis(); |
|
struct ecu_2 *data = (struct ecu_2 *)frame.data; |
|
switch ((NWPMU_PRESSURE_TYPE)data->pressure_type) { |
|
case NWPMU_PRESSURE_TYPE::kPa: |
|
internal_state.atmospheric_pressure_kpa = data->baro * 0.01f; |
|
internal_state.intake_manifold_pressure_kpa = data->baro * 0.01f; |
|
break; |
|
case NWPMU_PRESSURE_TYPE::psi: |
|
internal_state.atmospheric_pressure_kpa = data->baro * 0.0689476f; |
|
internal_state.intake_manifold_pressure_kpa = data->baro * 0.0689476f; |
|
break; |
|
default: |
|
break; |
|
} |
|
internal_state.cylinder_status[0].lambda_coefficient = data->lambda * 0.01f; |
|
break; |
|
} |
|
|
|
case NWPMU_ID::ECU_4: { |
|
internal_state.last_updated_ms = AP_HAL::millis(); |
|
struct ecu_4 *data = (struct ecu_4 *)frame.data; |
|
// remap the analog input for fuel pressure, 0.5 V == 0 PSI, 4.5V == 100 PSI |
|
internal_state.fuel_pressure = linear_interpolate(0, 689.476, |
|
data->analog_fuel_pres * 0.001, |
|
0.5f,4.5f); |
|
break; |
|
} |
|
|
|
case NWPMU_ID::ECU_5: { |
|
internal_state.last_updated_ms = AP_HAL::millis(); |
|
struct ecu_5 *data = (struct ecu_5 *)frame.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; |
|
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; |
|
break; |
|
default: |
|
break; |
|
} |
|
break; |
|
} |
|
|
|
case NWPMU_ID::ECU_6: { |
|
internal_state.last_updated_ms = AP_HAL::millis(); |
|
struct ecu_6 *data = (struct ecu_6 *)frame.data; |
|
if (!_emitted_version && (AP_HAL::millis() > 10000)) { // don't emit a version early in the boot process |
|
gcs().send_text(MAV_SEVERITY_INFO, "NWPMU Version: %d.%d.%d", |
|
data->firmware_major, |
|
data->firmware_minor, |
|
data->firmware_build); |
|
_emitted_version = true; |
|
} |
|
break; |
|
} |
|
case NWPMU_ID::GCU: |
|
case NWPMU_ID::ECU_3: |
|
case NWPMU_ID::ECU_7: |
|
case NWPMU_ID::ECU_8: |
|
case NWPMU_ID::ECU_9: |
|
case NWPMU_ID::ECU_10: |
|
case NWPMU_ID::ECU_11: |
|
case NWPMU_ID::ECU_12: |
|
break; |
|
} |
|
} |
|
|
|
void AP_EFI_NWPMU::update() |
|
{ |
|
// copy the data to the front end |
|
copy_to_frontend(); |
|
} |
|
|
|
#endif // HAL_EFI_NWPWU_ENABLED
|
|
|