321 lines
11 KiB
C++
321 lines
11 KiB
C++
#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
|
|
{
|
|
#ifndef HAL_BUILD_AP_PERIPH
|
|
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().WriteStreaming("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);
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// 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)) {
|
|
sequence = ++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 ¤t, 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
|