76 lines
2.0 KiB
C
76 lines
2.0 KiB
C
#include "stm32f1xx_matlab_gpio.h"
|
|
#include "upp.h"
|
|
|
|
|
|
#define detect_front(_in_numb_, _var_, _val_) { \
|
|
if ((in[_in_numb_] > 0.5) && (prev_in[_in_numb_] <= 0.5)) \
|
|
{ \
|
|
_var_ = _val_; \
|
|
} }
|
|
|
|
#define detect_rise(_in_numb_, _var_, _val_) { \
|
|
if ((in[_in_numb_] < 0.5) && (prev_in[_in_numb_] >= 0.5)) \
|
|
{ \
|
|
_var_ = _val_; \
|
|
} }
|
|
|
|
|
|
|
|
void WriteFromSFunc(real_T* disc)
|
|
{
|
|
|
|
for (int i = 0; i < PORT_WIDTH; i++)
|
|
{
|
|
if (GPIOA->ODR & (1 << i))
|
|
{
|
|
disc[i] = 1;
|
|
}
|
|
|
|
if (GPIOB->ODR & (1 << i))
|
|
{
|
|
disc[PORT_WIDTH + i] = 1;
|
|
}
|
|
}
|
|
disc[2 * PORT_WIDTH + 0] = phase_A.ctrl.angle.delay_us;
|
|
disc[2 * PORT_WIDTH + 1] = (uint16_t)((uint16_t)TIMER->CNT - phase_A.ctrl.angle.start_delay_tick);
|
|
disc[2 * PORT_WIDTH + 2] = phase_A.ctrl.angle.start_delay_tick;
|
|
disc[2 * PORT_WIDTH + 3] = TIMER->CNT;
|
|
}
|
|
|
|
|
|
|
|
void ReadToSFunc(real_T* in)
|
|
{
|
|
static real_T prev_in[IN_PORT_WIDTH];
|
|
|
|
detect_front(0, phase_A.zc_detector.f.EXTIZeroCrossDetected, 1);
|
|
detect_rise(0, phase_A.zc_detector.f.EXTIZeroCrossDetected, 1);
|
|
|
|
detect_front(1, phase_B.zc_detector.f.EXTIZeroCrossDetected, 1);
|
|
detect_rise(1, phase_B.zc_detector.f.EXTIZeroCrossDetected, 1);
|
|
|
|
detect_front(2, phase_C.zc_detector.f.EXTIZeroCrossDetected, 1);
|
|
detect_rise(2, phase_C.zc_detector.f.EXTIZeroCrossDetected, 1);
|
|
|
|
detect_front(3, Upp.GoSafe, 1);
|
|
detect_rise(3, Upp.GoSafe, 0);
|
|
|
|
detect_front(4, Upp.Prepare, 1);
|
|
detect_rise(4, Upp.Prepare, 0);
|
|
|
|
detect_front(5, Upp.ForceStop, 1);
|
|
detect_rise(5, Upp.ForceStop, 0);
|
|
|
|
detect_front(6, Upp.ForceDisconnect, 1);
|
|
detect_rise(6, Upp.ForceDisconnect, 0);
|
|
|
|
|
|
Upp.sine_freq = in[7];
|
|
Upp.Duration = in[8];
|
|
|
|
|
|
for (int i = 0; i < IN_PORT_WIDTH; i++)
|
|
{
|
|
prev_in[i] = in[i];
|
|
}
|
|
} |