12 changed files with 1205 additions and 88 deletions
@ -0,0 +1,175 @@
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
|
||||
#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 |
@ -0,0 +1,106 @@
@@ -0,0 +1,106 @@
|
||||
#pragma once |
||||
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
|
||||
#ifndef GENERATOR_ENABLED |
||||
#define GENERATOR_ENABLED !HAL_MINIMIZE_FEATURES |
||||
#endif |
||||
|
||||
#if GENERATOR_ENABLED |
||||
|
||||
#include <AP_Param/AP_Param.h> |
||||
#include <GCS_MAVLink/GCS.h> |
||||
#include <AP_BattMonitor/AP_BattMonitor.h> |
||||
|
||||
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 |
@ -0,0 +1,36 @@
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
#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 |
@ -0,0 +1,53 @@
@@ -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 |
@ -0,0 +1,206 @@
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
|
||||
#include "AP_Generator_IE_2400.h" |
||||
|
||||
#if GENERATOR_ENABLED |
||||
|
||||
#include <AP_Logger/AP_Logger.h> |
||||
|
||||
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 |
@ -0,0 +1,56 @@
@@ -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 |
@ -0,0 +1,125 @@
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
|
||||
#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 |
@ -0,0 +1,98 @@
@@ -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 |
@ -0,0 +1,192 @@
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
|
||||
#include "AP_Generator_IE_FuelCell.h" |
||||
#include <AP_SerialManager/AP_SerialManager.h> |
||||
|
||||
#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 |
@ -0,0 +1,106 @@
@@ -0,0 +1,106 @@
|
||||
#pragma once |
||||
|
||||
#include "AP_Generator_Backend.h" |
||||
|
||||
#if GENERATOR_ENABLED |
||||
|
||||
#include <AP_Param/AP_Param.h> |
||||
#include <GCS_MAVLink/GCS.h> |
||||
|
||||
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 |
Loading…
Reference in new issue