mirror of https://github.com/ArduPilot/ardupilot
AP_Generator: Split into frontend-backend and add IE fuel cells
This commit is contained in:
Gone4Dirt
Peter Barker
parent
6c136862b8
commit
8cfe4fc9f7
|
|
@ -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 @@
|
|||
#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 @@
|
|||
/*
|
||||
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 @@
|
|||
#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 @@
|
|||
/*
|
||||
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 @@
|
|||
#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 @@
|
|||
/*
|
||||
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 @@
|
|||
#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 @@
|
|||
/*
|
||||
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 @@
|
|||
#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
|
||||
|
|
@ -17,28 +17,14 @@
|
|||
|
||||
#if GENERATOR_ENABLED
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -1,19 +1,15 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <SRV_Channel/SRV_Channel.h>
|
||||
#include "AP_Generator_Backend.h"
|
||||
|
||||
#if GENERATOR_ENABLED
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <SRV_Channel/SRV_Channel.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#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
|
||||
|
|
|
|||
Loading…
Reference in New Issue