diff --git a/libraries/AP_Generator/AP_Generator.cpp b/libraries/AP_Generator/AP_Generator.cpp new file mode 100644 index 0000000000..0dff0ef098 --- /dev/null +++ b/libraries/AP_Generator/AP_Generator.cpp @@ -0,0 +1,175 @@ +/* + 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 . + */ + +#include "AP_Generator.h" + +#if GENERATOR_ENABLED + +#include "AP_Generator_IE_650_800.h" +#include "AP_Generator_IE_2400.h" +#include "AP_Generator_RichenPower.h" + +const AP_Param::GroupInfo AP_Generator::var_info[] = { + + // @Param: TYPE + // @DisplayName: Generator type + // @Description: Generator type + // @Values: 0:Disabled, 1:IE 650w 800w Fuel Cell, 2:IE 2.4kW Fuel Cell, 3: Richenpower + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO_FLAGS("TYPE", 1, AP_Generator, _type, 0, AP_PARAM_FLAG_ENABLE), + + AP_GROUPEND +}; + +// Constructor +AP_Generator::AP_Generator() +{ + AP_Param::setup_object_defaults(this, var_info); + + if (_singleton) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("Too many generators"); +#endif + return; + } + _singleton = this; +} + +void AP_Generator::init() +{ + // Select backend + switch (type()) { + case Type::GEN_DISABLED: + // Not using a generator + return; + + case Type::IE_650_800: + _driver_ptr = new AP_Generator_IE_650_800(*this); + break; + + case Type::IE_2400: + _driver_ptr = new AP_Generator_IE_2400(*this); + break; + + case Type::RICHENPOWER: + _driver_ptr = new AP_Generator_RichenPower(*this); + break; + } + + if (_driver_ptr != nullptr) { + _driver_ptr->init(); + } +} + +void AP_Generator::update() +{ + // Return immediatly if not enabled. Don't support run-time disabling of generator + if (_driver_ptr == nullptr) { + return; + } + + // Calling backend update will cause backend to update the front end variables + _driver_ptr->update(); +} + +// Helper to get param and cast to Type +enum AP_Generator::Type AP_Generator::type() const +{ + return (Type)_type.get(); +} + +// Pass through to backend +void AP_Generator::send_generator_status(const GCS_MAVLINK &channel) +{ + if (_driver_ptr == nullptr) { + return; + } + _driver_ptr->send_generator_status(channel); +} + +// Tell backend to perform arming checks +bool AP_Generator::pre_arm_check(char* failmsg, uint8_t failmsg_len) const +{ + if (type() == Type::GEN_DISABLED) { + // Don't prevent arming if generator is not enabled and has never been init + if (_driver_ptr == nullptr) { + return true; + } + // Don't allow arming if we have disabled the generator since boot + strncpy(failmsg, "Generator disabled, reboot reqired", failmsg_len); + return false; + } + if (_driver_ptr == nullptr) { + strncpy(failmsg, "No backend driver", failmsg_len); + return false; + } + return _driver_ptr->pre_arm_check(failmsg, failmsg_len); +} + +// Tell backend check failsafes +AP_BattMonitor::BatteryFailsafe AP_Generator::update_failsafes() +{ + // Don't invoke a failsafe if driver not assigned + if (_driver_ptr == nullptr) { + return AP_BattMonitor::BatteryFailsafe_None; + } + return _driver_ptr->update_failsafes(); +} + +// Pass through to backend +bool AP_Generator::stop() +{ + // Still allow + if (_driver_ptr == nullptr) { + return false; + } + return _driver_ptr->stop(); +} + +// Pass through to backend +bool AP_Generator::idle() +{ + if (_driver_ptr == nullptr) { + return false; + } + return _driver_ptr->idle(); +} + +// Pass through to backend +bool AP_Generator::run() +{ + // Don't allow operators to request generator be set to run if it has been disabled + if (_driver_ptr == nullptr) { + return false; + } + return _driver_ptr->run(); +} + +// Get the AP_Generator singleton +AP_Generator *AP_Generator::get_singleton() +{ + return _singleton; +} + +AP_Generator *AP_Generator::_singleton = nullptr; + +namespace AP { + AP_Generator *generator() + { + return AP_Generator::get_singleton(); + } +}; +#endif diff --git a/libraries/AP_Generator/AP_Generator.h b/libraries/AP_Generator/AP_Generator.h new file mode 100644 index 0000000000..25cb061c9d --- /dev/null +++ b/libraries/AP_Generator/AP_Generator.h @@ -0,0 +1,106 @@ +#pragma once + +#include + +#ifndef GENERATOR_ENABLED +#define GENERATOR_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +#if GENERATOR_ENABLED + +#include +#include +#include + +class AP_Generator_Backend; +class AP_Generator_IE_650_800; +class AP_Generator_IE_2400; +class AP_Generator_RichenPower; + +class AP_Generator +{ + friend class AP_Generator_Backend; + friend class AP_Generator_IE_650_800; + friend class AP_Generator_IE_2400; + friend class AP_Generator_RichenPower; + +public: + // Constructor + AP_Generator(); + + // Do not allow copies + AP_Generator(const AP_Generator &other) = delete; + AP_Generator &operator=(const AP_Generator&) = delete; + + static AP_Generator* get_singleton(); + + void init(void); + void update(void); + + bool pre_arm_check(char *failmsg, uint8_t failmsg_len) const; + + AP_BattMonitor::BatteryFailsafe update_failsafes(void); + + // Helpers to retrieve measurements + float get_voltage(void) const { return _voltage; } + float get_current(void) const { return _current; } + float get_fuel_remain(void) const { return _fuel_remain_pct; } + float get_batt_consumed(void) const { return _consumed_mah; } + uint16_t get_rpm(void) const { return _rpm; } + + // Helpers to see if backend has a measurement + bool has_current() const { return _has_current; } + bool has_consumed_energy() const { return _has_consumed_energy; } + bool has_fuel_remaining() const { return _has_fuel_remaining; } + + // healthy() returns true if the generator is not present, or it is + // present, providing telemetry and not indicating any errors. + bool healthy(void) const { return _healthy; } + + // Generator controls must return true if present in generator type + bool stop(void); + bool idle(void); + bool run(void); + + void send_generator_status(const GCS_MAVLINK &channel); + + // Parameter block + static const struct AP_Param::GroupInfo var_info[]; + +private: + + // Pointer to chosen driver + AP_Generator_Backend* _driver_ptr; + + // Parameters + AP_Int8 _type; // Select which generator to use + + enum class Type { + GEN_DISABLED = 0, + IE_650_800 = 1, + IE_2400 = 2, + RICHENPOWER = 3, + }; + + // Helper to get param and cast to GenType + Type type(void) const; + + // Front end variables + float _voltage; + float _current; + float _fuel_remain_pct; + float _consumed_mah; + uint16_t _rpm; + bool _healthy; + bool _has_current; + bool _has_consumed_energy; + bool _has_fuel_remaining; + + static AP_Generator *_singleton; + +}; + +namespace AP { + AP_Generator *generator(); +}; +#endif diff --git a/libraries/AP_Generator/AP_Generator_Backend.cpp b/libraries/AP_Generator/AP_Generator_Backend.cpp new file mode 100644 index 0000000000..9a7367eb7a --- /dev/null +++ b/libraries/AP_Generator/AP_Generator_Backend.cpp @@ -0,0 +1,36 @@ +/* + 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 . + */ +#include "AP_Generator_Backend.h" + +#if GENERATOR_ENABLED + +// Base class consructor +AP_Generator_Backend::AP_Generator_Backend(AP_Generator& frontend) : + _frontend(frontend) +{ +} + +// Called from the subclass update function to update the frontend variables for accessing +void AP_Generator_Backend::update_frontend() +{ + // Update the values in the front end + _frontend._voltage = _voltage; + _frontend._current = _current; + _frontend._consumed_mah = _consumed_mah; + _frontend._fuel_remain_pct = _fuel_remain_pct; + _frontend._rpm = _rpm; + _frontend._healthy = healthy(); +} +#endif diff --git a/libraries/AP_Generator/AP_Generator_Backend.h b/libraries/AP_Generator/AP_Generator_Backend.h new file mode 100644 index 0000000000..dac3e27592 --- /dev/null +++ b/libraries/AP_Generator/AP_Generator_Backend.h @@ -0,0 +1,53 @@ +#pragma once + +#include "AP_Generator.h" + +#if GENERATOR_ENABLED + +class AP_Generator_Backend +{ +public: + + // Constructor + AP_Generator_Backend(AP_Generator& frontend); + + // Declare a virtual destructor in case Generator drivers want to override + virtual ~AP_Generator_Backend() {}; + + virtual void init(void) = 0; + virtual void update(void) = 0; + + // Set default to not fail arming checks + virtual bool pre_arm_check(char *failmsg, uint8_t failmsg_len) const { return true; } + + // Set default to not fail failsafes + virtual AP_BattMonitor::BatteryFailsafe update_failsafes(void) const { + return AP_BattMonitor::BatteryFailsafe::BatteryFailsafe_None; + } + + virtual bool healthy(void) const = 0; + + // Generator controls must return true if present in generator type + virtual bool stop(void) { return false; } + virtual bool idle(void) { return false; } + virtual bool run(void) { return false; } + + // Use generator mavlink message + virtual void send_generator_status(const GCS_MAVLINK &channel) {} + +protected: + + // Update frontend + void update_frontend(void); + + // Measurements readings to write to front end + float _voltage; + float _current; + float _fuel_remain_pct; // Decimal from 0 to 1 + float _consumed_mah; + uint16_t _rpm; + + AP_Generator& _frontend; + +}; +#endif diff --git a/libraries/AP_Generator/AP_Generator_IE_2400.cpp b/libraries/AP_Generator/AP_Generator_IE_2400.cpp new file mode 100644 index 0000000000..77b5a92417 --- /dev/null +++ b/libraries/AP_Generator/AP_Generator_IE_2400.cpp @@ -0,0 +1,206 @@ +/* + 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 . + */ + +#include "AP_Generator_IE_2400.h" + +#if GENERATOR_ENABLED + +#include + +extern const AP_HAL::HAL& hal; + +void AP_Generator_IE_2400::init() +{ + // Call init from base class to do common setup + AP_Generator_IE_FuelCell::init(); + + // Tell frontend what measurements are available for this generator + _frontend._has_current = true; + _frontend._has_consumed_energy = true; + _frontend._has_fuel_remaining = true; +} + +// Update fuel cell, expected to be called at 20hz +void AP_Generator_IE_2400::assign_measurements(const uint32_t now) +{ + // Successfully decoded a new valid sentence + // Update internal fuel cell state + _pwr_out = _parsed.pwr_out; + _spm_pwr = _parsed.spm_pwr; + + _state = (State)_parsed.state; + _err_code = _parsed.err_code; + + // Scale tank pressure linearly to a percentage. + // Min = 5 bar, max = 300 bar, PRESS_GRAD = 1/295. + const float PRESS_GRAD = 0.003389830508f; + _fuel_remain_pct = constrain_float((_parsed.tank_bar-5)*PRESS_GRAD,0,1); + + // Update battery voltage + _voltage = _parsed.battery_volt; + + /* Calculate battery current. Convention: +current battery is discharging, -current + battery is charging. This is aligned with normal AP behaviour. This is the opposite + of IE's convention hence *-1 */ + if (_parsed.battery_volt > 0) { + _current = -1 * _parsed.battery_pwr / _parsed.battery_volt; + } else { + _current = 0; + } + + // Calculate consumed current + _consumed_mah += _current * (now - _last_time_ms) * AMS_TO_MAH; + + _last_time_ms = now; +} + +// Process characters received and extract terms for IE 2.4kW +void AP_Generator_IE_2400::decode_latest_term() +{ + // Null terminate and move onto the next term + _term[_term_offset] = 0; + _term_offset = 0; + _term_number++; + + switch (_term_number) { + case 1: + // Float + _parsed.tank_bar = atof(_term); + break; + + case 2: + // Float + _parsed.battery_volt = atof(_term); + break; + + case 3: + // Signed int base 10 + _parsed.pwr_out = strtol(_term, nullptr, 10); + break; + + case 4: + // Unsigned int base 10 + _parsed.spm_pwr = strtoul(_term, nullptr, 10); + break; + + case 5: + // Signed int base 10 + _parsed.battery_pwr = strtol(_term, nullptr, 10); + break; + + case 6: + // Unsigned int base 10 + _parsed.state = strtoul(_term, nullptr, 10); + break; + + case 7: + // Unsigned int base 10 + _parsed.err_code = strtoul(_term, nullptr, 10); + // Sentence only declared valid when we have the expected number of terms + _sentence_valid = true; + break; + + default: + // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence + _sentence_valid = false; + break; + } +} + +// Check for failsafes +AP_BattMonitor::BatteryFailsafe AP_Generator_IE_2400::update_failsafes() const +{ + // Check for error codes that lead to critical action battery monitor failsafe + if (is_critical_error(_err_code)) { + return AP_BattMonitor::BatteryFailsafe::BatteryFailsafe_Critical; + } + + // Check for error codes that lead to low action battery monitor failsafe + if (is_low_error(_err_code)) { + return AP_BattMonitor::BatteryFailsafe::BatteryFailsafe_Low; + } + + return AP_BattMonitor::BatteryFailsafe::BatteryFailsafe_None; +} + +// Check for error codes that are deemed critical +bool AP_Generator_IE_2400::is_critical_error(const uint32_t err_in) const +{ + switch ((ErrorCode)err_in) { + // Error codes that lead to critical action battery monitor failsafe + case ErrorCode::BATTERY_CRITICAL: + case ErrorCode::PRESSURE_CRITICAL: + case ErrorCode::SYSTEM_CRITICAL: + return true; + + default: + // Minor internal error is always ignored and caught by the default + return false; + } +} + +// Check for error codes that are deemed severe and would be cause to trigger a battery monitor low failsafe action +bool AP_Generator_IE_2400::is_low_error(const uint32_t err_in) const +{ + switch ((ErrorCode)err_in) { + // Error codes that lead to critical action battery monitor failsafe + case ErrorCode::START_DENIED: + case ErrorCode::PRESSURE_ALERT: + case ErrorCode::BATTERY_LOW: + case ErrorCode::PRESSURE_LOW: + case ErrorCode::SPM_LOST: + case ErrorCode::REDUCED_POWER: + return true; + + default: + // Minor internal error is always ignored and caught by the default + return false; + } +} + +// Check error codes and populate message with error code +bool AP_Generator_IE_2400::check_for_err_code(char* msg_txt, uint8_t msg_len) const +{ + // Check if we have received an error code + if (!is_critical_error(_err_code) && !is_low_error(_err_code)) { + return false; + } + + hal.util->snprintf(msg_txt, msg_len, "Fuel cell err code <%u>", (unsigned)_err_code); + return true; +} + +// log generator status to the onboard log +void AP_Generator_IE_2400::log_write() +{ +#define MASK_LOG_ANY 0xFFFF + if (!AP::logger().should_log(MASK_LOG_ANY)) { + return; + } + + AP::logger().Write( + "IE24", + "TimeUS,FUEL,SPMPWR,POUT,ERR", + "s%WW-", + "F2---", + "Qfiii", + AP_HAL::micros64(), + _fuel_remain_pct, + _spm_pwr, + _pwr_out, + _err_code + ); +} +#endif diff --git a/libraries/AP_Generator/AP_Generator_IE_2400.h b/libraries/AP_Generator/AP_Generator_IE_2400.h new file mode 100644 index 0000000000..8f15a5d093 --- /dev/null +++ b/libraries/AP_Generator/AP_Generator_IE_2400.h @@ -0,0 +1,56 @@ +#pragma once + +#include "AP_Generator_IE_FuelCell.h" + +#if GENERATOR_ENABLED + +class AP_Generator_IE_2400 : public AP_Generator_IE_FuelCell +{ + // Inherit constructor + using AP_Generator_IE_FuelCell::AP_Generator_IE_FuelCell; + +public: + + void init(void) override; + + AP_BattMonitor::BatteryFailsafe update_failsafes() const override; + +private: + + // Assigns the unit specific measurements once a valid sentence is obtained + void assign_measurements(const uint32_t now) override; + + // Process characters received and extract terms for IE 2.4kW + void decode_latest_term(void) override; + + // Check if we have received an error code and populate message with error code + bool check_for_err_code(char* msg_txt, uint8_t msg_len) const override; + + // Check for error codes that are deemed critical + bool is_critical_error(const uint32_t err_in) const; + + // Check for error codes that are deemed severe and would be cause to trigger a battery monitor low failsafe action + bool is_low_error(const uint32_t err_in) const; + + void log_write(void) override; + + // IE 2.4kW failsafes + enum class ErrorCode { + MINOR_INTERNAL = 1, // Minor internal error is to be ignored + REDUCED_POWER = 10, + SPM_LOST = 11, + PRESSURE_LOW = 20, + BATTERY_LOW = 21, + PRESSURE_ALERT = 30, + START_DENIED = 31, + SYSTEM_CRITICAL = 32, + PRESSURE_CRITICAL = 33, + BATTERY_CRITICAL = 40, + }; + + // These measurements are only available on this unit + int16_t _pwr_out; // Output power (Watts) + uint16_t _spm_pwr; // Stack Power Module (SPM) power draw (Watts) + +}; +#endif diff --git a/libraries/AP_Generator/AP_Generator_IE_650_800.cpp b/libraries/AP_Generator/AP_Generator_IE_650_800.cpp new file mode 100644 index 0000000000..a5b30f8a2c --- /dev/null +++ b/libraries/AP_Generator/AP_Generator_IE_650_800.cpp @@ -0,0 +1,125 @@ +/* + 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 . + */ + +#include "AP_Generator_IE_650_800.h" + +#if GENERATOR_ENABLED + +extern const AP_HAL::HAL& hal; + +void AP_Generator_IE_650_800::init() +{ + // Call init from base class to do common setup + AP_Generator_IE_FuelCell::init(); + + // Tell frontend what measurements are available for this generator + // This unit does not have current but this needs to be true to make use of consumed_mah in BattMonitor + _frontend._has_current = true; + _frontend._has_consumed_energy = true; + _frontend._has_fuel_remaining = false; +} + +// Update fuel cell, expected to be called at 20hz +void AP_Generator_IE_650_800::assign_measurements(const uint32_t now) +{ + // Successfully decoded a new valid sentence + // Update internal fuel cell state + _state = (State)_parsed.state; + _err_code = _parsed.err_code; + + // Update variables to be returned to front end + _fuel_remain_pct = _parsed.tank_pct * 0.01; + + // Invert bat remaining percent to match AP_BattMonitor convention + _consumed_mah = 100.0f - _parsed.battery_pct; + + // This unit does not report voltage, always report 1 volt + _voltage = 1; + + _last_time_ms = now; +} + +// Process characters received and extract terms for IE 650/800 W +void AP_Generator_IE_650_800::decode_latest_term() +{ + // Null terminate and move onto the next term + _term[_term_offset] = 0; + _term_offset = 0; + _term_number++; + + switch (_term_number) { + case 1: + _parsed.tank_pct = atof(_term); + // Out of range values + if (_parsed.tank_pct > 100.0f || _parsed.tank_pct < 0.0f) { + _data_valid = false; + } + break; + + case 2: + _parsed.battery_pct = atof(_term); + // Out of range values + if (_parsed.battery_pct > 100.0f || _parsed.battery_pct < 0.0f) { + _data_valid = false; + } + break; + + case 3: + _parsed.state = strtoul(_term, nullptr, 10); + break; + + case 4: + _parsed.err_code = strtoul(_term, nullptr, 16); + // Sentence only declared valid when we have the expected number of terms + _sentence_valid = true && _data_valid; + break; + + default: + // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence + _sentence_valid = false; + break; + } +} + +// Check error codes and populate message with error code +bool AP_Generator_IE_650_800::check_for_err_code(char* msg_txt, uint8_t msg_len) const +{ + // Check for any valid error codes + if ((_err_code & (fs_crit_mask | fs_low_mask)) == 0) { + return false; + } + + // Error codes are converted to hex to make it easier to compare to user manual for these units + hal.util->snprintf(msg_txt, msg_len, "Fuel cell err code <0x%x>", (unsigned)_err_code); + return true; +} + +// Check for failsafes +AP_BattMonitor::BatteryFailsafe AP_Generator_IE_650_800::update_failsafes() const +{ + // Check if we are in a critical failsafe + if ((_err_code & fs_crit_mask) != 0) { + return AP_BattMonitor::BatteryFailsafe::BatteryFailsafe_Critical; + } + + // Check if we are in a low failsafe + if ((_err_code & fs_low_mask) != 0) { + return AP_BattMonitor::BatteryFailsafe::BatteryFailsafe_Low; + } + + return AP_BattMonitor::BatteryFailsafe::BatteryFailsafe_None; +} + +#endif diff --git a/libraries/AP_Generator/AP_Generator_IE_650_800.h b/libraries/AP_Generator/AP_Generator_IE_650_800.h new file mode 100644 index 0000000000..aed690cc49 --- /dev/null +++ b/libraries/AP_Generator/AP_Generator_IE_650_800.h @@ -0,0 +1,98 @@ +#pragma once + +#include "AP_Generator_IE_FuelCell.h" + +#if GENERATOR_ENABLED + +class AP_Generator_IE_650_800 : public AP_Generator_IE_FuelCell +{ + // Inherit constructor + using AP_Generator_IE_FuelCell::AP_Generator_IE_FuelCell; + +public: + + void init(void) override; + + AP_BattMonitor::BatteryFailsafe update_failsafes() const override; + +private: + + // Assigns the unit specific measurements once a valid sentence is obtained + void assign_measurements(const uint32_t now) override; + + // Process characters received and extract terms for IE 650/800 W + void decode_latest_term(void) override; + + // Convert error code from fuel cell to AP failsafe bitmask + void set_failsafe_mask(uint32_t err_in); + + // Check if we have received an error code and populate message with error code + bool check_for_err_code(char* msg_txt, uint8_t msg_len) const override; + + // IE 650/800 W error codes + // Note: The error codes for this unit are defined as a bitmask + static const uint32_t ERROR_STACK_OT1_ALRT = (1U << 31); // (0x80000000) Stack 1 over temperature alert (>58 degC) + static const uint32_t ERROR_STACK_OT2_ALRT = (1U << 30); // (0x40000000) Stack 2 over temperature alert (>58 degC) + static const uint32_t ERROR_BAT_UV_ALRT = (1U << 29); // (0x20000000) Battery under volt alert (<19 V) + static const uint32_t ERROR_BAT_OT_ALRT = (1U << 28); // (0x10000000) Battery over temperature alert (>65 degC) + static const uint32_t ERROR_NO_FAN = (1U << 27); // (0x8000000), No fan current detected (<0.01 A) + static const uint32_t ERROR_FAN_OVERRUN = (1U << 26); // (0x4000000), Fan over current (>0.25 A) + static const uint32_t ERROR_STACK_OT1_CRT = (1U << 25); // (0x2000000), Stack 1 over temperature critical (>57 degC) + static const uint32_t ERROR_STACK_OT2_CRT = (1U << 24); // (0x1000000), Stack 2 over temperature critical (>57 degC) + static const uint32_t ERROR_BAT_UV_CRT = (1U << 23); // (0x800000), Battery under volt warning (<19.6 V) + static const uint32_t ERROR_BAT_OT_CRT = (1U << 22); // (0x400000), Battery over temperature warning (>60 degC) + static const uint32_t ERROR_START_TIMEOUT = (1U << 21); // (0x200000), Fuel cell's internal State == start for > 30 s + static const uint32_t ERROR_STOP_TIMEOUT = (1U << 20); // (0x100000), Fuel cell's internal State == stop for > 15 s + static const uint32_t ERROR_START_UNDER_PRESS = (1U << 19); // (0x80000), Tank pressure < 6 barg + static const uint32_t ERROR_TANK_UNDER_PRESS = (1U << 18); // (0x40000), Tank pressure < 5 barg + static const uint32_t ERROR_TANK_LOW_PRESS = (1U << 17); // (0x20000), Tank pressure < 15 barg + static const uint32_t ERROR_SAFETY_FLAG = (1U << 16); // (0x10000), Fuel cell's internal saftey flags not set true + static const uint32_t ERROR_DENY_START1 = (1U << 15); // (0x8000), Stack 1 denied start + static const uint32_t ERROR_DENY_START2 = (1U << 14); // (0x4000), Stack 2 denied start + static const uint32_t ERROR_STACK_UT1 = (1U << 13); // (0x2000), Stack 1 under temperature (<5 degC) + static const uint32_t ERROR_STACK_UT2 = (1U << 12); // (0x1000), Stack 2 under temperature (<5 degC) + static const uint32_t ERROR_BAT_UV_WRN = (1U << 11); // (0x800), Battery under voltage warning (21.6 V) + static const uint32_t ERROR_BAT_UV_DENY = (1U << 10); // (0x400), Battery under voltage (21.6 V) and master denied + static const uint32_t ERROR_FAN_PULSE = (1U << 9); // (0x200), Fan pulse aborted + static const uint32_t ERROR_STACK_UV = (1U << 8); // (0x100), Stack under voltage (650W < 17.4 V, 800W < 21.13 V) + static const uint32_t ERROR_SYS_OVERLOAD = (1U << 7); // (0x80), Stack under voltage and battery power below threshold (<-200 W) + static const uint32_t ERROR_SYS_OV_OC = (1U << 6); // (0x40), Over voltage and over current protection + static const uint32_t ERROR_INVALID_SN = (1U << 5); // (0x20), Invalid serial number + static const uint32_t ERROR_CHARGE_FAULT = (1U << 4); // (0x10), Battery charger fault + static const uint32_t ERROR_BAT_UT = (1U << 3); // (0x8), Battery under temperature (<-15 degC) + + // Assign error codes to critical FS mask + static const uint32_t fs_crit_mask = ERROR_STACK_OT1_ALRT // (0x80000000) Stack 1 over temperature alert (>58 degC) + | ERROR_STACK_OT2_ALRT // (0x40000000) Stack 2 over temperature alert (>58 degC) + | ERROR_BAT_UV_ALRT // (0x20000000) Battery undervolt alert (<19 V) + | ERROR_BAT_OT_ALRT // (0x10000000) Battery over temperature alert (>65 degC) + | ERROR_NO_FAN // (0x8000000), No fan current detected (<0.01 A) + | ERROR_STACK_OT1_CRT // (0x2000000), Stack 1 over temperature critical (>57 degC) + | ERROR_STACK_OT2_CRT // (0x1000000), Stack 2 over temperature critical (>57 degC) + | ERROR_BAT_UV_CRT // (0x800000), Battery undervolt warning (<19.6 V) + | ERROR_BAT_OT_CRT // (0x400000), Battery over temperature warning (>60 degC) + | ERROR_START_TIMEOUT // (0x200000), Fuel cell's internal State == start for > 30 s + | ERROR_START_UNDER_PRESS // (0x80000), Tank pressure < 6 barg + | ERROR_TANK_UNDER_PRESS // (0x40000), Tank pressure < 5 barg + | ERROR_SAFETY_FLAG // (0x10000), Fuel cell's internal saftey flags not set true + | ERROR_DENY_START1 // (0x8000), Stack 1 denied start + | ERROR_DENY_START2 // (0x4000), Stack 2 denied start + | ERROR_BAT_UV_DENY // (0x400), Battery under voltage (21.6 V) and master denied + | ERROR_SYS_OV_OC // (0x40), Over voltage and over current protection + | ERROR_INVALID_SN; // (0x20), Invalid serial number + + // Assign error codes to low FS mask + static const uint32_t fs_low_mask = ERROR_FAN_OVERRUN // (0x4000000), Fan over current (>0.25 A) + | ERROR_STOP_TIMEOUT // (0x100000), Fuel cell's internal State == stop for > 15 s + | ERROR_TANK_LOW_PRESS // (0x20000), Tank pressure < 15 barg + | ERROR_STACK_UT1 // (0x2000), Stack 1 under temperature (<5 degC) + | ERROR_STACK_UT2 // (0x1000), Stack 2 under temperature (<5 degC) + | ERROR_BAT_UV_WRN // (0x800), Battery under voltage warning (21.6 V) + | ERROR_FAN_PULSE // (0x200), Fan pulse aborted + | ERROR_STACK_UV // (0x100), Stack under voltage (650W < 17.4 V, 800W < 21.13 V) + | ERROR_SYS_OVERLOAD // (0x80), Stack under voltage and battery power below threshold (<-200 W) + | ERROR_CHARGE_FAULT // (0x10), Battery charger fault + | ERROR_BAT_UT; // (0x8), Battery undertemperature (<-15 degC) + +}; +#endif diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp new file mode 100644 index 0000000000..992ced107d --- /dev/null +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp @@ -0,0 +1,192 @@ +/* + 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 . + */ + +#include "AP_Generator_IE_FuelCell.h" +#include + +#if GENERATOR_ENABLED + +// 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_sent = false; +} + +// 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(); + + update_frontend(); + + log_write(); +} + +// 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 == '<') { + _sentence_valid = false; + _data_valid = true; + _term_number = 0; + _term_offset = 0; + _in_string = true; + return false; + } + if (!_in_string) { + return false; + } + + // End of a string + if (c == '>') { + decode_latest_term(); + _in_string = false; + + return _sentence_valid; + } + + // End of a term in the string + if (c == ',') { + decode_latest_term(); + return false; + } + + // Otherwise add the char to the current term + _term[_term_offset++] = 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, "Driver not healthy", failmsg_len); + return false; + } + + // Refuse arming if not in running state + if (_state != State::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() +{ + // Check driver health + if (!healthy() && !_health_warn_sent) { + // Don't spam GCS with unhealthy message + _health_warn_sent = true; + gcs().send_text(MAV_SEVERITY_ALERT, "Generator: Driver not healthy"); + + } else if (healthy()) { + _health_warn_sent = false; + } + + // 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; + } + + // Check error codes + char msg_txt[32]; + if (check_for_err_code_if_changed(msg_txt, sizeof(msg_txt))) { + gcs().send_text(MAV_SEVERITY_ALERT, "%s", msg_txt); + } +} + +// Check error codes and populate message with error code +bool AP_Generator_IE_FuelCell::check_for_err_code_if_changed(char* msg_txt, uint8_t msg_len) +{ + // Only check if there has been a change in error code + if (_err_code == _last_err_code) { + return false; + } + + if (check_for_err_code(msg_txt, msg_len)) { + _last_err_code = _err_code; + return true; + } + + return false; +} +#endif diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.h b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h new file mode 100644 index 0000000000..1f4f92e9ee --- /dev/null +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h @@ -0,0 +1,106 @@ +#pragma once + +#include "AP_Generator_Backend.h" + +#if GENERATOR_ENABLED + +#include +#include + +class AP_Generator_IE_FuelCell : public AP_Generator_Backend +{ + +public: + // Constructor + using AP_Generator_Backend::AP_Generator_Backend; + + // Initialize the fuel cell driver + void init(void) override; + + // Check if readings are healthy + bool healthy(void) const override { return _healthy; } + + // Check for arming + bool pre_arm_check(char *failmsg, uint8_t failmsg_len) const override; + + // Update fuel cell, expected to be called at 20hz + void update(void) override; + +protected: + + // Pointer to serial uart + AP_HAL::UARTDriver *_uart = nullptr; + + // IE fuel cell running states + enum class State { + STARTING = 0, + READY = 1, + RUNNING = 2, + FAULT = 3, + BATTERY_ONLY = 8, + }; + + // State enum to string lookup + struct Lookup_State { + State option; + const char *msg_txt; + }; + + static const Lookup_State lookup_state[]; + + uint32_t _err_code; // The error code from fuel cell + uint32_t _last_err_code; // The previous error code from fuel cell + State _state; // The PSU state + State _last_state; // The previous PSU state + uint32_t _last_time_ms; // Time we got a reading + bool _healthy; // Is the driver working + bool _health_warn_sent; + + // Temporary state params + struct ParsedValue { + float tank_pct; + uint16_t tank_bar; + float battery_pct; + float battery_volt; + int16_t pwr_out; + uint16_t spm_pwr; + int16_t battery_pwr; + uint8_t state; + uint32_t err_code; + } _parsed; + + // Constants + static const uint8_t TERM_BUFFER = 12; // Max length of term we expect + static const uint16_t HEALTHY_TIMEOUT_MS = 5000; // Time for driver to be marked un-healthy + + // Decoding vars + char _term[TERM_BUFFER]; // Term buffer + bool _sentence_valid; // Is current sentence valid + bool _data_valid; // Is data within expected limits + uint8_t _term_number; // Term index within the current sentence + uint8_t _term_offset; // Offset within the _term buffer where the next character should be placed + bool _in_string; // True if we should be decoding + + // Assigns the unit specific measurements once a valid sentence is obtained + virtual void assign_measurements(const uint32_t now) = 0; + + virtual void log_write(void) {} + + // Add a single character to the buffer and attempt to decode. + // Returns true if a complete sentence was successfully decoded or if the buffer is full. + bool decode(char c); + + // Unit specific decoding to process characters recieved and build sentence + virtual void decode_latest_term(void) = 0; + + // Check if we should notify on any change of fuel cell state + void check_status(void); + + // Check error codes and populate message with error code + virtual bool check_for_err_code(char* msg_txt, uint8_t msg_len) const = 0; + + // Only check the error code if it has changed since we last checked + bool check_for_err_code_if_changed(char* msg_txt, uint8_t msg_len); + +}; +#endif diff --git a/libraries/AP_Generator/AP_Generator_RichenPower.cpp b/libraries/AP_Generator/AP_Generator_RichenPower.cpp index e0fed44032..7361f8f785 100644 --- a/libraries/AP_Generator/AP_Generator_RichenPower.cpp +++ b/libraries/AP_Generator/AP_Generator_RichenPower.cpp @@ -17,28 +17,14 @@ #if GENERATOR_ENABLED -#include #include #include -#include #include #include extern const AP_HAL::HAL& hal; -// Generator constructor; only one generator is curently permitted -AP_Generator_RichenPower::AP_Generator_RichenPower() -{ - if (_singleton) { -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - AP_HAL::panic("Too many richenpower generators"); -#endif - return; - } - _singleton = this; -} - // init method; configure communications with the generator void AP_Generator_RichenPower::init() { @@ -49,8 +35,12 @@ void AP_Generator_RichenPower::init() const uint32_t baud = serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Generator, 0); uart->begin(baud, 256, 256); } -} + // Tell frontend what measurements are available for this generator + _frontend._has_current = true; + _frontend._has_consumed_energy = false; + _frontend._has_fuel_remaining = false; +} // find a RichenPower message in the buffer, starting at // initial_offset. If dound, that message (or partial message) will @@ -221,6 +211,8 @@ void AP_Generator_RichenPower::update(void) update_heat(); + update_frontend_readings(); + Log_Write(); } @@ -340,14 +332,14 @@ bool AP_Generator_RichenPower::pre_arm_check(char *failmsg, uint8_t failmsg_len) const uint32_t now = AP_HAL::millis(); if (now - last_reading_ms > 2000) { // we expect @1Hz - snprintf(failmsg, failmsg_len, "no messages in %ums", unsigned(now - last_reading_ms)); + hal.util->snprintf(failmsg, failmsg_len, "no messages in %ums", unsigned(now - last_reading_ms)); return false; } if (last_reading.seconds_until_maintenance == 0) { - snprintf(failmsg, failmsg_len, "requires maintenance"); + hal.util->snprintf(failmsg, failmsg_len, "requires maintenance"); } if (SRV_Channels::get_channel_for(SRV_Channel::k_generator_control) == nullptr) { - snprintf(failmsg, failmsg_len, "need a servo output channel"); + hal.util->snprintf(failmsg, failmsg_len, "need a servo output channel"); return false; } @@ -355,51 +347,42 @@ bool AP_Generator_RichenPower::pre_arm_check(char *failmsg, uint8_t failmsg_len) for (uint16_t i=0; i<16; i++) { if (last_reading.errors & (1U << (uint16_t)i)) { if (i < (uint16_t)Errors::LAST) { - snprintf(failmsg, failmsg_len, "error: %s", error_strings[i]); + hal.util->snprintf(failmsg, failmsg_len, "error: %s", error_strings[i]); } else { - snprintf(failmsg, failmsg_len, "unknown error: 1U<<%u", i); + hal.util->snprintf(failmsg, failmsg_len, "unknown error: 1U<<%u", i); } } } return false; } if (pilot_desired_runstate != RunState::RUN) { - snprintf(failmsg, failmsg_len, "requested state is not RUN"); + hal.util->snprintf(failmsg, failmsg_len, "requested state is not RUN"); return false; } if (commanded_runstate != RunState::RUN) { - snprintf(failmsg, failmsg_len, "Generator warming up (%.0f%%)", (heat *100 / heat_required_for_run())); + hal.util->snprintf(failmsg, failmsg_len, "Generator warming up (%.0f%%)", (heat *100 / heat_required_for_run())); return false; } if (last_reading.mode != Mode::RUN && last_reading.mode != Mode::CHARGE && last_reading.mode != Mode::BALANCE) { - snprintf(failmsg, failmsg_len, "not running"); + hal.util->snprintf(failmsg, failmsg_len, "not running"); return false; } return true; } -// voltage returns the last voltage read from the generator telemetry -bool AP_Generator_RichenPower::voltage(float &v) const +// Update front end with voltage, current, and rpm values +void AP_Generator_RichenPower::update_frontend_readings(void) { - if (last_reading_ms == 0) { - return false; - } - v = last_reading.output_voltage; - return true; + _voltage = last_reading.output_voltage; + _current = last_reading.output_current; + _rpm = last_reading.rpm; + + update_frontend(); } -// current returns the last current read from the generator telemetry -bool AP_Generator_RichenPower::current(float &curr) const -{ - if (last_reading_ms == 0) { - return false; - } - curr = last_reading.output_current; - return true; -} // healthy returns true if the generator is not present, or it is // present, providing telemetry and not indicating an errors. @@ -486,22 +469,22 @@ void AP_Generator_RichenPower::send_generator_status(const GCS_MAVLINK &channel) ); } -/* - * Get the AP_Generator singleton - */ -AP_Generator_RichenPower *AP_Generator_RichenPower::_singleton; -AP_Generator_RichenPower *AP_Generator_RichenPower::get_singleton() +// methods to control the generator state: +bool AP_Generator_RichenPower::stop() { - return _singleton; + set_pilot_desired_runstate(RunState::STOP); + return true; } -namespace AP { - -AP_Generator_RichenPower *generator() +bool AP_Generator_RichenPower::idle() { - return AP_Generator_RichenPower::get_singleton(); + set_pilot_desired_runstate(RunState::IDLE); + return true; } -}; - +bool AP_Generator_RichenPower::run() +{ + set_pilot_desired_runstate(RunState::RUN); + return true; +} #endif diff --git a/libraries/AP_Generator/AP_Generator_RichenPower.h b/libraries/AP_Generator/AP_Generator_RichenPower.h index bb1cb995db..94fdc3b01a 100644 --- a/libraries/AP_Generator/AP_Generator_RichenPower.h +++ b/libraries/AP_Generator/AP_Generator_RichenPower.h @@ -1,19 +1,15 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #pragma once -#include -#include -#include -#include +#include "AP_Generator_Backend.h" +#if GENERATOR_ENABLED + +#include +#include #include #include -#ifndef GENERATOR_ENABLED -#define GENERATOR_ENABLED !HAL_MINIMIZE_FEATURES -#endif - -#if GENERATOR_ENABLED /* * Example setup: @@ -23,50 +19,40 @@ * param set SERVO8_FUNCTION 42 # autopilot directive to generator */ -class AP_Generator_RichenPower +class AP_Generator_RichenPower : public AP_Generator_Backend { public: - - AP_Generator_RichenPower(); - - /* Do not allow copies */ - AP_Generator_RichenPower(const AP_Generator_RichenPower &other) = delete; - AP_Generator_RichenPower &operator=(const AP_Generator_RichenPower&) = delete; - - static AP_Generator_RichenPower *get_singleton(); + // constructor + using AP_Generator_Backend::AP_Generator_Backend; // init should be called at vehicle startup to get the generator library ready - void init(); - // update should be called regulkarly to update the generator state - void update(void); + void init(void) override; + // update should be called regularly to update the generator state + void update(void) override; // methods to control the generator state: - void stop() { set_pilot_desired_runstate(RunState::STOP); } - void idle() { set_pilot_desired_runstate(RunState::IDLE); } - void run() { set_pilot_desired_runstate(RunState::RUN); } + bool stop(void) override; + bool idle(void) override; + bool run(void) override; // method to send a GENERATOR_STATUS mavlink message - void send_generator_status(const GCS_MAVLINK &channel); + void send_generator_status(const GCS_MAVLINK &channel) override; // prearm checks to ensure generator is good for arming. Note // that if the generator has never sent us a message then these // automatically pass! - bool pre_arm_check(char *failmsg, uint8_t failmsg_len) const; + bool pre_arm_check(char *failmsg, uint8_t failmsg_len) const override; - // these return false if a reading is not available. They do not - // modify the passed-in value if they return false. - bool voltage(float &voltage) const; - bool current(float ¤t) const; + // Update front end with voltage, current, and rpm values + void update_frontend_readings(void); // healthy returns true if the generator is not present, or it is // present, providing telemetry and not indicating an errors. - bool healthy() const; + bool healthy() const override; private: - static AP_Generator_RichenPower *_singleton; - // read - read serial port, return true if a new reading has been found bool get_reading(); AP_HAL::UARTDriver *uart; @@ -219,9 +205,4 @@ private: return AP_HAL::millis() - idle_state_start_ms; } }; - -namespace AP { - AP_Generator_RichenPower *generator(); -}; - #endif