diff --git a/include/dsp.h b/include/dsp.h index 58640b596e..18198764f6 100644 --- a/include/dsp.h +++ b/include/dsp.h @@ -329,31 +329,43 @@ struct motor_phy_params_f32_s float one_by_ind; /* Inverse phase-to-neutral inductance */ }; -/* Field oriented control (FOC) data - * REVISIT: - */ +/* FOC initialize data */ + +struct foc_initdata_f32_s +{ + float id_kp; /* KP for d current */ + float id_ki; /* KI for d current */ + float iq_kp; /* KP for q current */ + float iq_ki; /* KI for q current */ +}; + +/* Field Oriented Control (FOC) data */ struct foc_data_f32_s { abc_frame_f32_t v_abc; /* Voltage in ABC frame */ ab_frame_f32_t v_ab; /* Voltage in alpha-beta frame */ - dq_frame_f32_t v_dq; /* Voltage in dq frame */ + dq_frame_f32_t v_dq; /* Requested voltage in dq frame */ ab_frame_f32_t v_ab_mod; /* Modulation voltage normalized to * magnitude (0.0, 1.0) */ abc_frame_f32_t i_abc; /* Current in ABC frame */ - ab_frame_f32_t i_ab; /* Current in apha-beta frame */ + ab_frame_f32_t i_ab; /* Current in alpha-beta frame */ dq_frame_f32_t i_dq; /* Current in dq frame */ dq_frame_f32_t i_dq_err; /* DQ current error */ - dq_frame_f32_t i_dq_ref; /* Current dq reference frame */ + dq_frame_f32_t i_dq_ref; /* Requested current for the FOC + * current controler + */ - pid_controller_f32_t id_pid; /* Current d-axis component PI controller */ - pid_controller_f32_t iq_pid; /* Current q-axis component PI controller */ + pid_controller_f32_t id_pid; /* Current d-axis component PI controller */ + pid_controller_f32_t iq_pid; /* Current q-axis component PI controller */ float vdq_mag_max; /* Maximum dq voltage magnitude */ float vab_mod_scale; /* Voltage alpha-beta modulation scale */ + + phase_angle_f32_t angle; /* Phase angle */ }; /**************************************************************************** @@ -423,16 +435,24 @@ void svm3(FAR struct svm3_state_f32_s *s, FAR ab_frame_f32_t *ab); void svm3_current_correct(FAR struct svm3_state_f32_s *s, float *c0, float *c1, float *c2); -/* Field Oriented control */ +/* Field Oriented Control */ +void foc_init(FAR struct foc_data_f32_s *foc, + FAR struct foc_initdata_f32_s *init); void foc_vbase_update(FAR struct foc_data_f32_s *foc, float vbase); -void foc_idq_ref_set(FAR struct foc_data_f32_s *data, float d, float q); - -void foc_init(FAR struct foc_data_f32_s *data, - float id_kp, float id_ki, float iq_kp, float iq_ki); -void foc_process(FAR struct foc_data_f32_s *foc, - FAR abc_frame_f32_t *i_abc, - FAR phase_angle_f32_t *angle); +void foc_angle_update(FAR struct foc_data_f32_s *foc, + FAR phase_angle_f32_t *angle); +void foc_iabc_update(FAR struct foc_data_f32_s *foc, + FAR abc_frame_f32_t *i_abc); +void foc_voltage_control(FAR struct foc_data_f32_s *foc, + FAR dq_frame_f32_t *vdq_ref); +void foc_current_control(FAR struct foc_data_f32_s *foc, + FAR dq_frame_f32_t *idq_ref, + FAR dq_frame_f32_t *vdq_comp, + FAR dq_frame_f32_t *v_dq_ref); +void foc_vabmod_get(FAR struct foc_data_f32_s *foc, + FAR ab_frame_f32_t *v_ab_mod); +void foc_vdq_mag_max_get(FAR struct foc_data_f32_s *foc, FAR float *max); /* BLDC/PMSM motor observers */ diff --git a/libs/libdsp/lib_foc.c b/libs/libdsp/lib_foc.c index 0b41216a54..74f66a3c12 100644 --- a/libs/libdsp/lib_foc.c +++ b/libs/libdsp/lib_foc.c @@ -32,24 +32,28 @@ ****************************************************************************/ /**************************************************************************** - * Name: foc_current_control + * Name: foc_current_controler * * Description: - * This function implements FOC current control algorithm. + * This function implements FOC current controler algorithm. * * Input Parameters: - * foc - (in/out) pointer to the FOC data + * foc - (in/out) pointer to the FOC data + * v_dq_req - (in) pointer to the voltage DQ request frame * * Returned Value: * None * ****************************************************************************/ -static void foc_current_control(FAR struct foc_data_f32_s *foc) +static void foc_current_controler(FAR struct foc_data_f32_s *foc, + FAR dq_frame_f32_t *v_dq_req) { FAR pid_controller_f32_t *id_pid = &foc->id_pid; FAR pid_controller_f32_t *iq_pid = &foc->iq_pid; - FAR dq_frame_f32_t *v_dq = &foc->v_dq; + + LIBDSP_DEBUGASSERT(foc != NULL); + LIBDSP_DEBUGASSERT(v_dq_req != NULL); /* Get dq current error */ @@ -60,18 +64,11 @@ static void foc_current_control(FAR struct foc_data_f32_s *foc) /* PI controller for d-current (flux loop) */ - v_dq->d = pi_controller(id_pid, foc->i_dq_err.d); + v_dq_req->d = pi_controller(id_pid, foc->i_dq_err.d); /* PI controller for q-current (torque loop) */ - v_dq->q = pi_controller(iq_pid, foc->i_dq_err.q); - - /* Saturate voltage DQ vector. - * The maximum DQ voltage magnitude depends on the maximum possible - * phase voltage and the maximum supported duty cycle. - */ - - dq_saturate(v_dq, foc->vdq_mag_max); + v_dq_req->q = pi_controller(iq_pid, foc->i_dq_err.q); } /**************************************************************************** @@ -91,6 +88,8 @@ static void foc_current_control(FAR struct foc_data_f32_s *foc) static void foc_vab_mod_scale_set(FAR struct foc_data_f32_s *foc, float scale) { + LIBDSP_DEBUGASSERT(foc != NULL); + foc->vab_mod_scale = scale; } @@ -111,6 +110,8 @@ static void foc_vab_mod_scale_set(FAR struct foc_data_f32_s *foc, static void foc_vdq_mag_max_set(FAR struct foc_data_f32_s *foc, float max) { + LIBDSP_DEBUGASSERT(foc != NULL); + foc->vdq_mag_max = max; /* Update regulators saturation */ @@ -119,6 +120,56 @@ static void foc_vdq_mag_max_set(FAR struct foc_data_f32_s *foc, float max) pi_saturation_set(&foc->iq_pid, -foc->vdq_mag_max, foc->vdq_mag_max); } +/**************************************************************************** + * Name: foc_vdq_ref_set + * + * Description: + * Set dq requested voltage vector + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * vdq_ref - (in) pointer to the requested idq voltage + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void foc_vdq_ref_set(FAR struct foc_data_f32_s *foc, + FAR dq_frame_f32_t *vdq_ref) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + LIBDSP_DEBUGASSERT(vdq_ref != NULL); + + foc->v_dq.d = vdq_ref->d; + foc->v_dq.q = vdq_ref->q; +} + +/**************************************************************************** + * Name: foc_idq_ref_set + * + * Description: + * Set dq reference current vector + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * idq_ref - (in) pointer to the reference idq current + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void foc_idq_ref_set(FAR struct foc_data_f32_s *foc, + FAR dq_frame_f32_t *idq_ref) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + LIBDSP_DEBUGASSERT(idq_ref != NULL); + + foc->i_dq_ref.d = idq_ref->d; + foc->i_dq_ref.q = idq_ref->q; +} + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -130,11 +181,8 @@ static void foc_vdq_mag_max_set(FAR struct foc_data_f32_s *foc, float max) * Initialize FOC controller * * Input Parameters: - * foc - (in/out) pointer to the FOC data - * id_kp - (in) KP for d current - * id_ki - (in) KI for d current - * iq_kp - (in) KP for q current - * iq_ki - (in) KI for q current + * foc - (in/out) pointer to the FOC data + * init - (in) pointer to the FOC initialization data * * Returned Value: * None @@ -142,7 +190,7 @@ static void foc_vdq_mag_max_set(FAR struct foc_data_f32_s *foc, float max) ****************************************************************************/ void foc_init(FAR struct foc_data_f32_s *foc, - float id_kp, float id_ki, float iq_kp, float iq_ki) + FAR struct foc_initdata_f32_s *init) { /* Reset data */ @@ -150,40 +198,28 @@ void foc_init(FAR struct foc_data_f32_s *foc, /* Initialize PI current d component */ - pi_controller_init(&foc->id_pid, id_kp, id_ki); + pi_controller_init(&foc->id_pid, init->id_kp, init->id_ki); /* Initialize PI current q component */ - pi_controller_init(&foc->iq_pid, iq_kp, iq_ki); -} + pi_controller_init(&foc->iq_pid, init->iq_kp, init->iq_ki); -/**************************************************************************** - * Name: foc_idq_ref_set - * - * Description: - * Set dq reference current vector - * - * Input Parameters: - * foc - (in/out) pointer to the FOC data - * d - (in) reference d current - * q - (in) reference q current - * - * Returned Value: - * None - * - ****************************************************************************/ + /* Disable PI intergral part reset when saturated */ -void foc_idq_ref_set(FAR struct foc_data_f32_s *foc, float d, float q) -{ - foc->i_dq_ref.d = d; - foc->i_dq_ref.q = q; + pi_ireset_enable(&foc->iq_pid, false); + pi_ireset_enable(&foc->id_pid, false); + + /* Enable PI anti-windup protection */ + + pi_antiwindup_enable(&foc->iq_pid, 0.99, true); + pi_antiwindup_enable(&foc->id_pid, 0.99, true); } /**************************************************************************** * Name: foc_vbase_update * * Description: - * Update base voltage for FOC controller + * Update base voltage for FOC controller * * Input Parameters: * foc - (in/out) pointer to the FOC data @@ -194,48 +230,87 @@ void foc_idq_ref_set(FAR struct foc_data_f32_s *foc, float d, float q) * ****************************************************************************/ -void foc_vbase_update(FAR struct foc_data_s *foc, float vbase) +void foc_vbase_update(FAR struct foc_data_f32_s *foc, float vbase) { float scale = 0.0f; float mag_max = 0.0f; - /* Only if voltage is valid */ + /* Prevent division by zero */ - if (vbase >= 0.0f) + if (vbase < 1e-10f) { - scale = 1.0f / vbase; - mag_max = vbase; + vbase = 1e-10f; } + /* NOTE: this is base voltage for FOC, not bus voltage! */ + + scale = (1.0f / vbase); + mag_max = vbase; + + /* Update */ + foc_vab_mod_scale_set(foc, scale); foc_vdq_mag_max_set(foc, mag_max); } /**************************************************************************** - * Name: foc_process + * Name: foc_angle_update * * Description: - * Process FOC (Field Oriented Control) + * Update FOC data with new motor phase angle. * * Input Parameters: * foc - (in/out) pointer to the FOC data - * i_abc - (in) pointer to the ABC current frame * angle - (in) pointer to the phase angle data * * Returned Value: * None * - * TODO: add some reference and a brief description of the FOC + ****************************************************************************/ + +void foc_angle_update(FAR struct foc_data_f32_s *foc, + FAR phase_angle_f32_t *angle) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + LIBDSP_DEBUGASSERT(angle != NULL); + + /* Copy angle to foc data */ + + foc->angle.angle = angle->angle; + foc->angle.sin = angle->sin; + foc->angle.cos = angle->cos; +} + +/**************************************************************************** + * Name: foc_iabc_update + * + * Description: + * Update FOC data with new iabc frame. + * + * To work properly this function requires some additional steps: + * 1. phase angle must be set with foc_angle_update() + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * i_abc - (in) pointer to the ABC current frame + * + * Returned Value: + * None * ****************************************************************************/ -void foc_process(FAR struct foc_data_f32_s *foc, - FAR abc_frame_f32_t *i_abc, - FAR phase_angle_f32_t *angle) +void foc_iabc_update(FAR struct foc_data_f32_s *foc, + FAR abc_frame_f32_t *i_abc) { + dq_frame_f32_t i_dq; + LIBDSP_DEBUGASSERT(foc != NULL); LIBDSP_DEBUGASSERT(i_abc != NULL); - LIBDSP_DEBUGASSERT(angle != NULL); + + /* Reset data */ + + i_dq.d = 0; + i_dq.q = 0; /* Copy ABC current to foc data */ @@ -249,15 +324,47 @@ void foc_process(FAR struct foc_data_f32_s *foc, /* Convert alpha-beta current to dq current */ - park_transform(angle, &foc->i_ab, &foc->i_dq); + park_transform(&foc->angle, &foc->i_ab, &i_dq); - /* Run FOC current control (current dq -> voltage dq) */ + /* Store dq current */ - foc_current_control(foc); + foc->i_dq.d = i_dq.d; + foc->i_dq.q = i_dq.q; +} + +/**************************************************************************** + * Name: foc_voltage_control + * + * Description: + * Process FOC voltage control. + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * vdq_ref - (in) voltage dq reference frame + * + * Returned Value: + * None + * + ****************************************************************************/ + +void foc_voltage_control(FAR struct foc_data_f32_s *foc, + FAR dq_frame_f32_t *vdq_ref) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + + /* Update VDQ request */ + + foc_vdq_ref_set(foc, vdq_ref); /* Inverse Park transform (voltage dq -> voltage alpha-beta) */ - inv_park_transform(angle, &foc->v_dq, &foc->v_ab); + inv_park_transform(&foc->angle, &foc->v_dq, &foc->v_ab); + +#ifdef CONFIG_LIBDSP_FOC_VABC + /* Inverse Clarke transform (voltage alpha-beta -> voltage abc) */ + + inv_clarke_transform(&foc->v_ab, &foc->v_abc); +#endif /* Normalize the alpha-beta voltage to get the alpha-beta modulation * voltage @@ -266,3 +373,91 @@ void foc_process(FAR struct foc_data_f32_s *foc, foc->v_ab_mod.a = foc->v_ab.a * foc->vab_mod_scale; foc->v_ab_mod.b = foc->v_ab.b * foc->vab_mod_scale; } + +/**************************************************************************** + * Name: foc_current_control + * + * Description: + * Process FOC current control. + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * idq_ref - (in) current dq reference frame + * vdq_comp - (in) voltage dq compensation frame + * vdq_ref - (out) voltage dq reference frame + * + * Returned Value: + * None + * + * TODO: add some reference and a brief description of the FOC + * + ****************************************************************************/ + +void foc_current_control(FAR struct foc_data_f32_s *foc, + FAR dq_frame_f32_t *idq_ref, + FAR dq_frame_f32_t *vdq_comp, + FAR dq_frame_f32_t *vdq_ref) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + + /* Update IDQ reference */ + + foc_idq_ref_set(foc, idq_ref); + + /* Run FOC current controler (current dq -> voltage dq) */ + + foc_current_controler(foc, vdq_ref); + + /* DQ voltage compensation */ + + vdq_ref->d = vdq_ref->d - vdq_comp->d; + vdq_ref->q = vdq_ref->q - vdq_comp->q; +} + +/**************************************************************************** + * Name: foc_vabmod_get + * + * Description: + * Get result from the FOC controler (foc_current_control or + * foc_voltage_control) + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * v_ab_mod - (out) pointer to the voltage alpha-beta modulation frame + * + * Returned Value: + * None + * + ****************************************************************************/ + +void foc_vabmod_get(FAR struct foc_data_f32_s *foc, + FAR ab_frame_f32_t *v_ab_mod) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + LIBDSP_DEBUGASSERT(v_ab_mod != NULL); + + v_ab_mod->a = foc->v_ab_mod.a; + v_ab_mod->b = foc->v_ab_mod.b; +} + +/**************************************************************************** + * Name: foc_vdq_mag_max_get + * + * Description: + * Get maximum dq voltage vector magnitude + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * max - (out) maximum dq voltage magnitude + * + * Returned Value: + * None + * + ****************************************************************************/ + +void foc_vdq_mag_max_get(FAR struct foc_data_f32_s *foc, FAR float *max) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + + *max = foc->vdq_mag_max; +}