Line data Source code
1 : #include "AP_Generator_Loweheiser.h"
2 :
3 : #if AP_GENERATOR_LOWEHEISER_ENABLED
4 :
5 : #include <AP_Vehicle/AP_Vehicle.h>
6 : #include <GCS_MAVLink/GCS.h>
7 :
8 : extern const AP_HAL::HAL& hal;
9 :
10 : #define MASK_LOG_ANY 0xFFFF
11 : #define LOWEHEISER_EFI_COMS_TIMEOUT_MS 1500
12 : #define LOWEHEISER_EFI_VOLT_MIN 11.0
13 : #define LOWEHEISER_EFI_VOLT_MAX 13.5
14 :
15 : const AP_Param::GroupInfo AP_Generator_Loweheiser::var_info[] = {
16 :
17 : // Param indexes must be between 10 and 19 to avoid conflict with other generator param tables loaded by pointer
18 :
19 : // @Param: MNT_TIME
20 : // @DisplayName: Seconds until maintenance required
21 : // @Description: Seconds until maintenance required
22 : // @User: Advanced
23 : AP_GROUPINFO("MNT_TIME", 10, AP_Generator_Loweheiser, time_until_maintenance, 0),
24 :
25 : // @Param: RUNTIME
26 : // @DisplayName: Total runtime
27 : // @Description: Total time this generator has run in seconds
28 : // @User: Advanced
29 : AP_GROUPINFO("RUNTIME", 11, AP_Generator_Loweheiser, total_runtime, 0),
30 :
31 : // @Param: IDLE_TH_H
32 : // @DisplayName: High Idle throttle
33 : // @Description: throttle value to use when warming up or cooling down
34 : // @User: Advanced
35 : AP_GROUPINFO("IDLE_TH_H", 12, AP_Generator_Loweheiser, high_idle_throttle, 15),
36 :
37 : // @Param: IDLE_TH
38 : // @DisplayName: Idle throttle
39 : // @Description: throttle value to use when idling
40 : // @User: Advanced
41 : AP_GROUPINFO("IDLE_TH", 13, AP_Generator_Loweheiser, idle_throttle, 15),
42 :
43 : // @Param: RUN_TEMP
44 : // @DisplayName: Run Temperature
45 : // @Description: temperature required for generator to start producing power in deg celsius
46 : // @User: Advanced
47 : AP_GROUPINFO("RUN_TEMP", 14, AP_Generator_Loweheiser, temp_required_for_run, 60),
48 :
49 : // @Param: IDLE_TEMP
50 : // @DisplayName: Idle Temperature
51 : // @Description: temperature required for generator to return to idle after having run
52 : // @User: Advanced
53 : AP_GROUPINFO("IDLE_TEMP", 15, AP_Generator_Loweheiser, temp_required_for_idle, 110),
54 :
55 : // @Param: OVER_TEMP
56 : // @DisplayName: Cylinder Head Over Temperature Warning Level
57 : // @Description: threshold temperature for the cylinder head above which the mavlink over temperature message gets sent
58 : // @Units: degC
59 : // @User: Advanced
60 : AP_GROUPINFO("OVER_TEMP", 16, AP_Generator_Loweheiser, temp_for_overtemp_warning, 205),
61 :
62 : // Param indexes must be between 10 and 19 to avoid conflict with other generator param tables loaded by pointer
63 :
64 : AP_GROUPEND
65 : };
66 :
67 10 : void AP_Generator_Loweheiser::init()
68 : {
69 10 : AP_Param::setup_object_defaults(this, var_info);
70 :
71 10 : _frontend._has_current = true;
72 10 : _frontend._has_consumed_energy = true;
73 10 : _frontend._has_fuel_remaining = false;
74 :
75 : // nothing in this method may use parameters as AP_Generator loads
76 : // values from eeprom!
77 10 : }
78 :
79 : // healthy returns true if the generator is not present, or it is
80 : // present, providing telemetry and not indicating an errors. Check
81 : // several fields as being within-range.
82 7410 : bool AP_Generator_Loweheiser::healthy() const
83 : {
84 7410 : if (last_packet_received_ms == 0) {
85 0 : return true;
86 : }
87 :
88 7410 : if (AP_HAL::millis() - last_packet_received_ms > LOWEHEISER_EFI_COMS_TIMEOUT_MS) {
89 0 : return false;
90 : }
91 :
92 : // these voltage constants were supplied by the manufacturer:
93 13832 : if (!isnan(packet.efi_batt) &&
94 6422 : (packet.efi_batt < LOWEHEISER_EFI_VOLT_MIN || packet.efi_batt > LOWEHEISER_EFI_VOLT_MAX)) {
95 0 : return false;
96 : }
97 :
98 7410 : return true;
99 : }
100 :
101 : // runstate_string provides a textual representation for the supplied
102 : // runstate. Useful in messages to the user.
103 66 : const char *AP_Generator_Loweheiser::runstate_string(PilotDesiredRunState runstate)
104 : {
105 66 : switch (runstate) {
106 30 : case PilotDesiredRunState::STOP:
107 30 : return "STOP";
108 16 : case PilotDesiredRunState::IDLE:
109 16 : return "IDLE";
110 20 : case PilotDesiredRunState::RUN:
111 20 : return "RUN";
112 : }
113 0 : return "?";
114 : }
115 :
116 33 : void AP_Generator_Loweheiser::set_pilot_desired_runstate(PilotDesiredRunState newstate) {
117 33 : gcs().send_text(MAV_SEVERITY_INFO, "LH: pilot-desired state to (%s) from (%s)", runstate_string(newstate), runstate_string(pilot_desired_runstate));
118 33 : pilot_desired_runstate = newstate;
119 33 : }
120 :
121 : // returns true if the generator should be allowed to move into the
122 : // "run" (high-RPM) state:
123 1295 : bool AP_Generator_Loweheiser::generator_ok_to_run() const
124 : {
125 1295 : if (isnan(packet.efi_clt)) {
126 10 : return false;
127 : }
128 1285 : return packet.efi_clt >= temp_required_for_run;
129 : }
130 :
131 : // returns true if the generator should be allowed to move into the
132 : // "stop" state:
133 451 : bool AP_Generator_Loweheiser::generator_ok_to_stop() const
134 : {
135 451 : return packet.efi_clt <= temp_required_for_idle;
136 : }
137 :
138 : // should_emergency_stop - returns true if the generator must stop
139 : // immediately
140 7410 : bool AP_Generator_Loweheiser::should_emergency_stop()
141 : {
142 7410 : const char *estop_reason = nullptr;
143 :
144 7410 : if (AP::vehicle()->is_crashed()) {
145 0 : estop_reason = "crash";
146 : }
147 :
148 7410 : if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
149 64 : estop_reason = "safety switch";
150 : }
151 :
152 7410 : if (estop_reason == nullptr) {
153 : // not emergency stopped
154 7346 : estop_reported = false;
155 7346 : return false;
156 : }
157 :
158 : // we are currrently emergency-stopped
159 64 : if (!estop_reported) {
160 2 : gcs().send_text(MAV_SEVERITY_INFO, "LH: %s; stopping generator", estop_reason);
161 2 : estop_reported = true;
162 : }
163 :
164 64 : return true;
165 : }
166 :
167 : // update_runstate updates the commanded runstate - what we are
168 : // telling the generator to do. Which state we request the generator
169 : // move to depends on the RC input control, the temperature the
170 : // generator is at and other information
171 7410 : void AP_Generator_Loweheiser::update_runstate()
172 : {
173 : // If the vehicle crashes then we assume the pilot wants to stop
174 : // the motor. This is done as a once-off when the crash is
175 : // detected to allow the operator to rearm the vehicle, or we end
176 : // up in a catch-22 situation where we force the stop state on the
177 : // generator so they can't arm and can't start the generator
178 : // because the vehicle is crashed.
179 7410 : if (should_emergency_stop()) {
180 64 : commanded_runstate = RunState::STOP;
181 64 : return;
182 : }
183 :
184 7346 : if (hal.util->get_soft_armed()) {
185 : // don't permit transitions while armed
186 3620 : return;
187 : }
188 :
189 : // if we have lost RC input, and the vehicle is disarmed then stop
190 : // the generator - but allow the thing to cool down....
191 3726 : if (!rc().has_valid_input() &&
192 0 : pilot_desired_runstate != PilotDesiredRunState::STOP) {
193 0 : gcs().send_text(MAV_SEVERITY_INFO, "LH: no RC and disarmed; stopping generator");
194 0 : pilot_desired_runstate = PilotDesiredRunState::STOP;
195 : }
196 :
197 : // skip changing the commanded runstate if we're already in the
198 : // target state. Note that this block also stops us forcing the
199 : // runstate to idle when we don't have data from the EFI!
200 3726 : switch (pilot_desired_runstate) {
201 1352 : case PilotDesiredRunState::STOP:
202 1352 : if (commanded_runstate == RunState::STOP) {
203 913 : return;
204 : }
205 439 : break;
206 358 : case PilotDesiredRunState::IDLE:
207 358 : if (commanded_runstate == RunState::IDLE) {
208 339 : return;
209 : }
210 19 : break;
211 2016 : case PilotDesiredRunState::RUN:
212 2016 : if (commanded_runstate == RunState::RUN) {
213 1476 : return;
214 : }
215 540 : break;
216 : }
217 :
218 998 : if (isnan(packet.efi_clt)) {
219 : // we don't know what the temperature is..... command idle
220 : // until we know what the temperature is. This can happen
221 : // because the EFI is actually powered off when we command the
222 : // generator to stop. Moving to IDLE should start it up so we
223 : // can get the data we need:
224 15 : commanded_runstate = RunState::IDLE;
225 15 : return;
226 : }
227 :
228 : // consider changing the commanded runstate to the pilot desired
229 : // runstate:
230 983 : commanded_runstate = RunState::IDLE;
231 983 : switch (pilot_desired_runstate) {
232 439 : case PilotDesiredRunState::STOP:
233 439 : if (!generator_ok_to_stop()) {
234 433 : commanded_runstate = RunState::COOLING_DOWN;
235 433 : break;
236 : }
237 6 : commanded_runstate = RunState::STOP;
238 6 : break;
239 12 : case PilotDesiredRunState::IDLE:
240 12 : if (!generator_ok_to_stop()) {
241 12 : commanded_runstate = RunState::COOLING_DOWN;
242 12 : break;
243 : }
244 0 : commanded_runstate = RunState::IDLE;
245 0 : break;
246 532 : case PilotDesiredRunState::RUN:
247 532 : if (!generator_ok_to_run()) {
248 524 : commanded_runstate = RunState::WARMING_UP;
249 524 : break;
250 : }
251 8 : commanded_runstate = RunState::RUN;
252 8 : break;
253 : }
254 : }
255 :
256 : // update_common_backend_variables changes data in the superclass
257 : // object, reporting our current state
258 7410 : void AP_Generator_Loweheiser::update_common_backend_variables()
259 : {
260 7410 : if (last_packet_received_ms == 0) {
261 : // no data from the generator
262 0 : return;
263 : }
264 :
265 : // common backend variables. These are used by the base class
266 : // (AP_Generator_Backend) to provide data to the battery monitor
267 : // library.
268 7410 : _voltage = packet.volt_batt;
269 7410 : _current = packet.curr_batt;
270 :
271 : // provide our own aggregate data:
272 7410 : _consumed_mah = accumulated_consumed_fuel_litres * 1000;
273 :
274 : // packet.efi_rpm_consumed goes to NaN while the EFI is off
275 : // (which is the case when the generator is in the "off" state).
276 : // In that case we assume the generator isn't turning.
277 7410 : if (isnan(packet.efi_rpm)) {
278 0 : _rpm = 0;
279 : } else {
280 7410 : _rpm = packet.efi_rpm;
281 : }
282 :
283 : // packet.efi_fuel_consumed goes to NaN while the EFI is off
284 : // (which is the case when the generator is in the "off" state).
285 : // In that case we send 0 as the amount of fuel remaining
286 7410 : if (isnan(packet.fuel_level)) {
287 7410 : _fuel_remaining_l = 0;
288 : } else {
289 0 : _fuel_remaining_l = packet.fuel_level;
290 : }
291 : }
292 :
293 7410 : void AP_Generator_Loweheiser::check_second_init()
294 : {
295 7410 : if (second_init_done) {
296 7400 : return;
297 : }
298 10 : second_init_done = true;
299 10 : if (time_until_maintenance == 0) {
300 : // manufacturer-recommended maintenance interval, 300 hours.
301 : // On the off-chance that the user manages to switch the
302 : // vehicle off at exactly 0 seconds remaining this *will*
303 : // reset.
304 2 : time_until_maintenance.set_and_save(300 * 60 * 60);
305 : }
306 : }
307 :
308 7410 : void AP_Generator_Loweheiser::update()
309 : {
310 7410 : check_second_init();
311 :
312 : // periodically check the user's configuration of RC input channels:
313 7410 : check_rc_input_channels();
314 :
315 : // consider our pilot-demanded run state and perhaps change the
316 : // way we are commanding the generator:
317 7410 : update_runstate();
318 :
319 : // update the variables the frontend looksat:
320 7410 : update_common_backend_variables();
321 :
322 : // now ask the frontend to look at that updated state:
323 7410 : update_frontend();
324 :
325 : // consider sending commands to the generator, which we do at
326 : // regular intervals:
327 7410 : command_generator();
328 :
329 : // see if the generator is producing good acks:
330 7410 : if (last_ack_packet_ms != last_ack_packet_processed_ms) {
331 7382 : last_ack_packet_processed_ms = last_ack_packet_ms;
332 7382 : good_ack = ack_packet.result == MAV_RESULT_ACCEPTED;
333 : }
334 :
335 : // onboard logging
336 7410 : Log_Write();
337 :
338 : // record runtime statistics (e.g. total runtime) to backing parameters:
339 7410 : update_stats();
340 7410 : }
341 :
342 7410 : void AP_Generator_Loweheiser::check_rc_input_channels()
343 : {
344 7410 : const uint32_t now_ms = AP_HAL::millis();
345 :
346 : // only check every now and then:
347 7410 : if (now_ms - last_rc_channel_check < 1000) {
348 6667 : return;
349 : }
350 743 : last_rc_channel_check = now_ms;
351 :
352 : // update our manual control throttle channel every now and then
353 : // (for runtime configuration):
354 743 : RC_Channel *x = rc().find_channel_for_option(RC_Channel::AUX_FUNC::LOWEHEISER_THROTTLE);
355 743 : if (x != rc_channel_manual_throttle) {
356 6 : rc_channel_manual_throttle = x;
357 6 : if (rc_channel_manual_throttle != nullptr) {
358 5 : rc_channel_manual_throttle->set_range(4500);
359 : }
360 : }
361 :
362 : // see if the user has defined a starter motor input channel.
363 : // This allows for manual control of the starter motor
364 743 : rc_channel_starter_motor = rc().find_channel_for_option(RC_Channel::AUX_FUNC::LOWEHEISER_STARTER);
365 : }
366 :
367 : // command_generator assembles COMMAND_LONG messages to send to the
368 : // generator. It does this at regular intervals so that in the case
369 : // the MCU dies the generator can undertake sensible failsafe actions.
370 7410 : void AP_Generator_Loweheiser::command_generator()
371 : {
372 7410 : if (mavlink_channel == nullptr) {
373 : // we've never seen a generator, so don't bother with any more work
374 0 : return;
375 : }
376 :
377 7410 : const uint32_t now_ms = AP_HAL::millis();
378 :
379 : // these values are appropriate to command the generator to the
380 : // "STOP state. Note that these are values which are placed
381 : // directly into the packet going to the generator.
382 7410 : float throttle = 0.0;
383 7410 : uint8_t desired_governor_state = 0;
384 7410 : uint8_t run_electric_starter = 0;
385 7410 : uint8_t desired_engine_state = 0;
386 :
387 7410 : bool user_controlled_starter = false;
388 7410 : switch (commanded_runstate) {
389 983 : case RunState::STOP:
390 : // the variable initialisation above is sufficient
391 983 : break;
392 969 : case RunState::COOLING_DOWN:
393 : case RunState::WARMING_UP: {
394 : // because we can spend a deal of time in this state we
395 : // comfort the user periodically by giving them a progress
396 : // message of sorts:
397 969 : if (now_ms - last_waiting_temperature_change_ms > 5000) {
398 27 : if (isnan(packet.efi_clt)) {
399 0 : gcs().send_text(MAV_SEVERITY_INFO, "LH: Waiting for EFI");
400 27 : } else if (commanded_runstate == RunState::WARMING_UP) {
401 16 : gcs().send_text(MAV_SEVERITY_INFO,
402 : "LH: Generator warming up (%f < %f)",
403 16 : packet.efi_clt,
404 16 : temp_required_for_run.get());
405 : } else {
406 11 : gcs().send_text(MAV_SEVERITY_INFO,
407 : "LH: Generator cooling down (%f > %f)",
408 11 : packet.efi_clt,
409 11 : temp_required_for_idle.get());
410 : }
411 27 : last_waiting_temperature_change_ms = now_ms;
412 : }
413 969 : desired_engine_state = 1;
414 969 : throttle = high_idle_throttle;
415 969 : break;
416 : }
417 354 : case RunState::IDLE:
418 : // consider acting on manual throttle input:
419 592 : if (rc_channel_manual_throttle != nullptr &&
420 238 : rc().has_valid_input()) {
421 238 : throttle = rc_channel_manual_throttle->percent_input();
422 : // honour an electric start channel, too:
423 238 : if (rc_channel_starter_motor != nullptr) {
424 238 : user_controlled_starter = true;
425 238 : switch (rc_channel_starter_motor->get_aux_switch_pos()) {
426 110 : case RC_Channel::AuxSwitchPos::LOW:
427 : case RC_Channel::AuxSwitchPos::MIDDLE:
428 110 : break;
429 128 : case RC_Channel::AuxSwitchPos::HIGH:
430 128 : run_electric_starter = 1;
431 128 : break;
432 : }
433 : }
434 : } else {
435 : // no manual throttle in play
436 116 : throttle = idle_throttle;
437 : }
438 354 : desired_engine_state = 1;
439 354 : break;
440 5104 : case RunState::RUN:
441 5104 : desired_engine_state = 1;
442 5104 : desired_governor_state = 1;
443 5104 : break;
444 : }
445 :
446 : // if our desired run state is not "stop", and the RPM is zero,
447 : // then consider running the starter motor. Run motor for 5s.
448 7410 : if (!user_controlled_starter && commanded_runstate != RunState::STOP) {
449 6189 : bool configure_for_start = false;
450 6189 : if (last_start_time_ms == 0) {
451 5404 : if (is_zero(packet.efi_rpm)) {
452 8 : gcs().send_text(MAV_SEVERITY_INFO, "LH: running starter motor");
453 8 : last_start_time_ms = now_ms;
454 8 : configure_for_start = true;
455 : }
456 785 : } else if (now_ms - last_start_time_ms < 5000) {
457 306 : configure_for_start = true;
458 479 : } else if (now_ms - last_start_time_ms > 20000) {
459 4 : last_start_time_ms = 0;
460 : }
461 :
462 6189 : if (configure_for_start) {
463 314 : run_electric_starter = 1;
464 314 : desired_governor_state = 0;
465 : // force the throttle to the idle throttle for starting:
466 314 : throttle = idle_throttle;
467 : }
468 : }
469 :
470 : // actually send the packet:
471 7410 : mavlink_msg_command_long_send(
472 7410 : mavlink_channel->get_chan(),
473 7410 : sysid,
474 7410 : compid,
475 : MAV_CMD_LOWEHEISER_SET_STATE,
476 : 0, // confirmation
477 7410 : efi_index, // p1, EFI index
478 : desired_engine_state, // p2, desired engine state
479 : desired_governor_state, // p3, desired governor state - 1 means governed
480 : throttle, // p4, desired manual throttle
481 : run_electric_starter, // p5, electric starter
482 : 0, // p6, empty
483 : 0 // p7, empty
484 : );
485 :
486 : #if HAL_LOGGING_ENABLED
487 : // log all commands to dataflash:
488 :
489 : // @LoggerMessage: LOEC
490 : // @Description: Gathered Loweheiser EFI/Governor telemetry
491 : // @Field: TimeUS: Time since system startup
492 : // @Field: SI: target system ID
493 : // @Field: CI: target component ID
494 : // @Field: C: command
495 : // @Field: I: efi index
496 : // @Field: ES: desired engine state (0:EFI off 1:EFI on)
497 : // @Field: GS: desired governor state (0:Governor off 1:Governor on)
498 : // @Field: Thr: manual throttle value
499 : // @Field: Strtr: desired electric start
500 :
501 7410 : if (AP::logger().should_log(MASK_LOG_ANY)) {
502 14752 : AP::logger().Write(
503 : "LOEC",
504 : "TimeUS," "SI," "CI," "C," "I," "ES," "GS," "Thr," "Strtr",
505 : "s" "-" "-" "-" "#" "-" "-" "-" "-" ,
506 : "F" "-" "-" "-" "-" "-" "-" "-" "-" ,
507 : "Q" "B" "B" "I" "B" "B" "B" "f" "B" ,
508 : AP_HAL::micros64(),
509 7376 : sysid,
510 7376 : compid,
511 : MAV_CMD_LOWEHEISER_SET_STATE,
512 7376 : efi_index,
513 : desired_engine_state,
514 : desired_governor_state,
515 : throttle,
516 : run_electric_starter
517 : );
518 : }
519 : #endif // HAL_LOGGING_ENABLED
520 : }
521 :
522 : // update (and perhaps persist) statistics (e.g. total runtime) to
523 : // backing parameters:
524 7410 : void AP_Generator_Loweheiser::update_stats()
525 : {
526 7410 : bool running = false;
527 7410 : switch (commanded_runstate) {
528 983 : case RunState::STOP:
529 983 : running = false;
530 983 : break;
531 6427 : case RunState::IDLE:
532 : case RunState::RUN:
533 : case RunState::WARMING_UP:
534 : case RunState::COOLING_DOWN:
535 6427 : running = true;
536 6427 : break;
537 : }
538 :
539 7410 : if (is_zero(packet.efi_rpm)) {
540 990 : running = false;
541 : }
542 :
543 7410 : const uint32_t now_ms = AP_HAL::millis();
544 :
545 7410 : if (running) {
546 6267 : if (was_running) {
547 6253 : runtime_delta_ms += now_ms - run_start_ms;
548 6253 : run_start_ms = now_ms;
549 : } else {
550 14 : run_start_ms = now_ms;
551 14 : was_running = true;
552 : }
553 : } else {
554 1143 : if (was_running) {
555 8 : runtime_delta_ms += now_ms - run_start_ms;
556 8 : was_running = false;
557 : }
558 : }
559 :
560 : // don't record small runtime changes:
561 7410 : if (runtime_delta_ms < 1000) {
562 1064 : return;
563 : }
564 :
565 : // only save to permanent storage every 30 seconds:
566 6346 : if (now_ms - last_stats_saved_ms < 30000) {
567 6324 : return;
568 : }
569 22 : last_stats_saved_ms = now_ms;
570 :
571 22 : uint32_t seconds = runtime_delta_ms / 1000;
572 22 : runtime_delta_ms -= seconds * 1000;
573 :
574 22 : total_runtime.set_and_save_ifchanged(total_runtime + seconds);
575 : // avoid resetting time until maintenance to initial value:
576 22 : if (seconds > 0 && (signed)seconds == time_until_maintenance) {
577 0 : seconds--;
578 : }
579 22 : time_until_maintenance.set_and_save_ifchanged(time_until_maintenance - seconds);
580 : }
581 :
582 : // ensure the generator is running and generally working before
583 : // allowing the vehicle to arm:
584 376 : bool AP_Generator_Loweheiser::pre_arm_check(char *failmsg, uint8_t failmsg_len) const
585 : {
586 376 : if (last_packet_received_ms == 0) {
587 : // allow optional use of generator
588 0 : return true;
589 : }
590 :
591 376 : const uint32_t now_ms = AP_HAL::millis();
592 :
593 376 : if (now_ms - last_packet_received_ms > 2000) { // we expect @1Hz
594 0 : hal.util->snprintf(failmsg, failmsg_len, "LH: no messages in %ums", unsigned(now_ms - last_packet_received_ms));
595 0 : return false;
596 : }
597 :
598 : // we warn the user that maintenance is required, but don't stop
599 : // them arming and flying:
600 376 : if (packet.until_maintenance == 0) {
601 0 : hal.util->snprintf(failmsg, failmsg_len, "LH: requires maintenance");
602 : }
603 :
604 : // check the user is trying to run the generator:
605 376 : if (pilot_desired_runstate != PilotDesiredRunState::RUN) {
606 168 : hal.util->snprintf(failmsg, failmsg_len, "LH: requested state is not RUN");
607 168 : return false;
608 : }
609 :
610 : // check ArduPilot is trying to run the generator:
611 208 : switch (commanded_runstate) {
612 152 : case RunState::RUN:
613 : // this is a good place to be
614 152 : break;
615 4 : case RunState::STOP:
616 : case RunState::IDLE:
617 : case RunState::COOLING_DOWN:
618 4 : break;
619 52 : case RunState::WARMING_UP:
620 52 : if (!isnan(packet.efi_clt)) {
621 52 : hal.util->snprintf(failmsg, failmsg_len, "LH: Generator warming up (%.0f%%)", ((packet.efi_clt*100.0) / temp_required_for_run));
622 : } else {
623 0 : hal.util->snprintf(failmsg, failmsg_len, "LH: Generator warming up (waiting for data)");
624 : }
625 52 : return false;
626 : }
627 :
628 156 : return true;
629 : }
630 :
631 : // handle mavlink packets received from the Loweheiser generator
632 : // control system. We keep an entire packet around for ease of use.
633 11205 : void AP_Generator_Loweheiser::handle_mavlink_msg(const GCS_MAVLINK &channel, const mavlink_message_t &msg)
634 : {
635 11205 : if (seen_good_message) {
636 : // ensure any subsequent messages we accept are only from the
637 : // sysid/compid tuple we initially saw:
638 11195 : if (msg.sysid != sysid || msg.compid != compid) {
639 0 : return;
640 : }
641 : }
642 :
643 : // good_message is set true if we discover a packet from a
644 : // Loweheiser generator.
645 11205 : bool good_message = false;
646 11205 : switch (msg.msgid) {
647 3815 : case MAVLINK_MSG_ID_LOWEHEISER_GOV_EFI: {
648 3815 : mavlink_msg_loweheiser_gov_efi_decode(&msg, &packet);
649 :
650 : // TODO: ensure from correct efi_number
651 3815 : efi_index = packet.efi_index;
652 :
653 3815 : const uint32_t now_ms = AP_HAL::millis();
654 3815 : const uint32_t delta_t_ms = now_ms - last_packet_received_ms;
655 3815 : if (last_packet_received_ms == 0 ||
656 : delta_t_ms > 30000) {
657 10 : gcs().send_text(MAV_SEVERITY_INFO, "LH: Found Loweheiser Generator");
658 : } else {
659 : // update the accumulated fuel, assuming we've been consuming
660 : // at the new rate since the last packet was received:
661 :
662 : // convert from litres/hour to litres/millisecond:
663 3805 : if (!isnan(packet.efi_fuel_flow)) {
664 3204 : const float litres_per_millisecond = packet.efi_fuel_flow/(60*60*1000);
665 3204 : accumulated_consumed_fuel_litres += litres_per_millisecond*delta_t_ms;
666 : }
667 : }
668 3815 : last_packet_received_ms = now_ms;
669 3815 : good_message = true;
670 3815 : break;
671 : }
672 7390 : case MAVLINK_MSG_ID_COMMAND_ACK:
673 : // only accept acks once we've seen a loweheiser packet:
674 7390 : if (!seen_good_message) {
675 0 : return;
676 : }
677 7390 : mavlink_msg_command_ack_decode(&msg, &ack_packet);
678 7390 : last_ack_packet_ms = AP_HAL::millis();
679 7390 : break;
680 : }
681 :
682 11205 : if (!good_message) {
683 7390 : return;
684 : }
685 3815 : if (seen_good_message) {
686 3805 : return;
687 : }
688 10 : seen_good_message = true;
689 :
690 10 : mavlink_channel = &channel;
691 10 : sysid = msg.sysid;
692 10 : compid = msg.compid;
693 : }
694 :
695 : // send mavlink generator status
696 4918 : void AP_Generator_Loweheiser::send_generator_status(const GCS_MAVLINK &channel)
697 : {
698 4918 : if (last_packet_received_ms == 0) {
699 : // nothing to report
700 0 : return;
701 : }
702 :
703 4918 : uint64_t status = 0;
704 4918 : switch (commanded_runstate) {
705 803 : case RunState::STOP:
706 : // we checking RPM here but confounded by starter motor running....
707 803 : status |= MAV_GENERATOR_STATUS_FLAG_OFF;
708 803 : break;
709 763 : case RunState::COOLING_DOWN:
710 : case RunState::IDLE:
711 763 : if (generator_ok_to_run()) {
712 464 : status |= MAV_GENERATOR_STATUS_FLAG_IDLE;
713 : }
714 763 : break;
715 2994 : case RunState::RUN:
716 2994 : status |= MAV_GENERATOR_STATUS_FLAG_GENERATING;
717 2994 : break;
718 358 : case RunState::WARMING_UP:
719 358 : status |= MAV_GENERATOR_STATUS_FLAG_WARMING_UP;
720 358 : break;
721 : }
722 :
723 : // note that the EFI unit can be turned off on the Loweheiser, in
724 : // which case we receive NaN values back. We translate these into
725 : // MAVLink "unknown" values throughout here:
726 :
727 9034 : if (!isnan(packet.efi_clt) &&
728 4116 : packet.efi_clt > temp_for_overtemp_warning) {
729 0 : status |= MAV_GENERATOR_STATUS_FLAG_OVERTEMP_WARNING;
730 : }
731 :
732 4918 : int16_t rectifier_temp = INT16_MAX;
733 4918 : if (!isnan(packet.rectifier_temp)) {
734 0 : rectifier_temp = packet.rectifier_temp;
735 : }
736 4918 : int16_t generator_temp = INT16_MAX;
737 4918 : if (!isnan(packet.generator_temp)) {
738 0 : generator_temp = packet.generator_temp;
739 : }
740 :
741 : // not all loweheiser firmwares provide runtime and
742 : // time-until-maintenance, so provide values we have in our
743 : // parameters for those:
744 4918 : uint32_t runtime = packet.runtime;
745 4918 : if (runtime == UINT32_MAX) {
746 4918 : runtime = total_runtime + runtime_delta_ms/1000;
747 : }
748 4918 : uint32_t maint_time = packet.until_maintenance;
749 4918 : if (maint_time == INT32_MAX) {
750 4918 : maint_time = time_until_maintenance - runtime_delta_ms/1000;
751 : }
752 :
753 4918 : mavlink_msg_generator_status_send(
754 : channel.get_chan(),
755 : status,
756 4918 : _rpm, // generator_speed
757 : packet.curr_batt, // Current into/out of battery
758 : packet.curr_gen, // load_current; Current going to UAV
759 4918 : (packet.curr_rot * _voltage), // power_generated (w)
760 : _voltage, // bus_voltage; Voltage of the bus seen at the generator
761 : rectifier_temp, // rectifier_temperature
762 4918 : std::numeric_limits<double>::quiet_NaN(), // bat_current_setpoint; The target battery current
763 : generator_temp, // generator temperature
764 : runtime, // runtime / time since boot,
765 : maint_time
766 : );
767 : }
768 :
769 : // methods to control the generator state:
770 12 : bool AP_Generator_Loweheiser::stop()
771 : {
772 12 : set_pilot_desired_runstate(PilotDesiredRunState::STOP);
773 12 : return true;
774 : }
775 :
776 8 : bool AP_Generator_Loweheiser::idle()
777 : {
778 8 : set_pilot_desired_runstate(PilotDesiredRunState::IDLE);
779 8 : return true;
780 : }
781 :
782 13 : bool AP_Generator_Loweheiser::run()
783 : {
784 13 : set_pilot_desired_runstate(PilotDesiredRunState::RUN);
785 13 : return true;
786 : }
787 :
788 : // log generator status to the onboard log
789 7410 : void AP_Generator_Loweheiser::Log_Write()
790 : {
791 : #if HAL_LOGGING_ENABLED
792 7410 : if (!AP::logger().should_log(MASK_LOG_ANY)) {
793 34 : return;
794 : }
795 :
796 : // only log new readings:
797 7376 : if (last_logged_reading_ms == last_packet_received_ms) {
798 3686 : return;
799 : }
800 3690 : last_logged_reading_ms = last_packet_received_ms;
801 :
802 : // @LoggerMessage: LOEG
803 : // @Description: Gathered Loweheiser EFI/Governor telemetry
804 : // @Field: TimeUS: Time since system startup
805 : // @Field: I: EFI/Gov sensor instance number
806 : // @Field: VB: battery voltage
807 : // @Field: CB: battery current
808 : // @Field: CG: generator current
809 : // @Field: Th: throttle input
810 : // @Field: EB: EFI battery voltage
811 : // @Field: RPM: generator RPM
812 : // @Field: PW: EFI pulse-width
813 : // @Field: FF: fuel flow
814 : // @Field: FC: fuel consumed
815 : // @Field: EP: EFI pressure
816 : // @Field: EMT: EFI manifold air temperature
817 : // @Field: CHT: cylinder head temperature
818 : // @Field: TPS: throttle position sensor
819 :
820 7380 : AP::logger().Write(
821 : "LOEG",
822 : "TimeUS," "I," "VB," "CB," "CG," "Th," "EB," "RPM," "PW," "FF," "FC," "EP," "EMT," "CHT," "TPS",
823 : "s" "#" "v" "A" "A" "%" "v" "q" "s" "y" "l" "P" "O" "O" "%" ,
824 : "F" "-" "-" "-" "-" "0" "0" "0" "F" "0" "0" "0" "0" "0" "0" ,
825 : "Q" "B" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" "f" ,
826 : AP_HAL::micros64(),
827 3690 : packet.efi_index,
828 3690 : packet.volt_batt,
829 3690 : packet.curr_batt,
830 3690 : packet.curr_gen,
831 3690 : packet.throttle,
832 3690 : packet.efi_batt,
833 3690 : packet.efi_rpm,
834 3690 : packet.efi_pw,
835 3690 : packet.efi_fuel_flow,
836 3690 : packet.efi_fuel_consumed,
837 3690 : packet.efi_baro,
838 3690 : packet.efi_mat,
839 3690 : packet.efi_clt,
840 3690 : packet.efi_tps
841 : );
842 : #endif // HAL_LOGGING_ENABLED
843 : }
844 :
845 : #endif
|
