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 : #pragma GCC optimize("Os")
17 :
18 : #include "AP_Generator_IE_2400.h"
19 :
20 : #if AP_GENERATOR_IE_2400_ENABLED
21 :
22 : #include <AP_Logger/AP_Logger.h>
23 : #include <GCS_MAVLink/GCS.h>
24 :
25 : extern const AP_HAL::HAL& hal;
26 :
27 4 : void AP_Generator_IE_2400::init()
28 : {
29 : // Call init from base class to do common setup
30 4 : AP_Generator_IE_FuelCell::init();
31 :
32 : // Tell frontend what measurements are available for this generator
33 4 : _frontend._has_current = true;
34 4 : _frontend._has_consumed_energy = true;
35 4 : _frontend._has_fuel_remaining = true;
36 4 : }
37 :
38 : // Assigns the unit specific measurements once a valid sentence is obtained
39 719 : void AP_Generator_IE_2400::assign_measurements(const uint32_t now)
40 : {
41 :
42 719 : if (_type == PacketType::V2_INFO) {
43 : // Got info packet
44 31 : if (_had_info) {
45 : // Not expecting the version to change
46 : return;
47 : }
48 2 : _had_info = true;
49 :
50 : // Info tells us the protocol version, so lock on straight away
51 2 : if (_version == ProtocolVersion::DETECTING) {
52 2 : if (strcmp(_info.Protocol_version, "4") == 0) {
53 2 : _version = ProtocolVersion::V2;
54 : } else {
55 : // Got a valid info packet, but don't know this protocol version
56 : // Give up
57 0 : _version = ProtocolVersion::UNKNOWN;
58 : }
59 : }
60 :
61 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IE Fuel cell detected, PCM: %s, Ver: %s, SN: %s", _info.PCM_number, _info.Software_version, _info.Serial_number);
62 :
63 2 : return;
64 : }
65 :
66 : // Try and lock onto version
67 688 : if (_version == ProtocolVersion::DETECTING) {
68 : ProtocolVersion new_version = ProtocolVersion::DETECTING;
69 64 : switch (_type) {
70 0 : case PacketType::NONE:
71 : // Should not get a valid packet of type none
72 0 : _last_version_packet_count = 0;
73 0 : return;
74 :
75 44 : case PacketType::LEGACY_DATA:
76 : new_version = ProtocolVersion::LEGACY;
77 44 : break;
78 :
79 20 : case PacketType::V2_DATA:
80 : case PacketType::V2_INFO:
81 : new_version = ProtocolVersion::V2;
82 20 : break;
83 : }
84 :
85 64 : if (_last_version == new_version) {
86 60 : _last_version_packet_count++;
87 : } else {
88 4 : _last_version_packet_count = 0;
89 : }
90 64 : _last_version = new_version;
91 :
92 : // If received 20 valid packets for a single protocol version then lock on
93 64 : if (_last_version_packet_count > 20) {
94 2 : _version = new_version;
95 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: IE using %s protocol", (_version == ProtocolVersion::V2) ? "V2" : "legacy" );
96 :
97 : } else {
98 : // Don't record any data during version detection
99 : return;
100 : }
101 : }
102 :
103 626 : if (_type == PacketType::V2_DATA) {
104 326 : memcpy(&_valid_V2, &_parsed_V2, sizeof(_valid_V2));
105 : }
106 :
107 : // Update internal fuel cell state
108 626 : _pwr_out = _parsed.pwr_out;
109 626 : _spm_pwr = _parsed.spm_pwr;
110 626 : _battery_pwr = _parsed.battery_pwr;
111 :
112 626 : _state = (State)_parsed.state;
113 626 : _v2_state = (V2_State)_parsed.state;
114 626 : _err_code = _parsed.err_code;
115 626 : _sub_err_code = _parsed.sub_err_code;
116 :
117 626 : _fuel_remaining = _fuel_rem;
118 :
119 : // Update battery voltage
120 626 : _voltage = _parsed.battery_volt;
121 :
122 : /* Calculate battery current. Convention: +current battery is discharging, -current
123 : battery is charging. This is aligned with normal AP behaviour. This is the opposite
124 : of IE's convention hence *-1 */
125 626 : if (_parsed.battery_volt > 0) {
126 624 : _current = -1 * _battery_pwr / _parsed.battery_volt;
127 : } else {
128 2 : _current = 0;
129 : }
130 :
131 : // Calculate consumed current
132 626 : _consumed_mah += _current * (now - _last_time_ms) * AMS_TO_MAH;
133 :
134 626 : _last_time_ms = now;
135 : }
136 :
137 : // Process characters received and extract terms for IE 2.4kW
138 6701 : void AP_Generator_IE_2400::decode_latest_term()
139 : {
140 : // Null terminate and move onto the next term
141 6701 : _term[_term_offset] = 0;
142 6701 : _term_offset = 0;
143 6701 : _term_number++;
144 6701 : _type = PacketType::NONE;
145 :
146 6701 : if (_start_char == '<') {
147 6546 : decode_data_packet();
148 :
149 155 : } else if (_start_char == '[') {
150 155 : decode_info_packet();
151 :
152 : } else {
153 0 : _sentence_valid = false;
154 :
155 : }
156 6701 : }
157 :
158 6546 : void AP_Generator_IE_2400::decode_data_packet()
159 : {
160 : // Try and decode both protocol versions until locked on
161 6546 : if ((_version == ProtocolVersion::LEGACY) || (_version == ProtocolVersion::DETECTING)) {
162 2634 : decode_legacy_data();
163 : }
164 6546 : if ((_version == ProtocolVersion::V2) || (_version == ProtocolVersion::DETECTING)) {
165 4460 : decode_v2_data();
166 : }
167 6546 : }
168 :
169 2634 : void AP_Generator_IE_2400::decode_legacy_data()
170 : {
171 2634 : switch (_term_number) {
172 362 : case 1: {
173 : // Float
174 362 : _parsed.tank_bar = strtof(_term, NULL);
175 :
176 : // Scale tank pressure linearly to a value between 0 and 1
177 : // Min = 5 bar, max = 300 bar, PRESS_GRAD = 1/295.
178 : const float PRESS_GRAD = 0.003389830508f;
179 362 : _fuel_rem = constrain_float((_parsed.tank_bar-5)*PRESS_GRAD,0,1);
180 362 : break;
181 : }
182 362 : case 2:
183 : // Float
184 362 : _parsed.battery_volt = strtof(_term, NULL);
185 362 : break;
186 :
187 362 : case 3:
188 : // Signed int base 10
189 362 : _parsed.pwr_out = strtol(_term, nullptr, 10);
190 362 : break;
191 :
192 362 : case 4:
193 : // Unsigned int base 10
194 362 : _parsed.spm_pwr = strtoul(_term, nullptr, 10);
195 362 : break;
196 :
197 362 : case 5:
198 : // Signed int base 10
199 362 : _parsed.battery_pwr = strtol(_term, nullptr, 10);
200 362 : break;
201 :
202 362 : case 6:
203 : // Unsigned int base 10
204 362 : _parsed.state = strtoul(_term, nullptr, 10);
205 362 : break;
206 :
207 362 : case 7:
208 : // Unsigned int base 10
209 362 : _parsed.err_code = strtoul(_term, nullptr, 10);
210 : // Sentence only declared valid when we have the expected number of terms
211 362 : _sentence_valid = true;
212 362 : _type = PacketType::LEGACY_DATA;
213 362 : break;
214 :
215 100 : default:
216 : // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence
217 100 : _sentence_valid = false;
218 100 : break;
219 : }
220 2634 : }
221 :
222 4460 : void AP_Generator_IE_2400::decode_v2_data()
223 : {
224 4460 : switch (_term_number) {
225 390 : case 1:
226 390 : _fuel_rem = strtof(_term, NULL) * 0.01;
227 390 : break;
228 :
229 390 : case 2:
230 390 : _parsed_V2.inlet_press = strtof(_term, NULL);
231 390 : break;
232 :
233 390 : case 3:
234 390 : _parsed.battery_volt = strtof(_term, NULL);
235 390 : break;
236 :
237 390 : case 4:
238 390 : _parsed.pwr_out = strtol(_term, nullptr, 10);
239 390 : break;
240 :
241 390 : case 5:
242 390 : _parsed.spm_pwr = strtoul(_term, nullptr, 10);
243 390 : break;
244 :
245 390 : case 6:
246 390 : _parsed_V2.unit_fault = strtoul(_term, nullptr, 10);
247 390 : break;
248 :
249 390 : case 7:
250 390 : _parsed.battery_pwr = strtol(_term, nullptr, 10);
251 390 : break;
252 :
253 346 : case 8:
254 346 : _parsed.state = strtoul(_term, nullptr, 10);
255 346 : break;
256 :
257 346 : case 9:
258 346 : _parsed.err_code = strtoul(_term, nullptr, 10);
259 346 : break;
260 :
261 346 : case 10:
262 346 : _parsed.sub_err_code = strtoul(_term, nullptr, 10);
263 346 : break;
264 :
265 346 : case 11:
266 346 : strncpy(_parsed_V2.info_str, _term, ARRAY_SIZE(_parsed_V2.info_str));
267 346 : break;
268 :
269 346 : case 12: {
270 : // The inverted checksum is sent, un-invert it
271 346 : uint8_t checksum = ~strtoul(_term, nullptr, 10);
272 :
273 : // Sent checksum only included characters up to the checksum term
274 : // Add on the checksum terms to match our running total
275 1044 : for (uint8_t i = 0; i < ARRAY_SIZE(_term); i++) {
276 1044 : if (_term[i] == 0) {
277 : break;
278 : }
279 698 : checksum += _term[i];
280 : }
281 :
282 346 : _sentence_valid = checksum == _checksum;
283 346 : _type = PacketType::V2_DATA;
284 346 : break;
285 : }
286 :
287 0 : default:
288 : // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence
289 0 : _sentence_valid = false;
290 0 : break;
291 : }
292 4460 : }
293 :
294 :
295 155 : void AP_Generator_IE_2400::decode_info_packet()
296 : {
297 :
298 155 : switch (_term_number) {
299 31 : case 1:
300 : // PCM software number
301 31 : strncpy(_info.PCM_number, _term, ARRAY_SIZE(_info.PCM_number));
302 31 : break;
303 :
304 31 : case 2:
305 : // Software version
306 31 : strncpy(_info.Software_version, _term, ARRAY_SIZE(_info.Software_version));
307 31 : break;
308 :
309 31 : case 3:
310 : // protocol version
311 31 : strncpy(_info.Protocol_version, _term, ARRAY_SIZE(_info.Protocol_version));
312 31 : break;
313 :
314 31 : case 4:
315 : // Hardware serial number
316 31 : strncpy(_info.Serial_number, _term, ARRAY_SIZE(_info.Serial_number));
317 31 : break;
318 :
319 31 : case 5: {
320 : // The inverted checksum is sent, un-invert it
321 31 : uint8_t checksum = ~strtoul(_term, nullptr, 10);
322 :
323 : // Sent checksum only included characters up to the checksum term
324 : // Add on the checksum terms to match our running total
325 62 : for (uint8_t i = 0; i < ARRAY_SIZE(_term); i++) {
326 62 : if (_term[i] == 0) {
327 : break;
328 : }
329 31 : checksum += _term[i];
330 : }
331 :
332 31 : _sentence_valid = checksum == _checksum;
333 31 : _type = PacketType::V2_INFO;
334 31 : break;
335 : }
336 :
337 0 : default:
338 : // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence
339 0 : _sentence_valid = false;
340 0 : break;
341 : }
342 :
343 155 : }
344 :
345 : // Check for failsafes
346 1472 : AP_BattMonitor::Failsafe AP_Generator_IE_2400::update_failsafes() const
347 : {
348 : // Check for error codes that lead to critical action battery monitor failsafe
349 1472 : if (is_critical_error(_err_code)) {
350 : return AP_BattMonitor::Failsafe::Critical;
351 : }
352 :
353 : // Check for error codes that lead to low action battery monitor failsafe
354 1471 : if (is_low_error(_err_code)) {
355 1 : return AP_BattMonitor::Failsafe::Low;
356 : }
357 :
358 : return AP_BattMonitor::Failsafe::None;
359 : }
360 :
361 : // Check for error codes that are deemed critical
362 1644 : bool AP_Generator_IE_2400::is_critical_error(const uint32_t err_in) const
363 : {
364 : // V2 protocol
365 1644 : if (_version == ProtocolVersion::V2) {
366 837 : return err_in >= 30;
367 : }
368 :
369 : // V1 protocol
370 807 : switch ((ErrorCode)err_in) {
371 : // Error codes that lead to critical action battery monitor failsafe
372 : case ErrorCode::BATTERY_CRITICAL:
373 : case ErrorCode::PRESSURE_CRITICAL:
374 : case ErrorCode::SYSTEM_CRITICAL:
375 : return true;
376 :
377 807 : default:
378 : // Minor internal error is always ignored and caught by the default
379 807 : return false;
380 : }
381 : }
382 :
383 : // Check for error codes that are deemed severe and would be cause to trigger a battery monitor low failsafe action
384 1641 : bool AP_Generator_IE_2400::is_low_error(const uint32_t err_in) const
385 : {
386 : // V2 protocol
387 1641 : if (_version == ProtocolVersion::V2) {
388 834 : return (err_in > 20) && (err_in < 30);
389 : }
390 :
391 : // V1 protocol
392 807 : switch ((ErrorCode)err_in) {
393 : // Error codes that lead to critical action battery monitor failsafe
394 : case ErrorCode::START_DENIED:
395 : case ErrorCode::PRESSURE_ALERT:
396 : case ErrorCode::BATTERY_LOW:
397 : case ErrorCode::PRESSURE_LOW:
398 : case ErrorCode::SPM_LOST:
399 : return true;
400 :
401 804 : default:
402 : // Minor internal error is always ignored and caught by the default
403 804 : return false;
404 : }
405 : }
406 :
407 : // Check error codes and populate message with error code
408 172 : bool AP_Generator_IE_2400::check_for_err_code(char* msg_txt, uint8_t msg_len) const
409 : {
410 : // Check if we have received an error code
411 172 : if (!is_critical_error(_err_code) && !is_low_error(_err_code)) {
412 : return false;
413 : }
414 :
415 4 : if (_version == ProtocolVersion::V2) {
416 2 : hal.util->snprintf(msg_txt, msg_len, "Fuel cell err %u.%u: %s", (unsigned)_err_code, (unsigned)_sub_err_code, _valid_V2.info_str);
417 2 : return true;
418 : }
419 :
420 2 : hal.util->snprintf(msg_txt, msg_len, "Fuel cell err code <%u>", (unsigned)_err_code);
421 2 : return true;
422 : }
423 :
424 2 : bool AP_Generator_IE_2400::check_for_warning_code(char* msg_txt, uint8_t msg_len) const
425 : {
426 2 : if (_err_code == 0) {
427 : // No error nothing to do.
428 : return false;
429 : }
430 0 : if (is_critical_error(_err_code) || is_low_error(_err_code)) {
431 : // Critical or low error are already reported
432 : return false;
433 : }
434 :
435 0 : switch (_version) {
436 : case ProtocolVersion::DETECTING:
437 : case ProtocolVersion::UNKNOWN:
438 : break;
439 :
440 0 : case ProtocolVersion::LEGACY:
441 0 : if ((ErrorCode)_err_code == ErrorCode::REDUCED_POWER) {
442 0 : hal.util->snprintf(msg_txt, msg_len, "Fuel cell reduced power <%u>", (unsigned)_err_code);
443 0 : return true;
444 : }
445 : break;
446 :
447 0 : case ProtocolVersion::V2:
448 0 : hal.util->snprintf(msg_txt, msg_len, "Fuel cell warning %u.%u: %s", (unsigned)_err_code, (unsigned)_sub_err_code, _valid_V2.info_str);
449 0 : return true;
450 : }
451 :
452 0 : hal.util->snprintf(msg_txt, msg_len, "Fuel cell warning code <%u>", (unsigned)_err_code);
453 0 : return true;
454 : }
455 :
456 : // Check if we should notify on any change of fuel cell state
457 3392 : void AP_Generator_IE_2400::check_status(const uint32_t now)
458 : {
459 3392 : if (ErrorCode(_err_code) == ErrorCode::REDUCED_POWER) {
460 0 : if (now - _last_low_power_warning_ms > 5000) {
461 0 : gcs().send_text(MAV_SEVERITY_INFO, "Fuel cell: reduced power; conditioning needed?");
462 0 : _last_low_power_warning_ms = now;
463 : }
464 : }
465 3392 : return AP_Generator_IE_FuelCell::check_status(now);
466 : }
467 :
468 : #if HAL_LOGGING_ENABLED
469 : // log generator status to the onboard log
470 3392 : void AP_Generator_IE_2400::log_write()
471 : {
472 : #define MASK_LOG_ANY 0xFFFF
473 3392 : if (!AP::logger().should_log(MASK_LOG_ANY)) {
474 : return;
475 : }
476 :
477 3392 : switch (_version) {
478 : case ProtocolVersion::DETECTING:
479 : case ProtocolVersion::UNKNOWN:
480 : return;
481 :
482 1516 : case ProtocolVersion::LEGACY:
483 : // @LoggerMessage: IE24
484 : // @Description: Intelligent Energy Fuel Cell generator (legacy protocol)
485 : // @URL: https://ardupilot.club/copter/docs/common-ie24-fuelcell.html
486 : // @Field: TimeUS: Time since system startup
487 : // @Field: FUEL: Fuel remaining
488 : // @Field: SPMPWR: stack power module power draw
489 : // @Field: POUT: output power
490 : // @Field: ERR: error codes
491 : // @FieldValueEnum: ERR: AP_Generator_IE_2400::ErrorCode
492 3032 : AP::logger().WriteStreaming(
493 : "IE24",
494 : "TimeUS,FUEL,SPMPWR,POUT,ERR",
495 : "s%WW-",
496 : "F2---",
497 : "Qfiii",
498 : AP_HAL::micros64(),
499 1516 : _fuel_remaining,
500 1516 : _spm_pwr,
501 1516 : _pwr_out,
502 : _err_code
503 : );
504 1516 : break;
505 :
506 1646 : case ProtocolVersion::V2:
507 : // @LoggerMessage: IEFC
508 : // @Description: Intelligent Energy Fuel Cell generator
509 : // @URL: https://ardupilot.club/copter/docs/common-ie24-fuelcell.html
510 : // @Field: TimeUS: Time since system startup
511 : // @Field: Tank: Fuel remaining
512 : // @Field: Inlet: Inlet pressure
513 : // @Field: BattV: battery voltage
514 : // @Field: OutPwr: output power
515 : // @Field: SPMPwr: stack power module power draw
516 : // @Field: FNo: fault number
517 : // @Field: BPwr: battery power draw
518 : // @Field: State: generator state
519 : // @FieldValueEnum: State: AP_Generator_IE_2400::V2_State
520 : // @Field: F1: error code
521 : // @FieldValueEnum: F1: AP_Generator_IE_2400::ErrorCode
522 : // @Field: F2: sub-error code
523 3292 : AP::logger().WriteStreaming(
524 : "IEFC",
525 : "TimeUS,Tank,Inlet,BattV,OutPwr,SPMPwr,FNo,BPwr,State,F1,F2",
526 : "s%-vWW-W---",
527 : "F----------",
528 : "QfffhHBhBII",
529 : AP_HAL::micros64(),
530 1646 : _fuel_remaining,
531 1646 : _valid_V2.inlet_press,
532 1646 : _voltage,
533 1646 : _pwr_out,
534 1646 : _spm_pwr,
535 1646 : _valid_V2.unit_fault,
536 1646 : _battery_pwr,
537 1646 : uint8_t(_v2_state),
538 : _err_code,
539 : _sub_err_code
540 : );
541 1646 : break;
542 : }
543 :
544 : }
545 : #endif // HAL_LOGGING_ENABLED
546 :
547 : // Return true is fuel cell is in running state suitable for arming
548 182 : bool AP_Generator_IE_2400::is_running() const
549 : {
550 182 : switch (_version) {
551 : case ProtocolVersion::DETECTING:
552 : case ProtocolVersion::UNKNOWN:
553 : return false;
554 :
555 82 : case ProtocolVersion::LEGACY:
556 : // Can use the base class method
557 82 : return AP_Generator_IE_FuelCell::is_running();
558 :
559 92 : case ProtocolVersion::V2:
560 92 : return _v2_state == V2_State::Running;
561 : }
562 :
563 : return false;
564 : }
565 :
566 : // Lookup table for running state. State code is the same for all IE units.
567 : const AP_Generator_IE_2400::Lookup_State_V2 AP_Generator_IE_2400::lookup_state_V2[] = {
568 : { V2_State::FCPM_Off, "FCPM Off"},
569 : { V2_State::Starting, "Starting"},
570 : { V2_State::Running, "Running"},
571 : { V2_State::Stopping, "Stopping"},
572 : { V2_State::Go_to_Sleep, "Sleep"},
573 : };
574 :
575 : // Print msg to user updating on state change
576 3392 : void AP_Generator_IE_2400::update_state_msg()
577 : {
578 3392 : switch (_version) {
579 : case ProtocolVersion::DETECTING:
580 : case ProtocolVersion::UNKNOWN:
581 : break;
582 :
583 1516 : case ProtocolVersion::LEGACY:
584 : // Can use the base class method
585 1516 : AP_Generator_IE_FuelCell::update_state_msg();
586 1516 : break;
587 :
588 1646 : case ProtocolVersion::V2: {
589 : // If fuel cell state has changed send gcs message
590 1646 : if (_v2_state != _last_v2_state) {
591 15 : for (const struct Lookup_State_V2 entry : lookup_state_V2) {
592 14 : if (_v2_state == entry.option) {
593 3 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: %s", entry.msg_txt);
594 3 : break;
595 : }
596 : }
597 4 : _last_v2_state = _v2_state;
598 : }
599 : break;
600 : }
601 : }
602 3392 : }
603 :
604 : #if HAL_GCS_ENABLED
605 : // Get the MAV_SEVERITY level of a given error code
606 2 : MAV_SEVERITY AP_Generator_IE_2400::get_mav_severity(uint32_t err_code) const
607 : {
608 2 : if (err_code <= 9) {
609 : return MAV_SEVERITY_INFO;
610 : }
611 2 : if (err_code <= 20) {
612 0 : return MAV_SEVERITY_WARNING;
613 : }
614 : return MAV_SEVERITY_CRITICAL;
615 : }
616 : #endif // HAL_GCS_ENABLED
617 :
618 : #endif // AP_GENERATOR_IE_2400_ENABLED
|
