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_Serial_Hirth.h
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
17
#pragma once
18
19
#include "AP_EFI_config.h"
20
21
#if AP_EFI_SERIAL_HIRTH_ENABLED
22
#include "AP_EFI.h"
23
#include "AP_EFI_Backend.h"
24
25
/*!
26
* class definition for Hirth 4103 ECU
27
*/
28
class AP_EFI_Serial_Hirth: public AP_EFI_Backend {
29
public:
30
AP_EFI_Serial_Hirth(AP_EFI &_frontend);
31
32
void update() override;
33
34
enum class Error_Excess_Temp_Bitfield : uint16_t {
35
CHT_1_LOW = 1U<<0, // true if CHT1 is too low
36
CHT_1_HIGH = 1U<<1, // true if CHT1 is too high
37
CHT_1_ENRC_ACTIVE = 1U<<2, // true if CHT1 Enrichment is active
38
CHT_2_LOW = 1U<<3, // true if CHT2 is too low
39
CHT_2_HIGH = 1U<<4, // true if CHT2 is too high
40
CHT_2_ENRC_ACTIVE = 1U<<5, // true if CHT2 Enrichment is active
41
EGT_1_LOW = 1U<<6, // true if EGT1 is too low
42
EGT_1_HIGH = 1U<<7, // true if EGT1 is too high
43
EGT_1_ENRC_ACTIVE = 1U<<8, // true if EGT1 Enrichment is active
44
EGT_2_LOW = 1U<<9, // true if EGT2 is too low
45
EGT_2_HIGH = 1U<<10, // true if EGT2 is too high
46
EGT_2_ENRC_ACTIVE = 1U<<11, // true if EGT2 Enrichment is active
47
};
48
49
enum class Sensor_Status_Bitfield : uint8_t {
50
ENGINE_TEMP_SENSOR_OK = 1U<<1, // true if engine temperature sensor is OK
51
AIR_TEMP_SENSOR_OK = 1U<<2, // true if air temperature sensor is OK
52
AIR_PRESSURE_SENSOR_OK = 1U<<3, // true if intake air pressure sensor is OK
53
THROTTLE_SENSOR_OK = 1U<<4, // true if throttle sensor is OK
54
};
55
56
private:
57
// serial port instance
58
AP_HAL::UARTDriver *port;
59
60
// periodic refresh
61
uint32_t last_request_ms;
62
uint32_t last_packet_ms;
63
uint32_t last_req_send_throttle_ms;
64
65
// raw bytes - max size
66
uint8_t raw_data[256];
67
68
// request and response data
69
uint8_t requested_code;
70
71
// meta-data for a response
72
struct {
73
uint8_t quantity;
74
uint8_t code;
75
uint8_t checksum;
76
} res_data;
77
78
// TRUE - Request is sent; waiting for response
79
// FALSE - Response is already received
80
bool waiting_response;
81
82
// Expected bytes from response
83
uint8_t expected_bytes;
84
85
// Throttle values
86
uint16_t last_throttle;
87
uint16_t throttle_to_hirth;
88
89
uint32_t last_fuel_integration_ms;
90
91
// custom status for Hirth
92
uint16_t sensor_status;
93
94
uint16_t error_excess_temperature;
95
uint32_t crc_fail_cnt;
96
uint32_t uptime;
97
uint32_t ack_fail_cnt;
98
99
struct PACKED Record1 {
100
uint8_t reserved1[2];
101
uint16_t save_in_flash; // "1 = data are saved in flash automatically"
102
uint8_t reserved2[4];
103
uint16_t engine_status;
104
uint16_t rpm;
105
uint8_t reserved3[12];
106
uint16_t number_of_interfering_pulses;
107
uint16_t reserved4[2];
108
uint16_t number_of_speed_errors;
109
uint16_t injection_time;
110
uint16_t ignition_angle;
111
uint16_t reserved5;
112
uint16_t voltage_throttle;
113
uint16_t reserved6;
114
uint8_t reserved7[2];
115
uint16_t voltage_engine_temperature;
116
uint16_t voltage_air_temperature;
117
uint8_t reserved8[2];
118
uint16_t voltage_int_air_pressure;
119
uint8_t reserved9[20];
120
int16_t throttle;
121
int16_t engine_temperature;
122
int16_t battery_voltage;
123
int16_t air_temperature;
124
int16_t reserved10;
125
uint16_t sensor_ok;
126
};
127
static_assert(sizeof(Record1) == 84, "incorrect Record1 length");
128
129
struct PACKED Record2 {
130
uint8_t reserved1[12];
131
int16_t injection_rate_from_basic_graphic_map;
132
int16_t reserved2;
133
int16_t basic_injection_rate;
134
int16_t injection_rate_from_air_correction;
135
int16_t reserved3;
136
int16_t injection_rate_from_warming_up_characteristic_curve;
137
int16_t injection_rate_from_acceleration_enrichment;
138
int16_t turn_on_time_of_intake_valves;
139
int16_t injection_rate_from_race_switch;
140
int16_t reserved4;
141
int16_t injection_angle_from_ignition_graphic_map;
142
int16_t injection_angle_from_air_temperature_characteristic_curve;
143
int16_t injection_angle_from_air_pressure_characteristic_curve;
144
int16_t ignition_angle_from_engine_temperature_characteristic_curve;
145
int16_t ignition_angle_from_acceleration;
146
int16_t ignition_angle_from_race_switch;
147
uint32_t total_time_in_26ms;
148
uint32_t total_number_of_rotations;
149
uint16_t fuel_consumption;
150
uint16_t number_of_errors_in_error_memory;
151
int16_t voltage_input1_throttle_target;
152
int16_t reserved5;
153
int16_t position_throttle_target;
154
int16_t throttle_percent_times_10; // percent * 0.1
155
int16_t reserved6[3];
156
uint16_t time_of_injector_opening_percent_times_10;
157
uint8_t reserved7[10];
158
uint32_t no_of_logged_data;
159
uint8_t reserved8[12];
160
};
161
static_assert(sizeof(Record2) == 98, "incorrect Record2 length");
162
163
struct PACKED Record3 {
164
int16_t voltage_excess_temperature_1;
165
int16_t voltage_excess_temperature_2;
166
int16_t voltage_excess_temperature_3;
167
int16_t voltage_excess_temperature_4;
168
int16_t voltage_excess_temperature_5;
169
uint8_t reserved1[6];
170
int16_t excess_temperature_1; // cht1
171
int16_t excess_temperature_2; // cht2
172
int16_t excess_temperature_3; // egt1
173
int16_t excess_temperature_4; // egt2
174
int16_t excess_temperature_5;
175
uint8_t reserved2[6];
176
int16_t enrichment_excess_temperature_cylinder_1;
177
int16_t enrichment_excess_temperature_cylinder_2;
178
int16_t enrichment_excess_temperature_cylinder_3;
179
int16_t enrichment_excess_temperature_cylinder_4;
180
uint8_t reserved3[6];
181
uint16_t error_excess_temperature_bitfield;
182
uint16_t mixing_ratio_oil_pump1;
183
uint16_t mixing_ratio_oil_pump2;
184
uint16_t ouput_value_water_pump;
185
uint16_t ouput_value_fuel_pump;
186
uint16_t ouput_value_exhaust_valve;
187
uint16_t ouput_value_air_vane;
188
uint16_t ouput_value_e_throttle;
189
uint16_t number_of_injections_oil_pump_1;
190
uint32_t system_time_in_ms;
191
int16_t number_of_injections_oil_pump_2;
192
uint16_t target_rpm;
193
uint16_t FPC;
194
uint16_t xenrichment_excess_temperature_cylinder_1;
195
uint16_t xenrichment_excess_temperature_cylinder_2;
196
uint16_t xenrichment_excess_temperature_cylinder_3;
197
uint16_t xenrichment_excess_temperature_cylinder_4;
198
uint16_t voltage_input_temperature_crankshaft_housing;
199
int16_t temperature_crankshaft_housing;
200
uint8_t reserved4[14];
201
};
202
static_assert(sizeof(Record3) == 100, "incorrect Record3 length");
203
204
void check_response();
205
void send_request();
206
void decode_data();
207
bool send_request_status();
208
bool send_target_values(uint16_t);
209
void log_status();
210
};
211
212
#endif // AP_EFI_SERIAL_HIRTH_ENABLED
213
214