mirror of https://github.com/ArduPilot/ardupilot
129 lines
4.5 KiB
C++
129 lines
4.5 KiB
C++
/*
|
|
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_EFI_Backend(_frontend)
|
|
{
|
|
register_driver(AP_CANManager::Driver_Type_EFI_NWPMU);
|
|
}
|
|
|
|
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
|