Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include "AP_EFI_config.h"
      17             : 
      18             : #if AP_EFI_NWPWU_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : 
      22             : #include <AP_Common/AP_Common.h>
      23             : #include <AP_HAL/utility/sparse-endian.h>
      24             : #include <AP_Math/AP_Math.h>
      25             : #include <GCS_MAVLink/GCS.h>
      26             : 
      27             : #include "AP_EFI_NWPMU.h"
      28             : 
      29             : extern const AP_HAL::HAL& hal;
      30             : 
      31           0 : AP_EFI_NWPMU::AP_EFI_NWPMU(AP_EFI &_frontend) :
      32             :     CANSensor("NWPMU"),
      33           0 :     AP_EFI_Backend(_frontend)
      34             : {
      35           0 :     register_driver(AP_CAN::Protocol::EFI_NWPMU);
      36           0 : }
      37             : 
      38           0 : void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame)
      39             : {
      40           0 :     const uint32_t id =  frame.id & AP_HAL::CANFrame::MaskExtID;
      41             : 
      42           0 :     WITH_SEMAPHORE(get_sem());
      43             : 
      44           0 :     switch ((NWPMU_ID)id) {
      45           0 :     case NWPMU_ID::ECU_1: {
      46           0 :         internal_state.last_updated_ms = AP_HAL::millis();
      47             :         struct ecu_1 data;
      48           0 :         memcpy(&data, frame.data, sizeof(data));
      49           0 :         internal_state.engine_speed_rpm = data.rpm;
      50           0 :         internal_state.throttle_position_percent = data.tps * 0.1f;
      51           0 :         internal_state.cylinder_status.ignition_timing_deg = data.ignition_angle * 0.1f;
      52           0 :         break;
      53             :     }
      54             : 
      55           0 :     case NWPMU_ID::ECU_2: {
      56           0 :         internal_state.last_updated_ms = AP_HAL::millis();
      57             :         struct ecu_2 data;
      58           0 :         memcpy(&data, frame.data, sizeof(data));
      59           0 :         switch ((NWPMU_PRESSURE_TYPE)data.pressure_type) {
      60           0 :         case NWPMU_PRESSURE_TYPE::kPa:
      61           0 :             internal_state.atmospheric_pressure_kpa = data.baro * 0.01f;
      62           0 :             internal_state.intake_manifold_pressure_kpa = data.baro * 0.01f;
      63           0 :             break;
      64           0 :         case NWPMU_PRESSURE_TYPE::psi:
      65           0 :             internal_state.atmospheric_pressure_kpa = data.baro * 0.0689476f;
      66           0 :             internal_state.intake_manifold_pressure_kpa = data.baro * 0.0689476f;
      67           0 :             break;
      68           0 :         default:
      69           0 :             break;
      70             :         }
      71           0 :         internal_state.cylinder_status.lambda_coefficient = data.lambda * 0.01f;
      72           0 :         break;
      73             :     }
      74             : 
      75           0 :     case NWPMU_ID::ECU_4: {
      76           0 :         internal_state.last_updated_ms = AP_HAL::millis();
      77             :         struct ecu_4 data;
      78           0 :         memcpy(&data, frame.data, sizeof(data));
      79             :         // remap the analog input for fuel pressure, 0.5 V == 0 PSI, 4.5V == 100 PSI
      80           0 :         internal_state.fuel_pressure = linear_interpolate(0, 689.476,
      81           0 :                                                           data.analog_fuel_pres * 0.001,
      82             :                                                           0.5f,4.5f);
      83           0 :         break;
      84             :     }
      85             : 
      86           0 :     case NWPMU_ID::ECU_5: {
      87           0 :         internal_state.last_updated_ms = AP_HAL::millis();
      88             :         struct ecu_5 data;
      89           0 :         memcpy(&data, frame.data, sizeof(data));
      90           0 :         switch((NWPMU_TEMPERATURE_TYPE)data.temp_type) {
      91           0 :         case NWPMU_TEMPERATURE_TYPE::C:
      92           0 :             internal_state.coolant_temperature = C_TO_KELVIN(data.coolant_temp * 0.1f);
      93           0 :             internal_state.cylinder_status.cylinder_head_temperature = C_TO_KELVIN(data.coolant_temp * 0.1f);
      94           0 :             break;
      95           0 :         case NWPMU_TEMPERATURE_TYPE::F:
      96           0 :             internal_state.coolant_temperature = F_TO_KELVIN(data.coolant_temp * 0.1f);
      97           0 :             internal_state.cylinder_status.cylinder_head_temperature = F_TO_KELVIN(data.coolant_temp * 0.1f);
      98           0 :             break;
      99           0 :         default:
     100           0 :             break;
     101             :         }
     102           0 :         break;
     103             :     }
     104             : 
     105           0 :     case NWPMU_ID::ECU_6: {
     106           0 :         internal_state.last_updated_ms = AP_HAL::millis();
     107             :         struct ecu_6 data;
     108           0 :         memcpy(&data, frame.data, sizeof(data));
     109           0 :         if (!_emitted_version && (AP_HAL::millis() > 10000)) { // don't emit a version early in the boot process
     110           0 :             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NWPMU Version: %d.%d.%d",
     111             :                             data.firmware_major,
     112             :                             data.firmware_minor,
     113             :                             data.firmware_build);
     114           0 :             _emitted_version = true;
     115             :         }
     116           0 :         break;
     117             :     }
     118           0 :     case NWPMU_ID::GCU:
     119             :     case NWPMU_ID::ECU_3:
     120             :     case NWPMU_ID::ECU_7:
     121             :     case NWPMU_ID::ECU_8:
     122             :     case NWPMU_ID::ECU_9:
     123             :     case NWPMU_ID::ECU_10:
     124             :     case NWPMU_ID::ECU_11:
     125             :     case NWPMU_ID::ECU_12:
     126           0 :         break;
     127             :     }
     128           0 : }
     129             : 
     130           0 : void AP_EFI_NWPMU::update()
     131             : {
     132             :     // copy the data to the front end
     133           0 :     copy_to_frontend();
     134           0 : }
     135             : 
     136             : #endif // AP_EFI_NWPWU_ENABLED