/**************************************************************************** * control/lib_observer.c * * Copyright (C) 2018 Gregory Nutt. All rights reserved. * Author: Mateusz Szafoni * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name NuttX nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /**************************************************************************** * Included Files ****************************************************************************/ #include #include #include /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ /**************************************************************************** * Private Functions ****************************************************************************/ /**************************************************************************** * Public Functions ****************************************************************************/ /**************************************************************************** * Name: motor_observer_init * * Description: * Initialize motor observer * * Input Parameters: * observer - pointer to the common observer data * ao - pointer to the angle specific observer data * so - pointer to the speed specific observer data * per - observer execution period * * Returned Value: * None * ****************************************************************************/ void motor_observer_init(FAR struct motor_observer_s *observer, FAR void *ao, FAR void *so, float per) { DEBUGASSERT(observer != NULL); DEBUGASSERT(ao != NULL); DEBUGASSERT(so != NULL); DEBUGASSERT(per > 0.0); /* Set observer period */ observer->per = per; /* Connect angle estimation observer data */ observer->ao = ao; /* Connect speed estimation observer data */ observer->so = so; } /**************************************************************************** * Name: motor_observer_smo_init * * Description: * Initialize motor sliding mode observer. * * Input Parameters: * smo - pointer to the sliding mode observer private data * kslide - SMO gain * err_max - linear region upper limit * * Returned Value: * None * ****************************************************************************/ void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo, float kslide, float err_max) { DEBUGASSERT(smo != NULL); DEBUGASSERT(kslide > 0.0); DEBUGASSERT(err_max > 0.0); /* Initialize structure */ smo->k_slide = kslide; smo->err_max = err_max; } /**************************************************************************** * Name: motor_observer_smo * * Description: * One step of the SMO observer. * REFERENCE: http://ww1.microchip.com/downloads/en/AppNotes/01078B.pdf * * Below some theoretical backgrounds about SMO. * * The digitalized motor model can be represent as: * * d(i_s.)/dt = (-R/L)*i_s. + (1/L)*(v_s - e_s. - z) * * We compare estimated current (i_s.) with measured current (i_s): * * err = i_s. - i_s * * and get correction factor (z): * * sign = sing(err) * z = sign*K_SLIDE * * Once the digitalized model is compensated, we estimate BEMF (e_s.) by * filtering z: * * e_s. = low_pass(z) * * The estimated BEMF is filtered once again and used to approximate the * motor angle: * * e_filtered_s. = low_pass(e_s.) * theta = arctan(-e_alpha/e_beta) * * The estimated theta is phase-shifted due to low pass filtration, so we * need some phase compensation. More details below. * * where: * v_s - phase input voltage vector * i_s. - estimated phase current vector * i_s - phase current vector * e_s. - estimated phase BEMF vector * R - motor winding resistance * L - motor winding inductance * z - output correction factor voltage * * Input Parameters: * observer - (in/out) pointer to the common observer data * i_ab - (in) inverter alpha-beta current * v_ab - (in) inverter alpha-beta voltage * phy - (in) pointer to the motor physical parameters * * Returned Value: * None * ****************************************************************************/ void motor_observer_smo(FAR struct motor_observer_s *observer, FAR ab_frame_t *i_ab, FAR ab_frame_t *v_ab, FAR struct motor_phy_params_s *phy) { FAR struct motor_observer_smo_s *smo = (FAR struct motor_observer_smo_s *)observer->ao; FAR ab_frame_t *emf = &smo->emf; FAR ab_frame_t *z = &smo->z; FAR ab_frame_t *i_est = &smo->i_est; FAR ab_frame_t *v_err = &smo->v_err; FAR ab_frame_t *i_err = &smo->i_err; FAR ab_frame_t *sign = &smo->sign; float i_err_a_abs = 0.0; float i_err_b_abs = 0.0; float angle = 0.0; /* REVISIT: observer works only when IQ current is high enough */ /* Calculate observer gains */ smo->F_gain = (1 - observer->per*phy->res/phy->ind); smo->G_gain = observer->per/phy->ind; /* Saturate F gain */ if (smo->F_gain < 0) { smo->F_gain = 0.0; } /* Saturate G gain */ if (smo->G_gain > 0.999) { smo->G_gain = 0.999; } /* Configure low pass filters * * We tune low-pass filters to achieve cutoff frequency equal to * input singal frequency. This gives us constant phase shift between * input and outpu signals equals to: * * phi = -arctan(f_in/f_c) = -arctan(1) = -PI/2 * * Input signal frequency is equal to the frequency of the motor currents, * which give us: * * f_c = omega_e/(2*PI) * omega_m = omega_e/poles * f_c = omega_m*poles/(2*PI) * * filter = T * (2*PI) * f_c * filter = T * omega_m * poles * * T - [s] period at which the digital filter is being calculated * f_in - [Hz] input frequency of the filter * f_c - [Hz] cutoff frequency of the filter * omega_m - [rad/s] mechanical angular velocity * omega_e - [rad/s] electrical angular velocity * */ smo->emf_lp_filter1 = observer->per * observer->speed * phy->poles; smo->emf_lp_filter2 = smo->emf_lp_filter1; /* Get voltage error: v_err = v_ab - emf */ v_err->a = v_ab->a - emf->a; v_err->b = v_ab->b - emf->b; /* Estimate stator current */ i_est->a = smo->F_gain * i_est->a + smo->G_gain * (v_err->a - z->a); i_est->b = smo->F_gain * i_est->b + smo->G_gain * (v_err->b - z->b); /* Get motor current errror */ i_err->a = i_ab->a - i_est->a; i_err->b = i_ab->b - i_est->b; /* Slide-mode controller */ sign->a = (i_err->a > 0 ? 1.0 : -1.0); sign->b = (i_err->b > 0 ? 1.0 : -1.0); /* Get current error absolute value - just multiply value with its sign */ i_err_a_abs = i_err->a * sign->a; i_err_b_abs = i_err->b * sign->b; /* Calculate new output correction factor voltage */ if (i_err_a_abs < smo->err_max) { /* Enter linear region if error is small enough */ z->a = i_err->a * smo->k_slide / smo->err_max; } else { /* Non-linear region */ z->a = sign->a * smo->k_slide; } if (i_err_b_abs < smo->err_max) { /* Enter linear region if error is small enough */ z->b = i_err->b * smo->k_slide / smo->err_max; } else { /* Non-linear region */ z->b = sign->b * smo->k_slide; } /* Filter z to obtain estimated emf */ LP_FILTER(emf->a, z->a, smo->emf_lp_filter1); LP_FILTER(emf->b, z->b, smo->emf_lp_filter1); /* Filter emf one more time before angle stimation */ LP_FILTER(emf->a, emf->a, smo->emf_lp_filter2); LP_FILTER(emf->b, emf->b, smo->emf_lp_filter2); /* Estimate phase angle according to: * emf_a = -|emf| * sin(th) * emf_b = |emf| * cos(th) * th = atan2(-emf_a, emf->b) */ angle = fast_atan2(-emf->a, emf->b); #if 1 /* Some assertions * TODO: simplify */ if (angle != angle) angle = 0.0; if (emf->a != emf->a) emf->a = 0.0; if (emf->b != emf->b) emf->b = 0.0; if (z->a != z->a) z->a = 0.0; if (z->b != z->b) z->b = 0.0; if (i_est->a != i_est->a) i_est->a = 0.0; if (i_est->b != i_est->b) i_est->b = 0.0; #endif /* Angle compensation. * Due to low pass filtering we have some delay in estimated phase angle. * * Adaptive filters introduced above cause -PI/2 phase shift for each filter. * We use 2 times filtering which give us constant -PI phase shift. */ angle = angle -M_PI_F; /* Normalize angle to range <0, 2PI> */ angle_norm_2pi(&angle, 0.0, 2*M_PI_F); /* Store estimated angle in observer data*/ observer->angle = angle; }