Files
STM32F4-Base/IEC60870-Slave/examples/iec104_dtu_measurement.c
T
STF bdcc8b6500
CI / L0 静态门禁 (pre-commit) (push) Failing after 7m46s
CI / L0.5 主机侧单测 (MSVC) (push) Successful in 21s
添加遥控、参数装载、文件传输等业务
2026-11-16 22:19:17 -08:00

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));
}