266 lines
12 KiB
C
266 lines
12 KiB
C
/* DTU measurement conversion and deterministic input model; C99, no allocation. */
|
|
#include "iec104_dtu_measurement.h"
|
|
#include "iec104_dtu_settings.h"
|
|
|
|
#include <math.h>
|
|
#include <float.h>
|
|
#include <string.h>
|
|
|
|
typedef struct DtuMeasurementRatings {
|
|
double voltage_primary, voltage_secondary, voltage_ratio;
|
|
double current_primary, current_secondary, current_ratio;
|
|
double residual_primary, residual_secondary, residual_ratio;
|
|
double power_ratio;
|
|
bool power_negative;
|
|
} DtuMeasurementRatings;
|
|
|
|
static bool dtu_measurement_positive(double value)
|
|
{
|
|
return isfinite(value) && value > 0.0;
|
|
}
|
|
|
|
static bool dtu_measurement_ratings(const DtuSettings *settings, DtuMeasurementRatings *ratings)
|
|
{
|
|
double polarity;
|
|
if (settings == NULL) { return false; }
|
|
ratings->voltage_primary = Dtu_Settings_Get(settings, 0x8024U, 0.1) * 1000.0;
|
|
ratings->voltage_secondary = Dtu_Settings_Get(settings, 0x8025U, 100.0);
|
|
ratings->voltage_ratio = Dtu_Settings_Get(settings, 0x8026U, 1.0);
|
|
ratings->current_primary = Dtu_Settings_Get(settings, 0x8040U, 1.0);
|
|
ratings->current_secondary = Dtu_Settings_Get(settings, 0x8041U, 1.0);
|
|
ratings->current_ratio = Dtu_Settings_Get(settings, 0x8042U, 1.0);
|
|
ratings->residual_primary = Dtu_Settings_Get(settings, 0x8043U, 1.0);
|
|
ratings->residual_secondary = Dtu_Settings_Get(settings, 0x8044U, 1.0);
|
|
ratings->residual_ratio = Dtu_Settings_Get(settings, 0x8045U, 1.0);
|
|
ratings->power_ratio = Dtu_Settings_Get(settings, 0x8054U, 1.0);
|
|
polarity = Dtu_Settings_Get(settings, 0x8056U, 0.0);
|
|
ratings->power_negative = polarity == 1.0;
|
|
return dtu_measurement_positive(ratings->voltage_primary) &&
|
|
dtu_measurement_positive(ratings->voltage_secondary) &&
|
|
dtu_measurement_positive(ratings->voltage_ratio) &&
|
|
dtu_measurement_positive(ratings->current_primary) &&
|
|
dtu_measurement_positive(ratings->current_secondary) &&
|
|
dtu_measurement_positive(ratings->current_ratio) &&
|
|
dtu_measurement_positive(ratings->residual_primary) &&
|
|
dtu_measurement_positive(ratings->residual_secondary) &&
|
|
dtu_measurement_positive(ratings->residual_ratio) &&
|
|
dtu_measurement_positive(ratings->power_ratio) && (polarity == 0.0 || polarity == 1.0);
|
|
}
|
|
|
|
/* Class order matches the four XML deadband/zero-drift fields: I, AC U, DC U, P. */
|
|
static int dtu_measurement_class(uint32_t source_addr)
|
|
{
|
|
if (source_addr >= 0x961FU && source_addr <= 0x9626U) { return 0; }
|
|
if ((source_addr >= 0x9605U && source_addr <= 0x960CU) ||
|
|
(source_addr >= 0x960EU && source_addr <= 0x9615U)) { return 1; }
|
|
if (source_addr == 0x9601U || source_addr == 0x9602U) { return 2; }
|
|
if (source_addr >= 0x9627U && source_addr <= 0x9629U) { return 3; }
|
|
return -1;
|
|
}
|
|
|
|
bool Dtu_Measurement_Source_Valid(uint32_t source_addr)
|
|
{
|
|
return source_addr >= DTU_MEASUREMENT_FIRST_SOURCE && source_addr <= DTU_MEASUREMENT_LAST_SOURCE &&
|
|
(source_addr < 0x961CU || source_addr > 0x961EU);
|
|
}
|
|
|
|
size_t Dtu_Measurement_Index(uint32_t source_addr)
|
|
{
|
|
return Dtu_Measurement_Source_Valid(source_addr) ?
|
|
(size_t)(source_addr - DTU_MEASUREMENT_FIRST_SOURCE) : DTU_MEASUREMENT_SOURCE_COUNT;
|
|
}
|
|
|
|
void Dtu_Measurement_Init(DtuMeasurementInput *input)
|
|
{
|
|
if (input != NULL) { memset(input, 0, sizeof(*input)); }
|
|
}
|
|
|
|
static bool dtu_measurement_override(DtuMeasurementInput *input, uint32_t source_addr,
|
|
double value, bool secondary)
|
|
{
|
|
size_t index = Dtu_Measurement_Index(source_addr);
|
|
uint64_t mask;
|
|
if (input == NULL || index == DTU_MEASUREMENT_SOURCE_COUNT || !isfinite(value)) { return false; }
|
|
mask = UINT64_C(1) << index;
|
|
input->source[index] = value;
|
|
input->override_mask |= mask;
|
|
if (secondary) { input->secondary_mask |= mask; }
|
|
else { input->secondary_mask &= ~mask; }
|
|
return true;
|
|
}
|
|
|
|
bool Dtu_Measurement_Override(DtuMeasurementInput *input, uint32_t source_addr, double value)
|
|
{
|
|
return dtu_measurement_override(input, source_addr, value, false);
|
|
}
|
|
|
|
bool Dtu_Measurement_Override_Secondary(DtuMeasurementInput *input, uint32_t source_addr, double value)
|
|
{
|
|
return dtu_measurement_override(input, source_addr, value, true);
|
|
}
|
|
|
|
bool Dtu_Measurement_Clear_Override(DtuMeasurementInput *input, uint32_t source_addr)
|
|
{
|
|
size_t index = Dtu_Measurement_Index(source_addr);
|
|
uint64_t mask;
|
|
if (input == NULL || index == DTU_MEASUREMENT_SOURCE_COUNT) { return false; }
|
|
mask = UINT64_C(1) << index;
|
|
input->override_mask &= ~mask;
|
|
input->secondary_mask &= ~mask;
|
|
return true;
|
|
}
|
|
|
|
bool Dtu_Measurement_Sample(const DtuSettings *settings, uint64_t elapsed_ms,
|
|
bool breaker_closed, DtuMeasurementInput *input)
|
|
{
|
|
DtuMeasurementRatings ratings;
|
|
double generated[DTU_MEASUREMENT_SOURCE_COUNT] = {0.0};
|
|
const double root_three = 1.7320508075688772935;
|
|
uint64_t phase = elapsed_ms % UINT64_C(30000);
|
|
double current_pu = breaker_closed ? (phase >= 5000U && phase < 15000U ? 1.3 : 0.4) : 0.0;
|
|
double apparent;
|
|
size_t index, pt;
|
|
if (input == NULL || !dtu_measurement_ratings(settings, &ratings)) { return false; }
|
|
generated[0] = 24.0;
|
|
generated[1] = 48.0;
|
|
generated[2] = 2.0; /* Demonstration maximum discharge duration, hours. */
|
|
for (pt = 0U; pt < 2U; ++pt) {
|
|
size_t offset = 4U + pt * 9U;
|
|
for (index = 0U; index < 3U; ++index) {
|
|
generated[offset + index] = ratings.voltage_primary / root_three;
|
|
generated[offset + 3U + index] = ratings.voltage_primary;
|
|
}
|
|
generated[offset + 6U] = ratings.voltage_primary * 0.002;
|
|
generated[offset + 7U] = ratings.voltage_primary * 0.002;
|
|
generated[offset + 8U] = 50.0;
|
|
}
|
|
generated[0x9617U - DTU_MEASUREMENT_FIRST_SOURCE] = 25.0;
|
|
generated[0x961BU - DTU_MEASUREMENT_FIRST_SOURCE] = 50.0;
|
|
/* Position, communication count and both discharge sensors start at zero;
|
|
* those are simulation inputs, not claimed observations or inferred units. */
|
|
for (index = 0x961FU - DTU_MEASUREMENT_FIRST_SOURCE;
|
|
index <= 0x9624U - DTU_MEASUREMENT_FIRST_SOURCE; ++index) {
|
|
generated[index] = ratings.current_primary * current_pu;
|
|
}
|
|
generated[0x9625U - DTU_MEASUREMENT_FIRST_SOURCE] = ratings.residual_primary * 0.005;
|
|
generated[0x9626U - DTU_MEASUREMENT_FIRST_SOURCE] = ratings.residual_primary * 0.005;
|
|
apparent = root_three * ratings.voltage_primary * ratings.current_primary * current_pu;
|
|
generated[0x9627U - DTU_MEASUREMENT_FIRST_SOURCE] = apparent * 0.95;
|
|
generated[0x9628U - DTU_MEASUREMENT_FIRST_SOURCE] = apparent * sqrt(1.0 - 0.95 * 0.95);
|
|
generated[0x9629U - DTU_MEASUREMENT_FIRST_SOURCE] = apparent;
|
|
generated[0x962AU - DTU_MEASUREMENT_FIRST_SOURCE] = 0.95;
|
|
for (index = 0U; index < DTU_MEASUREMENT_SOURCE_COUNT; ++index) {
|
|
if ((input->override_mask & (UINT64_C(1) << index)) == 0U && !isfinite(generated[index])) { return false; }
|
|
}
|
|
for (index = 0U; index < DTU_MEASUREMENT_SOURCE_COUNT; ++index) {
|
|
uint64_t mask = UINT64_C(1) << index;
|
|
if ((input->override_mask & mask) == 0U) {
|
|
input->source[index] = generated[index];
|
|
input->secondary_mask &= ~mask;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static double dtu_measurement_threshold(const DtuSettings *settings, uint32_t source_addr, uint32_t first)
|
|
{
|
|
int kind = dtu_measurement_class(source_addr);
|
|
double value;
|
|
if (settings == NULL || !Dtu_Measurement_Source_Valid(source_addr)) { return NAN; }
|
|
if (kind < 0) { return 0.0; }
|
|
value = Dtu_Settings_Get(settings, first + (uint32_t)kind, 0.0);
|
|
return isfinite(value) && value >= 0.0 ? value : NAN;
|
|
}
|
|
|
|
double Dtu_Measurement_Zero_Absolute(const DtuSettings *settings, uint32_t source_addr)
|
|
{
|
|
return dtu_measurement_threshold(settings, source_addr, 0x8030U);
|
|
}
|
|
|
|
double Dtu_Measurement_Deadband(const DtuSettings *settings, uint32_t source_addr)
|
|
{
|
|
return dtu_measurement_threshold(settings, source_addr, 0x8020U);
|
|
}
|
|
|
|
double Dtu_Measurement_Energy_Factor(const DtuSettings *settings)
|
|
{
|
|
double value;
|
|
if (settings == NULL) { return NAN; }
|
|
value = Dtu_Settings_Get(settings, 0x8055U, 1.0);
|
|
return dtu_measurement_positive(value) ? value : NAN;
|
|
}
|
|
|
|
bool Dtu_Measurement_Process(const DtuSettings *settings,
|
|
const DtuMeasurementInput *input, DtuMeasurementFrame *frame)
|
|
{
|
|
DtuMeasurementRatings ratings;
|
|
DtuMeasurementFrame candidate;
|
|
size_t index;
|
|
if (input == NULL || frame == NULL || !dtu_measurement_ratings(settings, &ratings)) { return false; }
|
|
memset(&candidate, 0, sizeof(candidate));
|
|
for (index = 0U; index < DTU_MEASUREMENT_SOURCE_COUNT; ++index) {
|
|
uint32_t source_addr = DTU_MEASUREMENT_FIRST_SOURCE + (uint32_t)index;
|
|
double primary_rating = 1.0, secondary_rating = 1.0, ratio = 1.0;
|
|
double secondary, report, multiplier, zero;
|
|
int kind;
|
|
if (!Dtu_Measurement_Source_Valid(source_addr)) { continue; }
|
|
if (!isfinite(input->source[index])) { return false; }
|
|
kind = dtu_measurement_class(source_addr);
|
|
if (kind == 1) {
|
|
primary_rating = ratings.voltage_primary;
|
|
secondary_rating = ratings.voltage_secondary;
|
|
ratio = ratings.voltage_ratio;
|
|
} else if (kind == 0) {
|
|
bool residual = source_addr == 0x9625U || source_addr == 0x9626U;
|
|
primary_rating = residual ? ratings.residual_primary : ratings.current_primary;
|
|
secondary_rating = residual ? ratings.residual_secondary : ratings.current_secondary;
|
|
ratio = residual ? ratings.residual_ratio : ratings.current_ratio;
|
|
} else if (kind == 3) {
|
|
primary_rating = ratings.voltage_primary * ratings.current_primary;
|
|
secondary_rating = ratings.voltage_secondary * ratings.current_secondary;
|
|
ratio = ratings.voltage_ratio * ratings.current_ratio * ratings.power_ratio;
|
|
}
|
|
if (!dtu_measurement_positive(primary_rating) || !dtu_measurement_positive(secondary_rating) ||
|
|
!dtu_measurement_positive(ratio)) { return false; }
|
|
secondary = (input->secondary_mask & (UINT64_C(1) << index)) != 0U ?
|
|
input->source[index] : input->source[index] / primary_rating * secondary_rating;
|
|
zero = Dtu_Measurement_Zero_Absolute(settings, source_addr);
|
|
multiplier = Dtu_Settings_Get(settings, source_addr + 0xCC0U, 1.0);
|
|
if (!isfinite(secondary) || !isfinite(zero) || !isfinite(multiplier)) { return false; }
|
|
if (fabs(secondary) <= zero) { secondary = 0.0; }
|
|
report = secondary * ratio * multiplier;
|
|
if (ratings.power_negative && (source_addr == 0x9627U || source_addr == 0x9628U)) { report = -report; }
|
|
if (!isfinite(report) || fabs(report) > FLT_MAX) { return false; }
|
|
candidate.secondary[index] = secondary;
|
|
candidate.primary[index] = report;
|
|
}
|
|
*frame = candidate;
|
|
return true;
|
|
}
|
|
|
|
bool Dtu_Measurement_Changed(double value, double last_submitted, double absolute_deadband)
|
|
{
|
|
return isfinite(value) && isfinite(last_submitted) && isfinite(absolute_deadband) &&
|
|
absolute_deadband >= 0.0 && value != last_submitted && fabs(value - last_submitted) >= absolute_deadband;
|
|
}
|
|
|
|
int16_t Dtu_Measurement_To_Scaled(double value, double scale, bool *overflow)
|
|
{
|
|
double scaled;
|
|
if (overflow != NULL) { *overflow = false; }
|
|
if (isnan(value) || !isfinite(scale) || scale <= 0.0) {
|
|
if (overflow != NULL) { *overflow = true; }
|
|
return 0;
|
|
}
|
|
scaled = value * scale;
|
|
if (scaled >= 32767.5) {
|
|
if (overflow != NULL) { *overflow = true; }
|
|
return INT16_MAX;
|
|
}
|
|
if (scaled <= -32768.5) {
|
|
if (overflow != NULL) { *overflow = true; }
|
|
return INT16_MIN;
|
|
}
|
|
return (int16_t)(scaled >= 0.0 ? floor(scaled + 0.5) : ceil(scaled - 0.5));
|
|
}
|