Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: add MPPT PacketDigital driver

This commit is contained in:
Tom Pittenger 2021-04-22 11:04:01 -07:00 committed by Tom Pittenger
parent c756138ede
commit a365e18420
4 changed files with 451 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_BattMonitor/AP_BattMonitor.cpp
View File

@ -13,6 +13,7 @@
#include "AP_BattMonitor_FuelFlow.h"
#include "AP_BattMonitor_FuelLevel_PWM.h"
#include "AP_BattMonitor_Generator.h"
#include "AP_BattMonitor_MPPT_PacketDigital.h"
#include <AP_HAL/AP_HAL.h>
@ -195,6 +196,11 @@ AP_BattMonitor::init()
drivers[instance] = new AP_BattMonitor_Generator_FuelLevel(*this, state[instance], _params[instance]);
break;
#endif // GENERATOR_ENABLED
#if HAL_MPPT_PACKETDIGITAL_CAN_ENABLE
case Type::MPPT_PacketDigital:
drivers[instance] = new AP_BattMonitor_MPPT_PacketDigital(*this, state[instance], _params[instance]);
break;
#endif // HAL_MPPT_PACKETDIGITAL_CAN_ENABLE
case Type::NONE:
default:
break;

3
libraries/AP_BattMonitor/AP_BattMonitor.h
View File

@ -39,6 +39,7 @@ class AP_BattMonitor_SMBus_Maxell;
class AP_BattMonitor_SMBus_Rotoye;
class AP_BattMonitor_UAVCAN;
class AP_BattMonitor_Generator;
class AP_BattMonitor_MPPT_PacketDigital;
class AP_BattMonitor
{
@ -54,6 +55,7 @@ class AP_BattMonitor
friend class AP_BattMonitor_FuelFlow;
friend class AP_BattMonitor_FuelLevel_PWM;
friend class AP_BattMonitor_Generator;
friend class AP_BattMonitor_MPPT_PacketDigital;
public:
@ -84,6 +86,7 @@ public:
GENERATOR_ELEC = 17,
GENERATOR_FUEL = 18,
Rotoye = 19,
MPPT_PacketDigital = 20,
};
FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);

318
libraries/AP_BattMonitor/AP_BattMonitor_MPPT_PacketDigital.cpp Normal file
View File

@ -0,0 +1,318 @@
#include "AP_BattMonitor_MPPT_PacketDigital.h"
#if HAL_MPPT_PACKETDIGITAL_CAN_ENABLE
#include <AP_CANManager/AP_CANManager.h>
#include <AP_Logger/AP_Logger.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
// read - read the voltage and current
void AP_BattMonitor_MPPT_PacketDigital::read()
{
WITH_SEMAPHORE(_sem_static);
const uint32_t now_ms = AP_HAL::millis();
if (device_count > 0 && now_ms - logger_last_ms >= 1000) {
logger_last_ms = now_ms;
perform_logging();
}
// get output voltage and current but allow for getting input if serial number is negative
// Using _params._serial_number == 0 will give you the average output of all devices
if (get_voltage_and_current_and_temp(_params._serial_number, _state.voltage, _state.current_amps, _state.temperature)) {
_state.temperature_time = now_ms;
_state.last_time_micros = AP_HAL::micros();
_state.healthy = true;
return;
}
_state.voltage = 0;
_state.current_amps = 0;
_state.healthy = false;
}
void AP_BattMonitor_MPPT_PacketDigital::perform_logging() const
{
if (device_count == 0) {
// nothing to log
return;
}
AP_Logger *logger = AP_Logger::get_singleton();
if (!logger || !logger->logging_enabled()) {
return;
}
// log to AP_Logger
// @LoggerMessage: MPPT
// @Description: Information about the Maximum Power Point Tracker sensor
// @Field: TimeUS: Time since system startup
// @Field: Inst: Driver Instance
// @Field: SN: Serial number
// @Field: F: Faults
// @FieldBits: F: Over-Voltage,Under-Voltage,Over-Current,Over-Temperature
// @Field: Temp: Temperature
// @Field: InV: Input Voltage
// @Field: InC: Input Current
// @Field: InP: Input Power
// @Field: OutV: Output Voltage
// @Field: OutC: Output Current
// @Field: OutP: Output Power
for (uint8_t i=0; i<device_count; i++) {
AP::logger().Write("MPPT", "TimeUS,Inst,SN,F,Temp,InV,InC,InP,OutV,OutC,OutP",
"s#--OVAWVAW",
"F----------",
"QBHBbffffff",
AP_HAL::micros64(),
i,
MPPT_devices[i].serialnumber,
(uint8_t)MPPT_devices[i].faults,
MPPT_devices[i].temperature,
(double)MPPT_devices[i].input.voltage,
(double)MPPT_devices[i].input.current,
(double)MPPT_devices[i].input.power,
(double)MPPT_devices[i].output.voltage,
(double)MPPT_devices[i].output.current,
(double)MPPT_devices[i].output.power);
}
}
// parse inbound frames
void AP_BattMonitor_MPPT_PacketDigital::handle_frame(AP_HAL::CANFrame &frame)
{
const uint16_t serialnumber = frame.id & 0x0000FFFF;
if (serialnumber == 0) {
// This is for broadcast and I don't think we should allow this inbound.
return;
}
WITH_SEMAPHORE(_sem_static);
uint8_t index = get_device_index(serialnumber);
if (index == UINT8_MAX) {
// we don't know this device
if (device_count >= ARRAY_SIZE(MPPT_devices)) {
// we don't have any room to remember it
return;
}
// add it
index = device_count;
device_count++;
MPPT_devices[index].serialnumber = serialnumber;
MPPT_devices[index].sequence = frame.data[0];
gcs().send_text(MAV_SEVERITY_DEBUG, "PDCAN: %u New device", serialnumber);
send_command(PacketType::VOLTAGE_GET, serialnumber);
send_command(PacketType::ALGORITHM_GET, serialnumber);
send_command(PacketType::CVT_GET, serialnumber);
} else if (index < device_count) {
MPPT_devices[index].sequence = frame.data[0];
} else {
// not sure how this can happen, but lets protect the array bounds just in case
return;
}
switch ((PacketType)frame.data[1]) {
case PacketType::STREAM_FAULT:
MPPT_devices[index].faults = (FaultFlags)frame.data[2];
MPPT_devices[index].temperature = frame.data[3];
break;
case PacketType::STREAM_INPUT:
MPPT_devices[index].input.voltage = fixed2float(UINT16_VALUE(frame.data[2], frame.data[3]));
MPPT_devices[index].input.current = fixed2float(UINT16_VALUE(frame.data[4], frame.data[5]));
MPPT_devices[index].input.power = fixed2float(UINT16_VALUE(frame.data[6], frame.data[7]), 4); // NOTE: this is using 12:4 fixed point
break;
case PacketType::STREAM_OUTPUT:
MPPT_devices[index].output.voltage = fixed2float(UINT16_VALUE(frame.data[2], frame.data[3]));
MPPT_devices[index].output.current = fixed2float(UINT16_VALUE(frame.data[4], frame.data[5]));
MPPT_devices[index].output.power = fixed2float(UINT16_VALUE(frame.data[6], frame.data[7]), 4); // NOTE: this is using 12:4 fixed point
break;
case PacketType::FAULT: {
// This msg is received when a new fault event happens. It contains the bitfield of all faults.
// We use this event to compare against existing faults to notify the user of just the new fault
const uint8_t all_current_faults = frame.data[2];
const uint8_t prev_faults = (uint8_t)MPPT_devices[index].faults;
const uint8_t new_single_fault = (~prev_faults & all_current_faults);
if (new_single_fault != 0) {
gcs().send_text(MAV_SEVERITY_DEBUG, "PDCAN: %u New Fault! %d: %s", serialnumber, (int)new_single_fault, get_fault_code_string((FaultFlags)new_single_fault));
}
MPPT_devices[index].faults = (FaultFlags)frame.data[2];
}
break;
case PacketType::ACK:
break;
case PacketType::NACK:
//gcs().send_text(MAV_SEVERITY_INFO, "PDCAN: %u NACK 0x%2X", serialnumber, (unsigned)packet_sent_prev);
break;
case PacketType::ALGORITHM_SET:
MPPT_devices[index].algorithm = frame.data[2];
break;
case PacketType::VOLTAGE_SET:
MPPT_devices[index].output_voltage_fixed = fixed2float(UINT16_VALUE(frame.data[2], frame.data[3]));
break;
case PacketType::CVT_SET:
MPPT_devices[index].cvt = fixed2float(UINT16_VALUE(frame.data[2], frame.data[3]));
break;
case PacketType::ALGORITHM_GET: // this is a request so we never expect it inbound
case PacketType::VOLTAGE_GET: // this is a request so we never expect it inbound
case PacketType::CVT_GET: // this is a request so we never expect it inbound
case PacketType::PING: // this is never received. When sent it generates an ACK
// nothing to do
return;
}
MPPT_devices[index].timestamp_ms = AP_HAL::millis();
}
void AP_BattMonitor_MPPT_PacketDigital::send_command(const PacketType type, const uint16_t serialnumber, const float data)
{
AP_HAL::CANFrame txFrame;
switch (type) {
case PacketType::STREAM_FAULT:
case PacketType::STREAM_INPUT:
case PacketType::STREAM_OUTPUT:
case PacketType::FAULT:
case PacketType::ACK:
case PacketType::NACK:
// we only receive these
return;
case PacketType::ALGORITHM_GET:
case PacketType::VOLTAGE_GET:
case PacketType::CVT_GET:
case PacketType::PING:
txFrame.dlc = 0;
break;
case PacketType::ALGORITHM_SET:
txFrame.dlc = 1;
txFrame.data[2] = data;
break;
case PacketType::VOLTAGE_SET:
case PacketType::CVT_SET:
{
txFrame.dlc = 2;
const uint16_t value = float2fixed(data);
txFrame.data[2] = HIGHBYTE(value);
txFrame.data[3] = LOWBYTE(value);
}
break;
}
if (serialnumber == 0) {
// send to all
txFrame.id = 0x00240000;
} else {
txFrame.id = 0x00210000 | (uint32_t)serialnumber;
}
txFrame.id |= AP_HAL::CANFrame::FlagEFF;
const uint8_t index = get_device_index(serialnumber);
uint8_t sequence = 0;
if (index < ARRAY_SIZE(MPPT_devices)) {
txFrame.data[0] = ++MPPT_devices[index].sequence;
}
txFrame.data[0] = sequence;
txFrame.data[1] = (uint8_t)type;
txFrame.dlc += 2;
if (write_frame(txFrame, 50000)) {
// keep track of what we sent last in case we get an ACK/NACK
packet_sent_prev = type;
}
}
// get MPPT_device index by serial number.
// if serial number found in MPP_devices list, return the 0 indexed value
// else return UINT8_MAX
uint8_t AP_BattMonitor_MPPT_PacketDigital::get_device_index(const uint16_t serial_number) const
{
for (uint8_t i=0; i<device_count; i++) {
if (MPPT_devices[i].serialnumber == serial_number) {
return i;
}
}
return UINT8_MAX;
}
// return fault code as string
const char* AP_BattMonitor_MPPT_PacketDigital::get_fault_code_string(const FaultFlags fault) const
{
switch (fault) {
case FaultFlags::OVER_VOLTAGE:
return "Over-Voltage";
case FaultFlags::UNDER_VOLTAGE:
return "Under-Voltage";
case FaultFlags::OVER_CURRENT:
return "Over-Current";
case FaultFlags::OVER_TEMPERATURE:
return "Over-Temperature";
default:
return "Unknown";
}
}
// get the voltage and current and temp of the input or the output MPPT device when returning true
// when returning false, no values were changed.
bool AP_BattMonitor_MPPT_PacketDigital::get_voltage_and_current_and_temp(const int32_t serialnumber, float &voltage, float &current, float &temperature) const
{
if (device_count == 0) {
return false;
}
if (serialnumber <= 0) {
// take the average output of all healthy devices
int8_t count = 0;
float voltage_out_avg = 0.0f;
float current_out_avg = 0.0f;
float temperature_avg = 0.0f;
for (uint8_t i=0; i<device_count; i++) {
if (!MPPT_devices[i].is_healthy()) {
continue;
}
count++;
voltage_out_avg += MPPT_devices[i].output.voltage;
current_out_avg += MPPT_devices[i].output.current;
temperature_avg += MPPT_devices[i].temperature;
}
if (count > 0) {
voltage = voltage_out_avg / count;
current = current_out_avg / count;
temperature = (float)temperature_avg / count;
// average OUTPUTs of all healthy devices
return true;
}
// no healthy devices found
return false;
}
// average
const uint8_t index = get_device_index(serialnumber);
if (!is_healthy_by_index(index)) {
return false;
}
// we only report output energy
voltage = MPPT_devices[index].output.voltage;
current = MPPT_devices[index].output.current;
temperature = MPPT_devices[index].temperature;
return true;
}
#endif // HAL_MPPT_PACKETDIGITAL_CAN_ENABLE

124
libraries/AP_BattMonitor/AP_BattMonitor_MPPT_PacketDigital.h Normal file
View File

@ -0,0 +1,124 @@
#pragma once
#include "AP_BattMonitor_Backend.h"
#include <AP_CANManager/AP_CANSensor.h>
#ifndef HAL_MPPT_PACKETDIGITAL_CAN_ENABLE
#define HAL_MPPT_PACKETDIGITAL_CAN_ENABLE HAL_MAX_CAN_PROTOCOL_DRIVERS && BOARD_FLASH_SIZE > 1024
#endif
#if HAL_MPPT_PACKETDIGITAL_CAN_ENABLE
#ifndef MPPT_PACKETIGITAL_DEVICE_COUNT_MAX
#define MPPT_PACKETIGITAL_DEVICE_COUNT_MAX 5
#endif
class AP_BattMonitor_MPPT_PacketDigital : public CANSensor, public AP_BattMonitor_Backend {
public:
// construct the CAN Sensor
AP_BattMonitor_MPPT_PacketDigital(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params):
AP_BattMonitor_Backend(mon, mon_state, params),
CANSensor("MPPT", AP_CANManager::Driver_Type_MPPT_PacketDigital)
{ };
/// Read the battery voltage and current. Should be called at 10hz
void read() override;
/// returns true if battery monitor provides current info
bool has_current() const override { return true; }
void init(void) override {}
protected:
// handler for incoming frames
void handle_frame(AP_HAL::CANFrame &frame) override;
private:
// Message groups form part of the CAN ID of each frame
enum class PacketType : uint8_t {
ACK = 0x21,
NACK = 0x22,
PING = 0x23,
FAULT = 0x42,
ALGORITHM_SET = 0x44,
ALGORITHM_GET = 0x45,
STREAM_FAULT = 0x47,
STREAM_INPUT = 0x48,
STREAM_OUTPUT = 0x49,
VOLTAGE_SET = 0x4B,
VOLTAGE_GET = 0x4C,
CVT_SET = 0x4D,
CVT_GET = 0x4E,
};
enum class FaultFlags : uint8_t {
OVER_VOLTAGE = (1<<0),
UNDER_VOLTAGE = (1<<1),
OVER_CURRENT = (1<<2),
OVER_TEMPERATURE = (1<<3),
};
enum class ReportMode : uint8_t {
DISABLED = 0,
REPORT_INPUTS = 1,
REPORT_OUTPUTS = 2,
REPORT_VOUT_CVT_ALG = 3,
};
// find first MPPT_devices[] index that contains this serialnumber. If not found, returns UINT8_MAX
uint8_t get_device_index(const uint16_t serial_number) const;
// send command to MPPT device
void send_command(const PacketType type, const uint16_t serialnumber, const float data = 0.0f);
bool is_healthy_by_index(const uint8_t index) const {
if (index >= ARRAY_SIZE(MPPT_devices) || index >= device_count) {
return false;
}
return MPPT_devices[index].is_healthy();
}
const char* get_fault_code_string(const FaultFlags fault) const;
//Frames received from the MPPT will use CAN Extended ID: 0x0042XXXX,
// the least significant 16 bits contain the serial number of the MPPT.
static constexpr uint32_t EXPECTED_FRAME_ID = (0x00420000 | AP_HAL::CANFrame::FlagEFF);
bool get_voltage_and_current_and_temp(const int32_t serialnumber, float &voltage, float &current, float &temperature) const;
void perform_logging() const;
HAL_Semaphore _sem_static;
uint32_t logger_last_ms;
uint8_t device_count;
PacketType packet_sent_prev;
struct MPPT_device {
uint16_t serialnumber;
uint32_t timestamp_ms;
uint8_t sequence;
FaultFlags faults;
int8_t temperature;
uint8_t algorithm;
float output_voltage_fixed;
float cvt;
struct {
float voltage;
float current;
float power;
} input, output;
bool is_healthy() const {
return ((timestamp_ms > 0) && ((AP_HAL::millis() - timestamp_ms) < 2000));
}
} MPPT_devices[MPPT_PACKETIGITAL_DEVICE_COUNT_MAX];
};
#endif // HAL_MPPT_PACKETDIGITAL_CAN_ENABLE