CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_EFI/AP_EFI_NWPMU.cpp
Views: 1798
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
AP_EFI_NWPMU::AP_EFI_NWPMU(AP_EFI &_frontend) :

32
    CANSensor("NWPMU"),

33
    AP_EFI_Backend(_frontend)

34
{

35
    register_driver(AP_CAN::Protocol::EFI_NWPMU);

36
}

37


38
void AP_EFI_NWPMU::handle_frame(AP_HAL::CANFrame &frame)

39
{

40
    const uint32_t id =  frame.id & AP_HAL::CANFrame::MaskExtID;

41


42
    WITH_SEMAPHORE(get_sem());

43


44
    switch ((NWPMU_ID)id) {

45
    case NWPMU_ID::ECU_1: {

46
        internal_state.last_updated_ms = AP_HAL::millis();

47
        struct ecu_1 data;

48
        memcpy(&data, frame.data, sizeof(data));

49
        internal_state.engine_speed_rpm = data.rpm;

50
        internal_state.throttle_position_percent = data.tps * 0.1f;

51
        internal_state.cylinder_status.ignition_timing_deg = data.ignition_angle * 0.1f;

52
        break;

53
    }

54


55
    case NWPMU_ID::ECU_2: {

56
        internal_state.last_updated_ms = AP_HAL::millis();

57
        struct ecu_2 data;

58
        memcpy(&data, frame.data, sizeof(data));

59
        switch ((NWPMU_PRESSURE_TYPE)data.pressure_type) {

60
        case NWPMU_PRESSURE_TYPE::kPa:

61
            internal_state.atmospheric_pressure_kpa = data.baro * 0.01f;

62
            internal_state.intake_manifold_pressure_kpa = data.baro * 0.01f;

63
            break;

64
        case NWPMU_PRESSURE_TYPE::psi:

65
            internal_state.atmospheric_pressure_kpa = data.baro * 0.0689476f;

66
            internal_state.intake_manifold_pressure_kpa = data.baro * 0.0689476f;

67
            break;

68
        default:

69
            break;

70
        }

71
        internal_state.cylinder_status.lambda_coefficient = data.lambda * 0.01f;

72
        break;

73
    }

74


75
    case NWPMU_ID::ECU_4: {

76
        internal_state.last_updated_ms = AP_HAL::millis();

77
        struct ecu_4 data;

78
        memcpy(&data, frame.data, sizeof(data));

79
        // remap the analog input for fuel pressure, 0.5 V == 0 PSI, 4.5V == 100 PSI

80
        internal_state.fuel_pressure = linear_interpolate(0, 689.476,

81
                                                          data.analog_fuel_pres * 0.001,

82
                                                          0.5f,4.5f);

83
        break;

84
    }

85


86
    case NWPMU_ID::ECU_5: {

87
        internal_state.last_updated_ms = AP_HAL::millis();

88
        struct ecu_5 data;

89
        memcpy(&data, frame.data, sizeof(data));

90
        switch((NWPMU_TEMPERATURE_TYPE)data.temp_type) {

91
        case NWPMU_TEMPERATURE_TYPE::C:

92
            internal_state.coolant_temperature = C_TO_KELVIN(data.coolant_temp * 0.1f);

93
            internal_state.cylinder_status.cylinder_head_temperature = C_TO_KELVIN(data.coolant_temp * 0.1f);

94
            break;

95
        case NWPMU_TEMPERATURE_TYPE::F:

96
            internal_state.coolant_temperature = F_TO_KELVIN(data.coolant_temp * 0.1f);

97
            internal_state.cylinder_status.cylinder_head_temperature = F_TO_KELVIN(data.coolant_temp * 0.1f);

98
            break;

99
        default:

100
            break;

101
        }

102
        break;

103
    }

104


105
    case NWPMU_ID::ECU_6: {

106
        internal_state.last_updated_ms = AP_HAL::millis();

107
        struct ecu_6 data;

108
        memcpy(&data, frame.data, sizeof(data));

109
        if (!_emitted_version && (AP_HAL::millis() > 10000)) { // don't emit a version early in the boot process

110
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NWPMU Version: %d.%d.%d",

111
                            data.firmware_major,

112
                            data.firmware_minor,

113
                            data.firmware_build);

114
            _emitted_version = true;

115
        }

116
        break;

117
    }

118
    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
        break;

127
    }

128
}

129


130
void AP_EFI_NWPMU::update()

131
{

132
    // copy the data to the front end

133
    copy_to_frontend();

134
}

135


136
#endif // AP_EFI_NWPWU_ENABLED

137


138