рефакторинг to_float->u2f
и всякое декоративное по модели
This commit is contained in:
@@ -22,14 +22,16 @@ SIM__MCUHandleTypeDef hmcu; ///< Хендл для управления
|
||||
*/
|
||||
const int inLengths[IN_PORT_NUMB] = {
|
||||
ADC_PORT_1_WIDTH,
|
||||
IN_PORT_2_WIDTH
|
||||
PUI_PORT_2_WIDTH,
|
||||
INTERNAL_PORT_3_WIDTH
|
||||
};
|
||||
/**
|
||||
* @brief Таблица смещений в выходном массиве IN
|
||||
*/
|
||||
const int inOffsets[IN_PORT_NUMB] = {
|
||||
OFFSET_IN_ARRAY_1,
|
||||
OFFSET_IN_ARRAY_2
|
||||
OFFSET_IN_ARRAY_2,
|
||||
OFFSET_IN_ARRAY_3
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@@ -53,9 +53,10 @@
|
||||
|
||||
// Parameters of S_Function
|
||||
// INPUT/OUTPUTS PARAMS START
|
||||
#define IN_PORT_NUMB 2
|
||||
#define IN_PORT_NUMB 3
|
||||
#define ADC_PORT_1_WIDTH 6
|
||||
#define IN_PORT_2_WIDTH 16
|
||||
#define PUI_PORT_2_WIDTH 10
|
||||
#define INTERNAL_PORT_3_WIDTH 16
|
||||
|
||||
#define OUT_PORT_NUMB 5
|
||||
#define THYR_PORT_1_WIDTH 6
|
||||
@@ -94,11 +95,12 @@
|
||||
|
||||
// INPUT/OUTPUTS AUTO-PARAMS START
|
||||
/// === Полный размер буфера ===
|
||||
#define TOTAL_IN_SIZE (ADC_PORT_1_WIDTH + IN_PORT_2_WIDTH)
|
||||
#define TOTAL_IN_SIZE (ADC_PORT_1_WIDTH + PUI_PORT_2_WIDTH + INTERNAL_PORT_3_WIDTH)
|
||||
|
||||
/// === Смещения массивов (внутри общего буфера) ===
|
||||
#define OFFSET_IN_ARRAY_1 0
|
||||
#define OFFSET_IN_ARRAY_2 (OFFSET_IN_ARRAY_1 + ADC_PORT_1_WIDTH)
|
||||
#define OFFSET_IN_ARRAY_3 (OFFSET_IN_ARRAY_2 + PUI_PORT_2_WIDTH)
|
||||
|
||||
/// === Полный размер буфера ===
|
||||
#define TOTAL_OUT_SIZE (THYR_PORT_1_WIDTH + DO_PORT_2_WIDTH + PM_PORT_3_WIDTH + ANGLE_PORT_4_WIDTH + OUT_PORT_5_WIDTH)
|
||||
|
||||
@@ -7,7 +7,6 @@
|
||||
#include "app_wrapper.h"
|
||||
|
||||
float dbg[16];
|
||||
extern float dbg_iref;
|
||||
#define PIN_READ(_verbname_) (_verbname_##_GPIO_Port->ODR & (_verbname_##_Pin)) ? 1 : 0
|
||||
|
||||
void Write_UPP_Outputs(real_T* Buffer, int ind_port)
|
||||
@@ -121,18 +120,29 @@ void app_readInputs(const real_T* Buffer) {
|
||||
ADC_Set_Channel_Value(ADC3, 8, ReadInputArray(0,4));
|
||||
ADC_Set_Channel_Value(ADC3, 10, ReadInputArray(0,5));
|
||||
|
||||
dbg_iref = ReadInputArray(1, 0);
|
||||
upp.call->go = ReadInputArray(1, 1);
|
||||
upp.call->go = ReadInputArray(1, 0);
|
||||
|
||||
MB_INTERNAL.param.angle.Angle_Max = ReadInputArray(1, 2)*65535;
|
||||
MB_INTERNAL.param.angle.Angle_Min = ReadInputArray(1, 3)*65535;
|
||||
MB_INTERNAL.param.angle.PID_Kp = ReadInputArray(1, 4) * 10000;
|
||||
MB_INTERNAL.param.angle.PID_Ki = ReadInputArray(1, 5) * 10000;
|
||||
MB_INTERNAL.param.angle.PID_Kd = ReadInputArray(1, 6) * 10000;
|
||||
MB_DATA.HoldRegs.pui_params.Tnt = ReadInputArray(1, 7) * 1000;
|
||||
if (upp.workmode != UPP_Work)
|
||||
{
|
||||
MB_DATA.HoldRegs.pui_params.Iref = ReadInputArray(1, 1);
|
||||
MB_DATA.HoldRegs.pui_params.Tnt = ReadInputArray(1, 2);
|
||||
MB_DATA.HoldRegs.pui_params.Umin = ReadInputArray(1, 3);
|
||||
MB_DATA.HoldRegs.pui_params.Umax = ReadInputArray(1, 4);
|
||||
MB_DATA.HoldRegs.pui_params.Imax = ReadInputArray(1, 5);
|
||||
MB_DATA.HoldRegs.pui_params.Imin = ReadInputArray(1, 6);
|
||||
MB_DATA.HoldRegs.pui_params.TiMax = ReadInputArray(1, 7);
|
||||
MB_DATA.HoldRegs.pui_params.Tdelay = ReadInputArray(1, 8);
|
||||
MB_DATA.HoldRegs.pui_params.Interlace = ReadInputArray(1, 9);
|
||||
|
||||
MB_INTERNAL.param.nominal.U = ReadInputArray(1, 8) * 10;
|
||||
MB_INTERNAL.param.nominal.I = ReadInputArray(1, 9) * 10;
|
||||
MB_INTERNAL.param.angle.Angle_Max = ReadInputArray(2, 0) * 65535;
|
||||
MB_INTERNAL.param.angle.Angle_Min = ReadInputArray(2, 1) * 65535;
|
||||
MB_INTERNAL.param.angle.PID_Kp = ReadInputArray(2, 2) * 10000;
|
||||
MB_INTERNAL.param.angle.PID_Ki = ReadInputArray(2, 3) * 10000;
|
||||
MB_INTERNAL.param.angle.PID_Kd = ReadInputArray(2, 4) * 10000;
|
||||
|
||||
MB_INTERNAL.param.nominal.U = ReadInputArray(2, 5) * 10;
|
||||
MB_INTERNAL.param.nominal.I = ReadInputArray(2, 6) * 10;
|
||||
}
|
||||
// USER APP INPUT END
|
||||
}
|
||||
|
||||
|
||||
Binary file not shown.
Submodule UPP/AllLibs/Modbus updated: e0ce0e6dbf...df3f71cdff
Submodule UPP/AllLibs/MyLibs updated: 30fdbc35dd...0fd9c78c32
@@ -21,6 +21,7 @@
|
||||
#include "modbus_holdregs.h"
|
||||
#include "modbus_inputregs.h"
|
||||
#include "modbus_devid.h"
|
||||
#include "upp_main.h"
|
||||
|
||||
/* DEFINE DATA FOR MODBUS */
|
||||
MB_DataStructureTypeDef MB_DATA = {0}; ///< Coils & Registers
|
||||
@@ -66,10 +67,16 @@ MB_ExceptionTypeDef MB_Check_Address_For_Arr(uint16_t Addr, uint16_t Qnt, uint16
|
||||
* @details Определение адреса начального регистра.
|
||||
* @note WriteFlag пока не используется.
|
||||
*/
|
||||
MB_ExceptionTypeDef MB_DefineRegistersAddress(uint16_t **pRegs, uint16_t Addr, uint16_t Qnt, uint8_t RegisterType)
|
||||
MB_ExceptionTypeDef MB_DefineRegistersAddress(uint16_t **pRegs, uint16_t Addr, uint16_t Qnt, uint8_t RegisterType, uint8_t WriteFlag)
|
||||
{
|
||||
/* В режиме работа ничего не записываем */
|
||||
if(upp.workmode == UPP_Work)
|
||||
{
|
||||
return ET_ILLEGAL_FUNCTION;
|
||||
}
|
||||
|
||||
/* check quantity error */
|
||||
if (Qnt > DATA_SIZE)
|
||||
if (Qnt > MbData_size)
|
||||
{
|
||||
return ET_ILLEGAL_DATA_VALUE; // return exception code
|
||||
}
|
||||
@@ -125,6 +132,12 @@ MB_ExceptionTypeDef MB_DefineRegistersAddress(uint16_t **pRegs, uint16_t Addr, u
|
||||
*/
|
||||
MB_ExceptionTypeDef MB_DefineCoilsAddress(uint16_t **pCoils, uint16_t Addr, uint16_t Qnt, uint16_t *start_shift, uint8_t WriteFlag)
|
||||
{
|
||||
/* В режиме работа ничего не записываем */
|
||||
if(upp.workmode == UPP_Work)
|
||||
{
|
||||
return ET_ILLEGAL_FUNCTION;
|
||||
}
|
||||
|
||||
/* check quantity error */
|
||||
if (Qnt > 2000)
|
||||
{
|
||||
|
||||
@@ -33,31 +33,31 @@ HAL_StatusTypeDef PowerMonitor_Init(PowerMonitor_t *hpm)
|
||||
/* Инициализация каналов АЦП */
|
||||
if(ADC_ConfigChannel(&hpm->adc, ADC_CHANNEL_UBA,
|
||||
PARAM_INTERNAL.adc.ADC_Zero[ADC_CHANNEL_UBA],
|
||||
to_float(PARAM_INTERNAL.adc.ADC_Max[ADC_CHANNEL_UBA], 10),
|
||||
u2f(PARAM_INTERNAL.adc.ADC_Max[ADC_CHANNEL_UBA], 10),
|
||||
4095) != HAL_OK)
|
||||
return HAL_ERROR;
|
||||
|
||||
if(ADC_ConfigChannel(&hpm->adc, ADC_CHANNEL_UAC,
|
||||
PARAM_INTERNAL.adc.ADC_Zero[ADC_CHANNEL_UAC],
|
||||
to_float(PARAM_INTERNAL.adc.ADC_Max[ADC_CHANNEL_UAC], 10),
|
||||
u2f(PARAM_INTERNAL.adc.ADC_Max[ADC_CHANNEL_UAC], 10),
|
||||
4095) != HAL_OK)
|
||||
return HAL_ERROR;
|
||||
|
||||
if(ADC_ConfigChannel(&hpm->adc, ADC_CHANNEL_IC,
|
||||
PARAM_INTERNAL.adc.ADC_Zero[ADC_CHANNEL_IC],
|
||||
to_float(PARAM_INTERNAL.adc.ADC_Max[ADC_CHANNEL_IC], 10),
|
||||
u2f(PARAM_INTERNAL.adc.ADC_Max[ADC_CHANNEL_IC], 10),
|
||||
4095) != HAL_OK)
|
||||
return HAL_ERROR;
|
||||
|
||||
if(ADC_ConfigChannel(&hpm->adc, ADC_CHANNEL_IA,
|
||||
PARAM_INTERNAL.adc.ADC_Zero[ADC_CHANNEL_IA],
|
||||
to_float(PARAM_INTERNAL.adc.ADC_Max[ADC_CHANNEL_IA], 10),
|
||||
u2f(PARAM_INTERNAL.adc.ADC_Max[ADC_CHANNEL_IA], 10),
|
||||
4095) != HAL_OK)
|
||||
return HAL_ERROR;
|
||||
|
||||
|
||||
/* Инициализация алгоритма перехода через ноль */
|
||||
if(ZC_Init(&hpm->zc, 3, to_float(PARAM_INTERNAL.zc.Hysteresis, 100), PARAM_INTERNAL.zc.DebouneCouner) != HAL_OK)
|
||||
if(ZC_Init(&hpm->zc, 3, u2f(PARAM_INTERNAL.zc.Hysteresis, 100), PARAM_INTERNAL.zc.DebouneCouner) != HAL_OK)
|
||||
return HAL_ERROR;
|
||||
|
||||
/* Инициализация каналов алгоритма перехода через ноль */
|
||||
@@ -72,7 +72,7 @@ HAL_StatusTypeDef PowerMonitor_Init(PowerMonitor_t *hpm)
|
||||
/* Инициализация экпоненциального фильтра медленного алга */
|
||||
for(int i = 0; i < EXP_ALL; i++)
|
||||
{
|
||||
if(FilterExp_Init(&hpm->exp[i], to_float(PARAM_INTERNAL.pm.mean_alpha,65535)))
|
||||
if(FilterExp_Init(&hpm->exp[i], u2f(PARAM_INTERNAL.pm.mean_alpha,65535)))
|
||||
return HAL_ERROR;
|
||||
Filter_Start(&hpm->exp[i]);
|
||||
}
|
||||
@@ -151,8 +151,8 @@ void PowerMonitor_SlowCalc(PowerMonitor_t *hpm)
|
||||
float iphase_mean = 0; // средний ток каждой фазы
|
||||
float uphase_mean = 0; // среднее напряжение каждой фазы
|
||||
// Дополнительно посчитаем значения в реальных Вольтах/Амперах
|
||||
float u_base = to_float(PARAM_INTERNAL.nominal.U, 10);
|
||||
float i_base = to_float(PARAM_INTERNAL.nominal.I, 10);
|
||||
float u_base = u2f(PARAM_INTERNAL.nominal.U, 10);
|
||||
float i_base = u2f(PARAM_INTERNAL.nominal.I, 10);
|
||||
for(int i = 0; i < 3; i++)
|
||||
{
|
||||
/* Получение частоты фазы */
|
||||
@@ -212,8 +212,8 @@ void PowerMonitor_FastCalc(PowerMonitor_t *hpm)
|
||||
ADC_Handle(&hpm->adc);
|
||||
|
||||
/* Заполняем Напряжения/Токи в о.е. */
|
||||
float u_base = to_float(PARAM_INTERNAL.nominal.U, 10);
|
||||
float i_base = to_float(PARAM_INTERNAL.nominal.I, 10);
|
||||
float u_base = u2f(PARAM_INTERNAL.nominal.U, 10);
|
||||
float i_base = u2f(PARAM_INTERNAL.nominal.I, 10);
|
||||
PowerMonitor_Measured_t *meas = &hpm->measured;
|
||||
meas->fast.U[U_BA] = hpm->adc.Data[ADC_CHANNEL_UBA]/u_base;
|
||||
meas->fast.U[U_AC] = hpm->adc.Data[ADC_CHANNEL_UAC]/u_base;
|
||||
|
||||
@@ -17,9 +17,9 @@
|
||||
int Protect_Voltages(PowerMonitor_Measured_t *measure, UPP_PUI_Params_t *protect, UPP_ParamsNominal_t *nominal)
|
||||
{
|
||||
/* Переводим уставки ПУИ в удобный вид */
|
||||
float lUmin = to_float(protect->Umin, 100)/**to_float(nominal->U, 10)*/;
|
||||
float lUmax = to_float(protect->Umax, 100)/**to_float(nominal->U, 10)*/;
|
||||
float lPhaseSequence = to_float(nominal->PhaseSequence, 100);
|
||||
float lUmin = u2f(protect->Umin, 100)/**u2f(nominal->U, 10)*/;
|
||||
float lUmax = u2f(protect->Umax, 100)/**u2f(nominal->U, 10)*/;
|
||||
float lPhaseSequence = u2f(nominal->PhaseSequence, 100);
|
||||
|
||||
/* Общее напряжение */
|
||||
if(measure->final.Uamp > lUmax)
|
||||
@@ -58,9 +58,9 @@ int Protect_Voltages(PowerMonitor_Measured_t *measure, UPP_PUI_Params_t *protect
|
||||
int Protect_Currents(PowerMonitor_Measured_t *measure, UPP_PUI_Params_t *protect, UPP_ParamsNominal_t *nominal)
|
||||
{
|
||||
/* Переводим уставки ПУИ в удобный вид */
|
||||
float lIref = to_float(protect->Iref, 100)/**to_float(nominal->I, 10)*/;
|
||||
float lImax = to_float(protect->Imax, 100)/**to_float(nominal->I, 10)*/;
|
||||
float lImin = to_float(protect->Imin, 100)/**to_float(nominal->I, 10)*/;
|
||||
float lIref = u2f(protect->Iref, 100)/**u2f(nominal->I, 10)*/;
|
||||
float lImin = u2f(protect->Imin, 100)/**u2f(nominal->I, 10)*/;
|
||||
float lImax = u2f(protect->Imax, 100) * 50 / u2f(nominal->I, 10); // Imax процентов от 50 А, в о.е. от номинального
|
||||
|
||||
/* Общий ток */
|
||||
if(measure->final.Iamp > lImax)
|
||||
@@ -133,12 +133,12 @@ int Protect_Currents(PowerMonitor_Measured_t *measure, UPP_PUI_Params_t *protect
|
||||
void Protect_Misc(PowerMonitor_Measured_t *measure, UPP_PUI_Params_t *protect, UPP_ParamsNominal_t *nominal)
|
||||
{
|
||||
/* Переводим внутренние уставки в удобный вид */
|
||||
float lFnom = to_float(PARAM_INTERNAL.nominal.F, 100);
|
||||
float lFmin = lFnom - lFnom*to_float(PARAM_INTERNAL.nominal.F_deviation_minus, 10000);
|
||||
float lFmax = lFnom + lFnom*to_float(PARAM_INTERNAL.nominal.F_deviation_plus, 10000);
|
||||
float lFnom = u2f(PARAM_INTERNAL.nominal.F, 100);
|
||||
float lFmin = lFnom - lFnom*u2f(PARAM_INTERNAL.nominal.F_deviation_minus, 10000);
|
||||
float lFmax = lFnom + lFnom*u2f(PARAM_INTERNAL.nominal.F_deviation_plus, 10000);
|
||||
|
||||
float lTwarn = to_float(PARAM_INTERNAL.setpoints.TemperatureWarn, 100);
|
||||
float lTerr = to_float(PARAM_INTERNAL.setpoints.TemperatureWarn, 100);
|
||||
float lTwarn = u2f(PARAM_INTERNAL.setpoints.TemperatureWarn, 100);
|
||||
float lTerr = u2f(PARAM_INTERNAL.setpoints.TemperatureWarn, 100);
|
||||
|
||||
|
||||
/*=============== ЗАЩИТЫ ПО ЧАСТОТЕ ==================*/
|
||||
|
||||
@@ -33,16 +33,16 @@ HAL_StatusTypeDef Angle_Init(Angle_Handle_t *hangle)
|
||||
Angle_Reset(hangle, UPP_PHASE_C);
|
||||
|
||||
// Инициализация углов
|
||||
float angle_max = to_float(PARAM_INTERNAL.angle.Angle_Max, 65535);
|
||||
float angle_min = to_float(PARAM_INTERNAL.angle.Angle_Min, 65535);
|
||||
float angle_max = u2f(PARAM_INTERNAL.angle.Angle_Max, 65535);
|
||||
float angle_min = u2f(PARAM_INTERNAL.angle.Angle_Min, 65535);
|
||||
|
||||
hangle->f.Initialized = 1;
|
||||
|
||||
hangle->Config.PeriodLimit = 1;
|
||||
// Инициализация ПИД
|
||||
float kp = to_float(PARAM_INTERNAL.angle.PID_Kp, 10000);
|
||||
float ki = to_float(PARAM_INTERNAL.angle.PID_Ki, 10000);
|
||||
float kd = to_float(PARAM_INTERNAL.angle.PID_Kd, 10000);
|
||||
float ref_alpha = PUI_Tnt_CalcAlpha(to_float(PARAM_PUI.Tnt, 1000), PM_SLOW_PERIOD_US);
|
||||
float kp = u2f(PARAM_INTERNAL.angle.PID_Kp, 10000);
|
||||
float ki = u2f(PARAM_INTERNAL.angle.PID_Ki, 10000);
|
||||
float kd = u2f(PARAM_INTERNAL.angle.PID_Kd, 10000);
|
||||
float ref_alpha = PUI_Tnt_CalcAlpha(u2f(PARAM_PUI.Tnt, 1000), PM_SLOW_PERIOD_US);
|
||||
|
||||
return Angle_PID_Init(hangle, kp, ki, kd, ref_alpha);;
|
||||
}
|
||||
@@ -309,10 +309,15 @@ HAL_StatusTypeDef Angle_SetLimit(Angle_Handle_t *hangle, float PeriodLimit)
|
||||
if(hangle->f.Running)
|
||||
return HAL_BUSY;
|
||||
|
||||
if(PeriodLimit < 0 || PeriodLimit > 1)
|
||||
if(PeriodLimit <= 0 || PeriodLimit > 1)
|
||||
return HAL_ERROR;
|
||||
|
||||
hangle->Config.PeriodLimit = PeriodLimit;
|
||||
|
||||
if (hangle->Config.AngleMax > hangle->Config.PeriodLimit)
|
||||
hangle->Config.AngleMax = hangle->Config.PeriodLimit;
|
||||
if (hangle->Config.AngleMin > hangle->Config.PeriodLimit)
|
||||
hangle->Config.AngleMin = hangle->Config.PeriodLimit;
|
||||
|
||||
return HAL_OK;
|
||||
}
|
||||
|
||||
@@ -61,7 +61,7 @@ HAL_StatusTypeDef PWM_Init(PWM_Handle_t *hpwm)
|
||||
|
||||
PWM_SetConfig(hpwm, PARAM_INTERNAL.pwm.PhaseMask,
|
||||
PARAM_INTERNAL.pwm.Frequency,
|
||||
to_float(PARAM_INTERNAL.pwm.Duty, 100),
|
||||
u2f(PARAM_INTERNAL.pwm.Duty, 100),
|
||||
PARAM_INTERNAL.pwm.PulseNumber);
|
||||
|
||||
HAL_TIM_PWM_Start(&hpwm1, PWM_CHANNEL_1);
|
||||
|
||||
@@ -77,7 +77,7 @@ void UPP_Errors_Power(void)
|
||||
void UPP_Errors_Ranges(void)
|
||||
{
|
||||
/* Преобразуем уставки в нормальные тики */
|
||||
float ticksTiMax = to_float(PARAM_PUI.TiMax, 1)/PM_SLOW_PERIOD_US;
|
||||
float ticksTiMax = u2f(PARAM_PUI.TiMax, 1)/PM_SLOW_PERIOD_US;
|
||||
/* Счетчики для отсчитывания задержки выставления ошибки */
|
||||
static int IMaxCnt = 0;
|
||||
static int UMaxCnt = 0;
|
||||
|
||||
@@ -15,7 +15,6 @@
|
||||
|
||||
UPP_t upp;
|
||||
HAL_StatusTypeDef res; // сюда сохраняется результат от выполения всяких функций
|
||||
float dbg_iref = 2;
|
||||
int dbg_polarity = 1;
|
||||
|
||||
/**
|
||||
@@ -66,6 +65,10 @@ int UPP_PreWhile(void)
|
||||
if(PowerMonitor_Start(&upp.pm) != HAL_OK)
|
||||
return 1;
|
||||
|
||||
#ifdef MATLAB
|
||||
dbg_polarity = 0;
|
||||
#endif
|
||||
|
||||
UPP_DO.CEN(ENABLE);
|
||||
return 0;
|
||||
}
|
||||
@@ -93,7 +96,7 @@ int UPP_While(void)
|
||||
PowerMonitor_Protect(&upp.pm, 0);
|
||||
#else
|
||||
// Защиты // Определенные защиты по току включаем только в после разгона
|
||||
PowerMonitor_Protect(&upp.pm, (fabsf(upp.hangle.Iref - dbg_iref) < 0.1));
|
||||
PowerMonitor_Protect(&upp.pm, (fabsf(upp.hangle.Iref - u2f(PARAM_PUI.Iref, 100)) < 0.1));
|
||||
#endif
|
||||
// Обрабока ошибок и выставление итоговой Ошибки
|
||||
UPP_Errors_Handle();
|
||||
@@ -175,7 +178,7 @@ int UPP_While(void)
|
||||
upp.workmode = UPP_Init;
|
||||
|
||||
// Регулирование тиристоров
|
||||
Angle_PID(&upp.hangle, dbg_iref, upp.pm.measured.final.Iamp);
|
||||
Angle_PID(&upp.hangle, u2f(PARAM_PUI.Iref,100), upp.pm.measured.final.Iamp);
|
||||
|
||||
// если слишком долгий запуск
|
||||
if((local_time() - upp.StartTick) > (upp.PUI.params->Tdelay*1000))
|
||||
|
||||
@@ -74,8 +74,8 @@ void UPP_Params_ControlPUI(void)
|
||||
if(upp.call->go) // при запущеном УПП ничего не меняем
|
||||
return;
|
||||
|
||||
/* Tnt Уставка на скорость нарастания пускового тока Iref */
|
||||
float angle_ref_alphaPUI = PUI_Tnt_CalcAlpha(to_float(PARAM_PUI.Tnt, 1000), PM_SLOW_PERIOD_US);
|
||||
/* Tnt - Уставка на скорость нарастания пускового тока */
|
||||
float angle_ref_alphaPUI = PUI_Tnt_CalcAlpha(u2f(PARAM_PUI.Tnt, 1000), PM_SLOW_PERIOD_US);
|
||||
float angle_ref_alpha = upp.hangle.refFilter.alpha;
|
||||
if(angle_ref_alpha != angle_ref_alphaPUI)
|
||||
{
|
||||
@@ -383,7 +383,7 @@ static void __AngleSetLimit(void)
|
||||
float pulses_percent_of_period = (((float)PARAM_INTERNAL.pwm.PulseNumber / PARAM_INTERNAL.pwm.Frequency) * 1000) / ANGLE_PERIOD_MS(upp.pm.measured.final.Fmean);
|
||||
// Вычитаем этот процент из 1 - получаем максимально безопасный угол
|
||||
float angle_limit = 1 - pulses_percent_of_period;
|
||||
angle_limit -= pulses_percent_of_period*to_float(PARAM_INTERNAL.angle.PulseLengthReserve, 100); // добавляем запас в PulseLengthReserve процентов от пачки импульсов
|
||||
angle_limit -= pulses_percent_of_period*u2f(PARAM_INTERNAL.angle.PulseLengthReserve, 100); // добавляем запас в PulseLengthReserve процентов от пачки импульсов
|
||||
Angle_SetLimit(&upp.hangle, angle_limit);
|
||||
}
|
||||
|
||||
|
||||
@@ -10,7 +10,7 @@
|
||||
#define _UPP_PARAMS_H
|
||||
#include "upp_defs.h"
|
||||
|
||||
#define to_float(_u16_, _coef_) ((float)_u16_/_coef_)
|
||||
#define u2f(_u16_, _coef_) ((float)_u16_/_coef_)
|
||||
|
||||
typedef struct
|
||||
{
|
||||
|
||||
@@ -17,8 +17,8 @@
|
||||
<TargetCommonOption>
|
||||
<Device>STM32F427ZGTx</Device>
|
||||
<Vendor>STMicroelectronics</Vendor>
|
||||
<PackID>Keil.STM32F4xx_DFP.2.17.1</PackID>
|
||||
<PackURL>https://www.keil.com/pack/</PackURL>
|
||||
<PackID>Keil.STM32F4xx_DFP.2.16.0</PackID>
|
||||
<PackURL>http://www.keil.com/pack/</PackURL>
|
||||
<Cpu>IRAM(0x20000000-0x2002FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4") TZ</Cpu>
|
||||
<FlashUtilSpec></FlashUtilSpec>
|
||||
<StartupFile></StartupFile>
|
||||
@@ -1004,8 +1004,8 @@
|
||||
<TargetCommonOption>
|
||||
<Device>STM32F417ZGTx</Device>
|
||||
<Vendor>STMicroelectronics</Vendor>
|
||||
<PackID>Keil.STM32F4xx_DFP.2.17.1</PackID>
|
||||
<PackURL>https://www.keil.com/pack/</PackURL>
|
||||
<PackID>Keil.STM32F4xx_DFP.2.16.0</PackID>
|
||||
<PackURL>http://www.keil.com/pack/</PackURL>
|
||||
<Cpu>IRAM(0x20000000,0x00020000) IRAM2(0x10000000,0x00010000) IROM(0x08000000,0x00100000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE</Cpu>
|
||||
<FlashUtilSpec></FlashUtilSpec>
|
||||
<StartupFile></StartupFile>
|
||||
|
||||
Reference in New Issue
Block a user