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_config.h"
19 :
20 : #if AP_GENERATOR_RICHENPOWER_ENABLED
21 :
22 : #include "AP_Generator_RichenPower.h"
23 :
24 : #include <AP_Logger/AP_Logger.h>
25 : #include <AP_SerialManager/AP_SerialManager.h>
26 : #include <AP_Vehicle/AP_Vehicle.h>
27 : #include <GCS_MAVLink/GCS.h>
28 : #include <SRV_Channel/SRV_Channel.h>
29 :
30 : #include <AP_HAL/utility/sparse-endian.h>
31 :
32 : extern const AP_HAL::HAL& hal;
33 :
34 : // init method; configure communications with the generator
35 2 : void AP_Generator_RichenPower::init()
36 : {
37 : ASSERT_STORAGE_SIZE(RichenPacket, 70);
38 :
39 2 : const AP_SerialManager &serial_manager = AP::serialmanager();
40 :
41 2 : uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Generator, 0);
42 2 : if (uart != nullptr) {
43 2 : const uint32_t baud = serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Generator, 0);
44 2 : uart->begin(baud, 256, 256);
45 : }
46 :
47 : // Tell frontend what measurements are available for this generator
48 2 : _frontend._has_current = true;
49 2 : _frontend._has_consumed_energy = false;
50 2 : _frontend._has_fuel_remaining = false;
51 2 : }
52 :
53 : // find a RichenPower message in the buffer, starting at
54 : // initial_offset. If dound, that message (or partial message) will
55 : // be moved to the start of the buffer.
56 93 : void AP_Generator_RichenPower::move_header_in_buffer(uint8_t initial_offset)
57 : {
58 : uint8_t header_offset;
59 93 : for (header_offset=initial_offset; header_offset<body_length; header_offset++) {
60 93 : if (u.parse_buffer[header_offset] == HEADER_MAGIC1) {
61 : break;
62 : }
63 : }
64 93 : if (header_offset != 0) {
65 : // header was found, but not at index 0; move it back to start of array
66 0 : memmove(u.parse_buffer, &u.parse_buffer[header_offset], body_length - header_offset);
67 0 : body_length -= header_offset;
68 : }
69 93 : }
70 :
71 : // read - read serial port, return true if a new reading has been found
72 938 : bool AP_Generator_RichenPower::get_reading()
73 : {
74 : // Example of a packet from a H2 controller:
75 : //AA 55 00 0A 00 00 00 00 00 04 1E B0 00 10 00 00 23 7A 23
76 : //7A 11 1D 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00
77 : //00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
78 : //00 00 00 00 00 00 00 00 00 1E BE 55 AA
79 : //16 bit one data Hight + Low
80 :
81 : // fill our buffer some more:
82 938 : uint32_t nbytes = uart->read(&u.parse_buffer[body_length],
83 938 : ARRAY_SIZE(u.parse_buffer)-body_length);
84 938 : if (nbytes == 0) {
85 : return false;
86 : }
87 93 : body_length += nbytes;
88 :
89 93 : move_header_in_buffer(0);
90 :
91 : // header byte 1 is correct.
92 93 : if (body_length < ARRAY_SIZE(u.parse_buffer)) {
93 : // need a full buffer to have a valid message...
94 : return false;
95 : }
96 :
97 93 : if (u.packet.headermagic2 != HEADER_MAGIC2) {
98 0 : move_header_in_buffer(1);
99 0 : return false;
100 : }
101 :
102 : // check for the footer signature:
103 93 : if (u.packet.footermagic1 != FOOTER_MAGIC1) {
104 0 : move_header_in_buffer(1);
105 0 : return false;
106 : }
107 93 : if (u.packet.footermagic2 != FOOTER_MAGIC2) {
108 0 : move_header_in_buffer(1);
109 0 : return false;
110 : }
111 :
112 : // calculate checksum....
113 : uint16_t checksum = 0;
114 93 : const uint8_t *checksum_buffer = &u.parse_buffer[2];
115 558 : for (uint8_t i=0; i<5; i++) {
116 465 : checksum += be16toh_ptr(&checksum_buffer[2*i]);
117 : }
118 :
119 93 : if (checksum != be16toh(u.packet.checksum)) {
120 0 : move_header_in_buffer(1);
121 0 : return false;
122 : }
123 :
124 : // check the version:
125 93 : const uint16_t version = be16toh(u.packet.version);
126 93 : const uint8_t major = version / 100;
127 93 : const uint8_t minor = (version % 100) / 10;
128 93 : const uint8_t point = version % 10;
129 93 : if (!protocol_information_anounced) {
130 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "RichenPower: protocol %u.%u.%u", major, minor, point);
131 1 : protocol_information_anounced = true;
132 : }
133 :
134 93 : last_reading.runtime = be16toh(u.packet.runtime_hours) * 3600 +
135 93 : u.packet.runtime_minutes * 60 +
136 93 : u.packet.runtime_seconds;
137 93 : last_reading.seconds_until_maintenance = be16toh(u.packet.seconds_until_maintenance_high) * 65536 + be16toh(u.packet.seconds_until_maintenance_low);
138 93 : last_reading.errors = be16toh(u.packet.errors);
139 93 : last_reading.rpm = be16toh(u.packet.rpm);
140 93 : last_reading.output_voltage = be16toh(u.packet.output_voltage) * 0.01f;
141 93 : last_reading.output_current = be16toh(u.packet.output_current) * 0.01f;
142 93 : last_reading.mode = (Mode)u.packet.mode;
143 :
144 93 : last_reading_ms = AP_HAL::millis();
145 :
146 93 : body_length = 0;
147 :
148 : // update the time we started idling at:
149 93 : if (last_reading.mode == Mode::IDLE) {
150 59 : if (idle_state_start_ms == 0) {
151 2 : idle_state_start_ms = last_reading_ms;
152 : }
153 : } else {
154 34 : idle_state_start_ms = 0;
155 : }
156 :
157 : return true;
158 : }
159 :
160 : // update the synthetic heat measurement we are keeping for the
161 : // generator. We keep a synthetic heat measurement as the telemetry
162 : // from the unit does not include the motor temperature, so we
163 : // approximate one by assuming it produces heat in proportion to its
164 : // current RPM.
165 938 : void AP_Generator_RichenPower::update_heat()
166 : {
167 : // assume heat increase is directly proportional to RPM.
168 938 : const uint32_t now = AP_HAL::millis();
169 938 : uint16_t rpm = last_reading.rpm;
170 938 : if (now - last_reading_ms > 2000) {
171 : // if we're not getting updates, assume we're getting colder
172 : rpm = 0;
173 : // ... and resend the version information when we get something again
174 20 : protocol_information_anounced = false;
175 : }
176 :
177 938 : const uint32_t time_delta_ms = now - last_heat_update_ms;
178 938 : last_heat_update_ms = now;
179 :
180 938 : heat += rpm * time_delta_ms * (1/1000.0f);
181 : // cap the heat of the motor:
182 938 : heat = MIN(heat, 60 * RUN_RPM); // so cap heat at 60 seconds at run-speed
183 : // now lose some heat to the environment
184 938 : heat -= (heat * heat_environment_loss_factor * (time_delta_ms * (1/1000.0f))); // lose some % of heat per second
185 938 : }
186 :
187 : // returns true if the generator should be allowed to move into
188 : // the "run" (high-RPM) state:
189 267 : bool AP_Generator_RichenPower::generator_ok_to_run() const
190 : {
191 267 : return heat > heat_required_for_run();
192 : }
193 :
194 : // returns an amount of synthetic heat required for the generator
195 : // to move into the "run" state:
196 293 : constexpr float AP_Generator_RichenPower::heat_required_for_run()
197 : {
198 : // assume that heat is proportional to RPM. Return a number
199 : // proportional to RPM. Reduce it to account for the cooling some%/s
200 : // cooling
201 293 : return (45 * IDLE_RPM) * heat_environment_loss_30s;
202 : }
203 :
204 :
205 917 : void AP_Generator_RichenPower::check_maintenance_required()
206 : {
207 : // don't bother the user while flying:
208 917 : if (hal.util->get_soft_armed()) {
209 : return;
210 : }
211 :
212 917 : if (!AP::generator()->option_set(AP_Generator::Option::INHIBIT_MAINTENANCE_WARNINGS)) {
213 917 : const uint32_t now = AP_HAL::millis();
214 :
215 917 : if (last_reading.errors & (1U<<uint16_t(Errors::MaintenanceRequired))) {
216 0 : if (now - last_maintenance_warning_ms > 60000) {
217 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Generator: requires maintenance");
218 0 : last_maintenance_warning_ms = now;
219 : }
220 : }
221 : }
222 : }
223 :
224 : /*
225 : update the state of the sensor
226 : */
227 938 : void AP_Generator_RichenPower::update(void)
228 : {
229 938 : if (uart == nullptr) {
230 : return;
231 : }
232 :
233 938 : if (last_reading_ms != 0) {
234 917 : update_runstate();
235 917 : check_maintenance_required();
236 : }
237 :
238 938 : (void)get_reading();
239 :
240 938 : update_heat();
241 :
242 938 : update_frontend_readings();
243 :
244 : #if HAL_LOGGING_ENABLED
245 938 : Log_Write();
246 : #endif
247 : }
248 :
249 : // update_runstate updates the servo output we use to control the
250 : // generator. Which state we request the generator move to depends on
251 : // the RC inputcontrol and the temperature the generator is at.
252 917 : void AP_Generator_RichenPower::update_runstate()
253 : {
254 : // don't run the generator while the safety is on:
255 : // if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
256 : // _servo_channel->set_output_pwm(SERVO_PWM_STOP);
257 : // return;
258 : // }
259 :
260 : static const uint16_t SERVO_PWM_STOP = 1200;
261 : static const uint16_t SERVO_PWM_IDLE = 1500;
262 : static const uint16_t SERVO_PWM_RUN = 1900;
263 :
264 : // if the vehicle crashes then we assume the pilot wants to stop
265 : // the motor. This is done as a once-off when the crash is
266 : // detected to allow the operator to rearm the vehicle, or we end
267 : // up in a catch-22 situation where we force the stop state on the
268 : // generator so they can't arm and can't start the generator
269 : // because the vehicle is crashed.
270 917 : if (AP::vehicle()->is_crashed()) {
271 0 : if (!vehicle_was_crashed) {
272 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Crash; stopping generator");
273 0 : pilot_desired_runstate = RunState::STOP;
274 0 : vehicle_was_crashed = true;
275 : }
276 : } else {
277 917 : vehicle_was_crashed = false;
278 : }
279 :
280 917 : if (commanded_runstate != pilot_desired_runstate &&
281 278 : !hal.util->get_soft_armed()) {
282 : // consider changing the commanded runstate to the pilot
283 : // desired runstate:
284 278 : commanded_runstate = RunState::IDLE;
285 278 : switch (pilot_desired_runstate) {
286 9 : case RunState::STOP:
287 : // The H2 will keep the motor running for ~20 seconds for cool-down
288 : // we must go via the idle state or the H2 disallows moving to stop!
289 9 : if (time_in_idle_state_ms() > 1000) {
290 1 : commanded_runstate = pilot_desired_runstate;
291 : }
292 : break;
293 2 : case RunState::IDLE:
294 : // can always switch to idle
295 2 : commanded_runstate = pilot_desired_runstate;
296 2 : break;
297 267 : case RunState::RUN:
298 : // must have idled for a while before moving to run:
299 267 : if (generator_ok_to_run()) {
300 2 : commanded_runstate = pilot_desired_runstate;
301 : }
302 : break;
303 : }
304 : }
305 :
306 917 : switch (commanded_runstate) {
307 579 : case RunState::STOP:
308 579 : SRV_Channels::set_output_pwm(SRV_Channel::k_generator_control, SERVO_PWM_STOP);
309 579 : break;
310 283 : case RunState::IDLE:
311 283 : SRV_Channels::set_output_pwm(SRV_Channel::k_generator_control, SERVO_PWM_IDLE);
312 283 : break;
313 55 : case RunState::RUN:
314 55 : SRV_Channels::set_output_pwm(SRV_Channel::k_generator_control, SERVO_PWM_RUN);
315 55 : break;
316 : }
317 917 : }
318 :
319 : #if HAL_LOGGING_ENABLED
320 : // log generator status to the onboard log
321 938 : void AP_Generator_RichenPower::Log_Write()
322 : {
323 : #define MASK_LOG_ANY 0xFFFF
324 938 : if (!AP::logger().should_log(MASK_LOG_ANY)) {
325 : return;
326 : }
327 926 : if (last_logged_reading_ms == last_reading_ms) {
328 : return;
329 : }
330 93 : last_logged_reading_ms = last_reading_ms;
331 :
332 : // @LoggerMessage: RICH
333 : // @Description: Richenpower generator telemetry
334 : // @Field: TimeUS: Time since system startup
335 : // @Field: runTime: total generator runtime
336 : // @Field: maintTime: time until generator requires maintenance
337 : // @Field: errors: bitmask of error received from generator
338 : // @Field: rpm: current generator RPM
339 : // @Field: ovolt: output voltage
340 : // @Field: ocurr: output current
341 : // @Field: mode: generator mode; idle/run/charge/balance
342 186 : AP::logger().WriteStreaming(
343 : "RICH",
344 : "TimeUS,runTime,maintTime,errors,rpm,ovolt,ocurr,mode",
345 : "s-------",
346 : "F-------",
347 : "QIIHHffB",
348 : AP_HAL::micros64(),
349 : last_reading.runtime,
350 : last_reading.seconds_until_maintenance,
351 93 : last_reading.errors,
352 93 : last_reading.rpm,
353 93 : last_reading.output_voltage,
354 93 : last_reading.output_current,
355 : last_reading.mode
356 : );
357 : }
358 : #endif
359 :
360 : // generator prearm checks; notably, if we never see a generator we do
361 : // not run the checks. Generators are attached/detached at will, and
362 : // reconfiguring is painful.
363 92 : bool AP_Generator_RichenPower::pre_arm_check(char *failmsg, uint8_t failmsg_len) const
364 : {
365 92 : if (uart == nullptr) {
366 : // not configured in serial manager
367 : return true;
368 : }
369 92 : if (last_reading_ms == 0) {
370 : // allow optional use of generator
371 : return true;
372 : }
373 :
374 90 : const uint32_t now = AP_HAL::millis();
375 :
376 90 : if (now - last_reading_ms > 2000) { // we expect @1Hz
377 0 : hal.util->snprintf(failmsg, failmsg_len, "no messages in %ums", unsigned(now - last_reading_ms));
378 0 : return false;
379 : }
380 90 : if (SRV_Channels::get_channel_for(SRV_Channel::k_generator_control) == nullptr) {
381 0 : hal.util->snprintf(failmsg, failmsg_len, "need a servo output channel");
382 0 : return false;
383 : }
384 :
385 90 : uint16_t errors = last_reading.errors;
386 :
387 : // requiring maintenance isn't something that should stop
388 : // people flying - they have work to do. But we definitely
389 : // complain about it - a lot.
390 90 : errors &= ~(1U << uint16_t(Errors::MaintenanceRequired));
391 :
392 90 : if (errors) {
393 :
394 0 : for (uint16_t i=0; i<16; i++) {
395 0 : if (errors & (1U << i)) {
396 0 : if (i < (uint16_t)Errors::LAST) {
397 0 : hal.util->snprintf(failmsg, failmsg_len, "error: %s", error_strings[i]);
398 : } else {
399 0 : hal.util->snprintf(failmsg, failmsg_len, "unknown error: 1U<<%u", i);
400 : }
401 : }
402 : }
403 : return false;
404 : }
405 90 : if (pilot_desired_runstate != RunState::RUN) {
406 59 : hal.util->snprintf(failmsg, failmsg_len, "requested state is not RUN");
407 59 : return false;
408 : }
409 31 : if (commanded_runstate != RunState::RUN) {
410 26 : hal.util->snprintf(failmsg, failmsg_len, "Generator warming up (%.0f%%)", (heat *100 / heat_required_for_run()));
411 26 : return false;
412 : }
413 5 : if (last_reading.mode != Mode::RUN &&
414 5 : last_reading.mode != Mode::CHARGE &&
415 : last_reading.mode != Mode::BALANCE) {
416 2 : hal.util->snprintf(failmsg, failmsg_len, "not running");
417 2 : return false;
418 : }
419 :
420 : return true;
421 : }
422 :
423 : // Update front end with voltage, current, and rpm values
424 938 : void AP_Generator_RichenPower::update_frontend_readings(void)
425 : {
426 938 : _voltage = last_reading.output_voltage;
427 938 : _current = last_reading.output_current;
428 938 : _rpm = last_reading.rpm;
429 :
430 938 : update_frontend();
431 938 : }
432 :
433 :
434 : // healthy returns true if the generator is not present, or it is
435 : // present, providing telemetry and not indicating an errors.
436 938 : bool AP_Generator_RichenPower::healthy() const
437 : {
438 938 : const uint32_t now = AP_HAL::millis();
439 :
440 938 : if (last_reading_ms == 0 || now - last_reading_ms > 2000) {
441 : return false;
442 : }
443 918 : if (last_reading.errors) {
444 0 : return false;
445 : }
446 : return true;
447 : }
448 :
449 : //send mavlink generator status
450 760 : void AP_Generator_RichenPower::send_generator_status(const GCS_MAVLINK &channel)
451 : {
452 760 : if (last_reading_ms == 0) {
453 : // nothing to report
454 : return;
455 : }
456 :
457 : uint64_t status = 0;
458 753 : if (last_reading.rpm == 0) {
459 : status |= MAV_GENERATOR_STATUS_FLAG_OFF;
460 : } else {
461 608 : switch (last_reading.mode) {
462 0 : case Mode::OFF:
463 : status |= MAV_GENERATOR_STATUS_FLAG_OFF;
464 0 : break;
465 588 : case Mode::IDLE:
466 588 : if (pilot_desired_runstate == RunState::RUN) {
467 : status |= MAV_GENERATOR_STATUS_FLAG_WARMING_UP;
468 : } else {
469 : status |= MAV_GENERATOR_STATUS_FLAG_IDLE;
470 : }
471 : break;
472 20 : case Mode::RUN:
473 : status |= MAV_GENERATOR_STATUS_FLAG_GENERATING;
474 20 : break;
475 0 : case Mode::CHARGE:
476 : status |= MAV_GENERATOR_STATUS_FLAG_GENERATING;
477 : status |= MAV_GENERATOR_STATUS_FLAG_CHARGING;
478 0 : break;
479 0 : case Mode::BALANCE:
480 : status |= MAV_GENERATOR_STATUS_FLAG_GENERATING;
481 : status |= MAV_GENERATOR_STATUS_FLAG_CHARGING;
482 0 : break;
483 : }
484 : }
485 :
486 753 : if (last_reading.errors & (uint8_t)Errors::Overload) {
487 0 : status |= MAV_GENERATOR_STATUS_FLAG_OVERCURRENT_FAULT;
488 : }
489 753 : if (last_reading.errors & (uint8_t)Errors::LowVoltageOutput) {
490 0 : status |= MAV_GENERATOR_STATUS_FLAG_REDUCED_POWER;
491 : }
492 :
493 : if (last_reading.errors & (uint8_t)Errors::MaintenanceRequired) {
494 : status |= MAV_GENERATOR_STATUS_FLAG_MAINTENANCE_REQUIRED;
495 : }
496 753 : if (last_reading.errors & (uint8_t)Errors::StartDisabled) {
497 0 : status |= MAV_GENERATOR_STATUS_FLAG_START_INHIBITED;
498 : }
499 753 : if (last_reading.errors & (uint8_t)Errors::LowBatteryVoltage) {
500 0 : status |= MAV_GENERATOR_STATUS_FLAG_BATTERY_UNDERVOLT_FAULT;
501 : }
502 :
503 753 : mavlink_msg_generator_status_send(
504 : channel.get_chan(),
505 : status,
506 : last_reading.rpm, // generator_speed
507 : std::numeric_limits<double>::quiet_NaN(), // battery_current; current into/out of battery
508 : last_reading.output_current, // load_current; Current going to UAV
509 : std::numeric_limits<double>::quiet_NaN(), // power_generated; the power being generated
510 : last_reading.output_voltage, // bus_voltage; Voltage of the bus seen at the generator
511 : INT16_MAX, // rectifier_temperature
512 753 : std::numeric_limits<double>::quiet_NaN(), // bat_current_setpoint; The target battery current
513 : INT16_MAX, // generator temperature
514 : last_reading.runtime,
515 753 : (int32_t)last_reading.seconds_until_maintenance
516 : );
517 : }
518 :
519 : // methods to control the generator state:
520 3 : bool AP_Generator_RichenPower::stop()
521 : {
522 3 : set_pilot_desired_runstate(RunState::STOP);
523 3 : return true;
524 : }
525 :
526 3 : bool AP_Generator_RichenPower::idle()
527 : {
528 3 : set_pilot_desired_runstate(RunState::IDLE);
529 3 : return true;
530 : }
531 :
532 3 : bool AP_Generator_RichenPower::run()
533 : {
534 3 : set_pilot_desired_runstate(RunState::RUN);
535 3 : return true;
536 : }
537 : #endif // AP_GENERATOR_RICHENPOWER_ENABLED
|
