forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 4
/
AP_Generator_IE_FuelCell.cpp
219 lines (178 loc) · 5.99 KB
/
AP_Generator_IE_FuelCell.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Generator_IE_FuelCell.h"
#if AP_GENERATOR_IE_ENABLED
#include <AP_SerialManager/AP_SerialManager.h>
#include <GCS_MAVLink/GCS.h>
// Initialize the fuelcell object and prepare it for use
void AP_Generator_IE_FuelCell::init()
{
_uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Generator, 0);
if (_uart == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: No serial port found");
return;
}
_uart->begin(AP::serialmanager().find_baudrate(AP_SerialManager::SerialProtocol_Generator, 0));
_health_warn_last_ms = AP_HAL::millis();
}
// Update fuelcell, expected to be called at 20hz
void AP_Generator_IE_FuelCell::update()
{
if (_uart == nullptr) {
return;
}
const uint32_t now = AP_HAL::millis();
// Read any available data
uint32_t nbytes = MIN(_uart->available(),30u);
while (nbytes-- > 0) {
const int16_t c = _uart->read();
if (c < 0) {
// Nothing to decode
break;
}
if (!decode(c)) {
// Sentence not yet valid, don't assign state and output
continue;
}
// We have a valid sentence, write the parsed values to unit specific measurements
assign_measurements(now);
}
_healthy = (now - _last_time_ms) < HEALTHY_TIMEOUT_MS;
// Check if we should notify gcs off any change of fuel cell state
check_status(now);
update_frontend();
#if HAL_LOGGING_ENABLED
log_write();
#endif
}
// Add a single character to the buffer and attempt to decode
// Returns true if a complete sentence was successfully decoded
bool AP_Generator_IE_FuelCell::decode(char c)
{
// Start of a string
if ((c == '<') || (c == '[')) {
_start_char = c;
_sentence_valid = false;
_data_valid = true;
_term_number = 0;
_term_offset = 0;
_in_string = true;
_checksum = c;
return false;
}
if (!_in_string) {
return false;
}
// End of a string
const char end_char = (_start_char == '[') ? ']' : '>';
if (c == end_char) {
decode_latest_term();
_in_string = false;
return _sentence_valid;
}
// End of a term in the string
if (c == ',') {
decode_latest_term();
_checksum += c;
return false;
}
// Otherwise add the char to the current term
_term[_term_offset++] = c;
_checksum += c;
// We have overrun the expected sentence
if (_term_offset >TERM_BUFFER) {
_in_string = false;
}
return false;
}
// Check for arming
bool AP_Generator_IE_FuelCell::pre_arm_check(char *failmsg, uint8_t failmsg_len) const
{
// Refuse arming if not healthy
if (!healthy()) {
strncpy(failmsg, "Not healthy", failmsg_len);
return false;
}
// Refuse arming if not in running state
if (!is_running()) {
strncpy(failmsg, "Status not running", failmsg_len);
return false;
}
// Check for error codes
if (check_for_err_code(failmsg, failmsg_len)) {
return false;
}
return true;
}
// Lookup table for running state. State code is the same for all IE units.
const AP_Generator_IE_FuelCell::Lookup_State AP_Generator_IE_FuelCell::lookup_state[] = {
{ State::STARTING,"Starting"},
{ State::READY,"Ready"},
{ State::RUNNING,"Running"},
{ State::FAULT,"Fault"},
{ State::BATTERY_ONLY,"Battery Only"},
};
// Check for any change in error state or status and report to gcs
void AP_Generator_IE_FuelCell::check_status(const uint32_t now)
{
// Check driver health
if (!healthy() && (!_health_warn_last_ms || (now - _health_warn_last_ms >= 20000))) {
// Don't spam GCS with unhealthy message
_health_warn_last_ms = now;
GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "Generator: Not healthy");
} else if (healthy()) {
_health_warn_last_ms = 0;
}
// If fuel cell state has changed send gcs message
update_state_msg();
// Check error codes
check_for_err_code_if_changed();
}
// Check error codes and populate message with error code
void AP_Generator_IE_FuelCell::check_for_err_code_if_changed()
{
// Only check if there has been a change in error code
if ((_err_code == _last_err_code) && (_sub_err_code == _last_sub_err_code)) {
return;
}
#if HAL_GCS_ENABLED
char msg_txt[64];
if (check_for_err_code(msg_txt, sizeof(msg_txt)) || check_for_warning_code(msg_txt, sizeof(msg_txt))) {
GCS_SEND_TEXT(get_mav_severity(_err_code), "%s", msg_txt);
} else if ((_err_code == 0) && (_sub_err_code == 0)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fuel cell error cleared");
}
#endif
_last_err_code = _err_code;
_last_sub_err_code = _sub_err_code;
}
// Return true is fuel cell is in running state suitable for arming
bool AP_Generator_IE_FuelCell::is_running() const
{
return _state == State::RUNNING;
}
// Print msg to user updating on state change
void AP_Generator_IE_FuelCell::update_state_msg()
{
// If fuel cell state has changed send gcs message
if (_state != _last_state) {
for (const struct Lookup_State entry : lookup_state) {
if (_state == entry.option) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: %s", entry.msg_txt);
break;
}
}
_last_state = _state;
}
}
#endif // AP_GENERATOR_IE_ENABLED