}
}
-void pxmcc_axis_get_cur_dq_filt(pxmc_state_t *mcs, int *p_cur_d, int *p_cur_q)
+static inline
+void pxmcc_axis_get_cur_dq_filt_raw(pxmc_state_t *mcs,
+ int32_t *p_cur_d_raw, int32_t *p_cur_q_raw)
{
volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
uint32_t cur_q_cum = mcc_axis->cur_q_cum;
int32_t cur_d;
int32_t cur_q;
- int cur_d_div;
- int cur_q_div;
cur_d = cur_d_cum - mcsrc->cur_d_cum_prev;
mcsrc->cur_d_cum_prev = cur_d_cum;
cur_q = cur_q_cum - mcsrc->cur_q_cum_prev;
mcsrc->cur_q_cum_prev = cur_q_cum;
- cur_d_div = cur_d & 0x1f;
- cur_d = cur_d / cur_d_div;
- cur_q_div = cur_q & 0x1f;
- cur_q = cur_q / cur_q_div;
+ *p_cur_d_raw = cur_d;
+ *p_cur_q_raw = cur_q;
+}
- *p_cur_d = cur_d >> 12;
- *p_cur_q = cur_q >> 12;
+static inline
+void pxmcc_axis_cur_dq_raw2filt(int *p_cur, int32_t cur_raw)
+{
+ int cur_div;
+ int32_t cur;
+ cur_div = cur_raw & 0x1f;
+ cur = cur_raw / cur_div;
+ cur += (1 << 11) | 0x20;
+ *p_cur = cur >> 12;
+}
+
+
+void pxmcc_axis_get_cur_dq_filt(pxmc_state_t *mcs, int *p_cur_d, int *p_cur_q)
+{
+ int32_t cur_d_raw;
+ int32_t cur_q_raw;
+
+ pxmcc_axis_get_cur_dq_filt_raw(mcs, &cur_d_raw, &cur_q_raw);
+
+ pxmcc_axis_cur_dq_raw2filt(p_cur_d, cur_d_raw);
+ pxmcc_axis_cur_dq_raw2filt(p_cur_q, cur_q_raw);
+}
+
+static inline
+void pxmcc_cur_ctrl_pi(int *p_pwm, int32_t *p_cur_err_sum,
+ int cur_err, short cur_ctrl_p, short cur_ctrl_i, int max_pwm)
+{
+ int pwm;
+ int32_t cur_err_sum = *p_cur_err_sum;
+
+ pwm = (cur_err * cur_ctrl_p) >> 8;
+
+ if (cur_ctrl_i)
+ cur_err_sum += cur_err * cur_ctrl_i;
+ else
+ cur_err_sum = 0;
+
+ pwm += cur_err_sum >> 16;
+
+ if (pwm > max_pwm) {
+ cur_err_sum -= (pwm - max_pwm) << 16;
+ pwm = max_pwm;
+ } else if (-pwm > max_pwm) {
+ cur_err_sum -= (pwm + max_pwm) << 16;
+ pwm = 0 - max_pwm;
+ }
+ *p_cur_err_sum = cur_err_sum;
+ *p_pwm = pwm;
}
int
{
pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
+ int32_t cur_d_raw;
+ int32_t cur_q_raw;
+
+ pxmcc_axis_get_cur_dq_filt_raw(mcs, &cur_d_raw, &cur_q_raw);
if (mcs->pxms_flg & PXMS_ERR_m) {
pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
pxmcc_axis_pwm_dq_out(mcs, 0, 0);
mcc_axis->steps_inc = 0;
+ mcsrc->cur_d_err_sum = 0;
+ mcsrc->cur_q_err_sum = 0;
} else {
int pwm_d, pwm_q;
int cur_d, cur_q;
int max_pwm = mcs->pxms_me;
int32_t stpinc;
- /* cur_d_err_sum; */
- /* cur_q_err_sum; */
-
- pxmcc_axis_get_cur_dq_filt(mcs, &cur_d, &cur_q);
+ pxmcc_axis_cur_dq_raw2filt(&cur_d, cur_d_raw);
+ pxmcc_axis_cur_dq_raw2filt(&cur_q, cur_q_raw);
if (!mcs->pxms_ene)
cur_d_req = 0;
cur_d_err = cur_d_req - cur_d;
- pwm_d = (cur_d_err * mcsrc->cur_d_p) >> 8;
-
- if (mcsrc->cur_d_i)
- mcsrc->cur_d_err_sum += cur_d_err * mcsrc->cur_d_i;
- else
- mcsrc->cur_d_err_sum = 0;
-
- pwm_d += mcsrc->cur_d_err_sum >> 16;
+ pxmcc_cur_ctrl_pi(&pwm_d, &mcsrc->cur_d_err_sum, cur_d_err,
+ mcsrc->cur_d_p, mcsrc->cur_d_i, max_pwm);
/* pwm_d = (pxmc_rocon_pwm_magnitude * ene) >> 15; */
- if (pwm_d > max_pwm) {
- mcsrc->cur_d_err_sum -= (pwm_d - max_pwm) << 16;
- pwm_d = max_pwm;
- } else if (-pwm_d > max_pwm) {
- mcsrc->cur_d_err_sum -= (pwm_d + max_pwm) << 16;
- pwm_d = 0 - max_pwm;
- }
-
cur_q_req = 0;
cur_q_err = cur_q_req - cur_q;
- pwm_q = (cur_q_req * mcsrc->cur_q_p) >> 8;
-
- if (mcsrc->cur_q_i)
- mcsrc->cur_q_err_sum += cur_q_err * mcsrc->cur_q_i;
- else
- mcsrc->cur_q_err_sum = 0;
-
- pwm_q += mcsrc->cur_q_err_sum >> 16;
-
- if (pwm_q > max_pwm) {
- mcsrc->cur_q_err_sum -= (pwm_q - max_pwm) << 16;
- pwm_q = max_pwm;
- } else if (-pwm_q > max_pwm) {
- mcsrc->cur_q_err_sum -= (pwm_q + max_pwm) << 16;
- pwm_q = 0 - max_pwm;
- }
+ pxmcc_cur_ctrl_pi(&pwm_q, &mcsrc->cur_q_err_sum, cur_q_err,
+ mcsrc->cur_q_p, mcsrc->cur_q_i, max_pwm);
pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);