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 : Simulator for the Loweheiser generators
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_LOWEHEISER_ENABLED
22 :
23 : #include <AP_Math/AP_Math.h>
24 :
25 : #include "SIM_Loweheiser.h"
26 : #include "SITL.h"
27 : #include "SIM_Aircraft.h"
28 :
29 : #include <GCS_MAVLink/GCS.h>
30 :
31 : #include <stdio.h>
32 : #include <errno.h>
33 :
34 : using namespace SITL;
35 :
36 2096 : Loweheiser::Loweheiser() : SerialDevice::SerialDevice()
37 : {
38 2096 : }
39 :
40 922454 : void Loweheiser::update(const class Aircraft &aircraft)
41 : {
42 : // if (!_enabled.get()) {
43 : // return;
44 : // }
45 922454 : maybe_send_heartbeat();
46 922454 : update_receive();
47 922454 : update_send(aircraft);
48 922454 : }
49 :
50 922454 : void Loweheiser::maybe_send_heartbeat()
51 : {
52 922454 : const uint32_t now = AP_HAL::millis();
53 :
54 922454 : if (now - last_heartbeat_ms < 100) {
55 : // we only provide a heartbeat every so often
56 914774 : return;
57 : }
58 7680 : last_heartbeat_ms = now;
59 :
60 : mavlink_message_t msg;
61 7680 : mavlink_msg_heartbeat_pack(system_id,
62 7680 : component_id,
63 : &msg,
64 : MAV_TYPE_GCS,
65 : MAV_AUTOPILOT_INVALID,
66 : 0,
67 : 0,
68 : 0);
69 :
70 : uint8_t buf[300];
71 7680 : uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &msg);
72 :
73 7680 : if (write_to_autopilot((const char*)&buf, buf_len) != buf_len) {
74 : // ::fprintf(stderr, "write failure\n");
75 : }
76 : }
77 :
78 25539 : void Loweheiser::handle_message(const mavlink_message_t &msg)
79 : {
80 25539 : switch (msg.msgid) {
81 7410 : case MAVLINK_MSG_ID_COMMAND_LONG: {
82 : mavlink_command_long_t pkt;
83 7410 : mavlink_msg_command_long_decode(&msg, &pkt);
84 :
85 7410 : if (pkt.target_system != system_id ||
86 7410 : pkt.target_component != component_id) {
87 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Not for me");
88 0 : return;
89 : }
90 :
91 7410 : switch (pkt.command) {
92 7410 : case MAV_CMD_LOWEHEISER_SET_STATE:
93 : // decode the desired run state:
94 : // param2 physically turns power on/off to the EFI!
95 7410 : switch ((uint8_t)pkt.param2) {
96 983 : case 0:
97 983 : autopilot_desired_engine_run_state = EngineRunState::OFF;
98 983 : break;
99 6427 : case 1:
100 6427 : autopilot_desired_engine_run_state = EngineRunState::ON;
101 6427 : break;
102 0 : default:
103 0 : AP_HAL::panic("Bad desired engine run state");
104 : }
105 7410 : switch ((uint8_t)pkt.param3) {
106 2355 : case 0:
107 2355 : autopilot_desired_governor_state = GovernorState::OFF;
108 2355 : break;
109 5055 : case 1:
110 5055 : autopilot_desired_governor_state = GovernorState::ON;
111 5055 : break;
112 0 : default:
113 0 : AP_HAL::panic("Bad desired governor state");
114 : }
115 7410 : switch ((uint8_t)pkt.param5) {
116 6968 : case 0:
117 6968 : autopilot_desired_startup_state = StartupState::OFF;
118 6968 : break;
119 442 : case 1:
120 442 : autopilot_desired_startup_state = StartupState::ON;
121 442 : break;
122 0 : default:
123 0 : AP_HAL::panic("Bad electronic startup state");
124 : }
125 7410 : manual_throttle_pct = pkt.param4;
126 : mavlink_message_t ack;
127 7410 : mavlink_msg_command_ack_pack(
128 7410 : system_id,
129 7410 : component_id,
130 : &ack,
131 : MAV_CMD_LOWEHEISER_SET_STATE,
132 : MAV_RESULT_ACCEPTED,
133 : 0,
134 : 0,
135 7410 : msg.sysid,
136 7410 : msg.compid
137 : );
138 : uint8_t buf[300];
139 7410 : uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &ack);
140 :
141 7410 : if (write_to_autopilot((const char*)&buf, buf_len) != buf_len) {
142 : // ::fprintf(stderr, "write failure\n");
143 : }
144 7410 : break;
145 : }
146 : };
147 : }
148 : }
149 :
150 922454 : void Loweheiser::update_receive()
151 : {
152 : char buf[128];
153 922454 : const ssize_t bytes_read = read_from_autopilot(buf, sizeof(buf));
154 922454 : if (bytes_read <= 0) {
155 903114 : return;
156 : }
157 1314207 : for (uint16_t i=0; i<bytes_read; i++) {
158 2589734 : if (mavlink_frame_char_buffer(
159 : &rxmsg,
160 : &rxstatus,
161 1294867 : buf[i],
162 : &rxmsg,
163 1294867 : &rxstatus) == MAVLINK_FRAMING_OK) {
164 25539 : handle_message(rxmsg);
165 : }
166 : }
167 : }
168 :
169 3823 : void Loweheiser::update_fuel_level()
170 : {
171 3823 : const uint32_t now_ms = AP_HAL::millis();
172 :
173 3823 : const uint32_t tdelta = now_ms - last_fuel_update_ms;
174 3823 : last_fuel_update_ms = now_ms;
175 :
176 3823 : const float fuel_consumption_lperh_at_8000rpm = 10;
177 3823 : const float fuel_consumption_lpers_at_8000rpm = fuel_consumption_lperh_at_8000rpm / 3600;
178 :
179 3823 : fuel_flow_lps = generatorengine.current_rpm/8000 * fuel_consumption_lpers_at_8000rpm;
180 3823 : const float fuel_delta = tdelta/1000.0 * fuel_flow_lps; // litres
181 3823 : fuel_consumed += fuel_delta;
182 3823 : fuel_level -= fuel_delta;
183 3823 : }
184 :
185 922454 : void Loweheiser::update_send(const class Aircraft &aircraft)
186 : {
187 922454 : const uint32_t now = AP_HAL::millis();
188 922454 : if (now - last_sent_ms < 200) {
189 918631 : return;
190 : }
191 3835 : last_sent_ms = now;
192 :
193 3835 : auto sitl = AP::sitl();
194 3835 : if (!sitl || sitl->efi_type == SIM::EFIType::EFI_TYPE_NONE) {
195 12 : return;
196 : }
197 :
198 3823 : float throttle = 0;
199 3823 : float throttle_output = 0;
200 :
201 3823 : switch (autopilot_desired_engine_run_state) {
202 615 : case EngineRunState::OFF:
203 615 : generatorengine.desired_rpm = 0;
204 615 : break;
205 3208 : case EngineRunState::ON:
206 3208 : switch (autopilot_desired_governor_state) {
207 681 : case GovernorState::OFF: {
208 681 : throttle = manual_throttle_pct;
209 681 : throttle_output = throttle;
210 766 : if (is_positive(generatorengine.desired_rpm) ||
211 85 : autopilot_desired_startup_state == StartupState::ON) {
212 609 : const uint16_t manual_rpm_min = 0;
213 609 : const uint16_t manual_rpm_max = 8000;
214 609 : generatorengine.desired_rpm = linear_interpolate(
215 : manual_rpm_min,
216 : manual_rpm_max,
217 : throttle,
218 : 0,
219 : 100
220 : );
221 : }
222 681 : break;
223 : }
224 2527 : case GovernorState::ON:
225 : // should probably base this on current draw from battery
226 : // / motor throttle output?
227 2527 : throttle = 80;
228 2527 : throttle_output = 80;
229 2527 : generatorengine.desired_rpm = 8000;
230 2527 : break;
231 : }
232 : }
233 :
234 : // a simulation for a stuck throttle - once the egine starts it
235 : // won't stop based on servo position,, only engine run state
236 3823 : const bool stuck_throttle_failure_simulation = false;
237 : static bool throttle_is_stuck;
238 : if (stuck_throttle_failure_simulation) {
239 : if (generatorengine.desired_rpm > 7000) {
240 : throttle_is_stuck = true;
241 : }
242 : // if the throttle is stuck then the engine runs - except if
243 : // the autopilot is saying the desired runstate is off,
244 : // because that just shuts down the spark entirely, so the
245 : // engine will not run
246 : if (throttle_is_stuck &&
247 : autopilot_desired_engine_run_state == EngineRunState::ON) {
248 : // same numbers as "on" case, above
249 : throttle = 80;
250 : throttle_output = 80;
251 : generatorengine.desired_rpm = 8000;
252 : }
253 : }
254 :
255 3823 : _current_current = AP::sitl()->state.battery_current;
256 3823 : _current_current = MIN(_current_current, max_current);
257 :
258 3823 : generatorengine.current_current = _current_current;
259 3823 : generatorengine.max_current = max_current;
260 3823 : generatorengine.max_slew_rpm_per_second = 2000;
261 :
262 3823 : generatorengine.update();
263 :
264 3823 : update_fuel_level();
265 :
266 3823 : float efi_pw = std::numeric_limits<float>::quiet_NaN();
267 3823 : float efi_fuel_flow = std::numeric_limits<float>::quiet_NaN();
268 3823 : float efi_fuel_consumed = std::numeric_limits<float>::quiet_NaN();
269 3823 : float efi_fuel_level = std::numeric_limits<float>::quiet_NaN();
270 3823 : float efi_baro = std::numeric_limits<float>::quiet_NaN();
271 3823 : float efi_mat = std::numeric_limits<float>::quiet_NaN();
272 3823 : float efi_clt = std::numeric_limits<float>::quiet_NaN();
273 3823 : float efi_tps = std::numeric_limits<float>::quiet_NaN();
274 3823 : float efi_egt = std::numeric_limits<float>::quiet_NaN();
275 3823 : float efi_batt = std::numeric_limits<float>::quiet_NaN();
276 :
277 3823 : float curr_batt = -0.3;
278 3823 : float curr_gen = 10.12;
279 :
280 : // Current from the generator is the battery charging current (defined to be negative) plus the generator current
281 3823 : float curr_rot = (curr_batt < 0 ? -curr_batt : 0.0) + curr_gen;
282 :
283 : // controlled by param2, this turns on/off the DC/DC component which
284 : // powers the efi
285 3823 : if (autopilot_desired_engine_run_state == EngineRunState::ON) {
286 3208 : efi_baro = aircraft.ambient_outside_pressure_Pascal() / 1000.0;
287 3208 : efi_mat = aircraft.ambient_outside_temperature_degC();
288 3208 : efi_clt = generatorengine.temperature;
289 3208 : efi_tps = MAX(throttle_output, 40);
290 3208 : efi_batt = 12.5;
291 3208 : efi_fuel_flow = fuel_flow_lps * 3600; // litres/second -> litres/hour
292 : }
293 :
294 : if (false) {
295 : efi_fuel_level = fuel_level;
296 : efi_fuel_consumed = fuel_consumed;
297 : }
298 :
299 : // +/- 3V, depending on draw
300 3823 : const float volt_batt = base_supply_voltage - (3 * (_current_current / max_current));
301 :
302 3823 : const mavlink_loweheiser_gov_efi_t loweheiser_efi_gov{
303 : volt_batt: volt_batt, // volt_batt
304 : curr_batt: curr_batt, // curr_batt
305 : curr_gen: curr_gen, // curr_gen
306 : curr_rot: curr_rot, // curr_rot
307 : fuel_level: efi_fuel_level, // fuel_level in litres
308 : throttle: throttle, // throttle
309 : runtime: UINT32_MAX, // runtime in seconds
310 : until_maintenance: INT32_MAX, // time until maintenance
311 : rectifier_temp: std::numeric_limits<float>::quiet_NaN(), // rectifier temperature
312 : generator_temp: std::numeric_limits<float>::quiet_NaN(), // generator temperature
313 : efi_batt: efi_batt, // efi_batt
314 3823 : efi_rpm: generatorengine.current_rpm, // efi_rpm
315 : efi_pw: efi_pw, // efi_pw
316 : efi_fuel_flow: efi_fuel_flow, // efi_fuelflow
317 : efi_fuel_consumed: efi_fuel_consumed, // efi_fuel_consumed
318 : efi_baro: efi_baro, // efi_baro
319 : efi_mat: efi_mat, // efi_mat
320 : efi_clt: efi_clt, // efi_clt
321 : efi_tps: efi_tps, // efi_tps
322 : efi_exhaust_gas_temperature: efi_egt, // efi_exhaust_gas_temperature
323 : generator_status: 0, // generator_status
324 : efi_status: 0, // EFI status
325 : efi_index: 1, // EFI index
326 3823 : };
327 :
328 : mavlink_message_t msg;
329 3823 : mavlink_msg_loweheiser_gov_efi_encode_status(
330 3823 : system_id,
331 3823 : component_id,
332 : &mav_status,
333 : &msg,
334 : &loweheiser_efi_gov
335 : );
336 :
337 : uint8_t buf[300];
338 3823 : uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &msg);
339 :
340 3823 : if (write_to_autopilot((char*)buf, buf_len) != buf_len) {
341 0 : AP_HAL::panic("Failed to write to autopilot: %s", strerror(errno));
342 : }
343 : }
344 :
345 : #endif // AP_SIM_LOWEHEISER_ENABLED
|
