2018-05-30 14:36:06 +02:00
|
|
|
/****************************************************************************
|
2021-03-08 22:39:04 +01:00
|
|
|
* libs/libdsp/lib_observer.c
|
2018-05-30 14:36:06 +02:00
|
|
|
*
|
2020-10-10 14:50:05 +02:00
|
|
|
* Licensed to the Apache Software Foundation (ASF) under one or more
|
|
|
|
* contributor license agreements. See the NOTICE file distributed with
|
|
|
|
* this work for additional information regarding copyright ownership. The
|
|
|
|
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
|
|
|
* "License"); you may not use this file except in compliance with the
|
|
|
|
* License. You may obtain a copy of the License at
|
2018-05-30 14:36:06 +02:00
|
|
|
*
|
2020-10-10 14:50:05 +02:00
|
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
2018-05-30 14:36:06 +02:00
|
|
|
*
|
2020-10-10 14:50:05 +02:00
|
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
|
|
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
|
|
|
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
|
|
|
* License for the specific language governing permissions and limitations
|
|
|
|
* under the License.
|
2018-05-30 14:36:06 +02:00
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Included Files
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
#include <dsp.h>
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Pre-processor Definitions
|
|
|
|
****************************************************************************/
|
|
|
|
|
2020-04-03 21:18:18 +02:00
|
|
|
#define ANGLE_DIFF_THR (M_PI_F)
|
2018-05-30 14:36:06 +02:00
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* 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
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2021-02-28 21:36:51 +01:00
|
|
|
void motor_observer_init(FAR struct motor_observer_f32_s *observer,
|
2018-05-30 14:36:06 +02:00
|
|
|
FAR void *ao, FAR void *so, float per)
|
|
|
|
{
|
2021-02-28 21:41:59 +01:00
|
|
|
LIBDSP_DEBUGASSERT(observer != NULL);
|
|
|
|
LIBDSP_DEBUGASSERT(ao != NULL);
|
|
|
|
LIBDSP_DEBUGASSERT(so != NULL);
|
|
|
|
LIBDSP_DEBUGASSERT(per > 0.0f);
|
2018-07-07 19:04:57 +02:00
|
|
|
|
|
|
|
/* Reset observer data */
|
|
|
|
|
2021-02-28 21:36:51 +01:00
|
|
|
memset(observer, 0, sizeof(struct motor_observer_f32_s));
|
2018-05-30 14:36:06 +02:00
|
|
|
|
|
|
|
/* 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
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2021-02-28 21:36:51 +01:00
|
|
|
void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
|
|
|
|
float kslide, float err_max)
|
2018-05-30 14:36:06 +02:00
|
|
|
{
|
2021-02-28 21:41:59 +01:00
|
|
|
LIBDSP_DEBUGASSERT(smo != NULL);
|
|
|
|
LIBDSP_DEBUGASSERT(kslide > 0.0f);
|
|
|
|
LIBDSP_DEBUGASSERT(err_max > 0.0f);
|
2018-07-07 19:04:57 +02:00
|
|
|
|
|
|
|
/* Reset structure */
|
|
|
|
|
2021-02-28 21:36:51 +01:00
|
|
|
memset(smo, 0, sizeof(struct motor_observer_smo_f32_s));
|
2018-05-30 14:36:06 +02:00
|
|
|
|
|
|
|
/* Initialize structure */
|
|
|
|
|
|
|
|
smo->k_slide = kslide;
|
|
|
|
smo->err_max = err_max;
|
2021-03-01 19:56:36 +01:00
|
|
|
|
|
|
|
/* Store inverted err_max to avoid division */
|
|
|
|
|
2021-03-30 15:14:12 +02:00
|
|
|
smo->one_by_err_max = (1.0f / err_max);
|
2018-05-30 14:36:06 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* 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:
|
2018-07-07 19:04:57 +02:00
|
|
|
* o - (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
|
2021-03-01 19:56:36 +01:00
|
|
|
* dir - (in) rotation direction (1.0 for CCW, -1.0 for CW)
|
|
|
|
* NOTE: (mechanical dir) = -(electrical dir)
|
2018-05-30 14:36:06 +02:00
|
|
|
*
|
|
|
|
* Returned Value:
|
|
|
|
* None
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2021-02-28 21:36:51 +01:00
|
|
|
void motor_observer_smo(FAR struct motor_observer_f32_s *o,
|
|
|
|
FAR ab_frame_f32_t *i_ab, FAR ab_frame_f32_t *v_ab,
|
|
|
|
FAR struct motor_phy_params_f32_s *phy, float dir)
|
2018-05-30 14:36:06 +02:00
|
|
|
{
|
2021-02-28 21:41:59 +01:00
|
|
|
LIBDSP_DEBUGASSERT(o != NULL);
|
|
|
|
LIBDSP_DEBUGASSERT(i_ab != NULL);
|
|
|
|
LIBDSP_DEBUGASSERT(v_ab != NULL);
|
|
|
|
LIBDSP_DEBUGASSERT(phy != NULL);
|
2018-07-07 19:04:57 +02:00
|
|
|
|
2021-02-28 21:36:51 +01:00
|
|
|
FAR struct motor_observer_smo_f32_s *smo =
|
|
|
|
(FAR struct motor_observer_smo_f32_s *)o->ao;
|
|
|
|
FAR ab_frame_f32_t *emf = &smo->emf;
|
|
|
|
FAR ab_frame_f32_t *emf_f = &smo->emf_f;
|
|
|
|
FAR ab_frame_f32_t *z = &smo->z;
|
|
|
|
FAR ab_frame_f32_t *i_est = &smo->i_est;
|
|
|
|
FAR ab_frame_f32_t *v_err = &smo->v_err;
|
|
|
|
FAR ab_frame_f32_t *i_err = &smo->i_err;
|
|
|
|
FAR ab_frame_f32_t *sign = &smo->sign;
|
2018-07-07 19:04:57 +02:00
|
|
|
float i_err_a_abs = 0.0f;
|
|
|
|
float i_err_b_abs = 0.0f;
|
|
|
|
float angle = 0.0f;
|
|
|
|
float filter = 0.0f;
|
2018-05-30 14:36:06 +02:00
|
|
|
|
2021-03-01 19:56:36 +01:00
|
|
|
/* REVISIT: observer works only when IQ current is high enough
|
|
|
|
* Lower IQ current -> lower K_SLIDE
|
|
|
|
*/
|
2018-05-30 14:36:06 +02:00
|
|
|
|
|
|
|
/* Calculate observer gains */
|
|
|
|
|
2020-04-03 21:18:18 +02:00
|
|
|
smo->F = (1.0f - o->per * phy->res * phy->one_by_ind);
|
|
|
|
smo->G = o->per * phy->one_by_ind;
|
2018-05-30 14:36:06 +02:00
|
|
|
|
|
|
|
/* Saturate F gain */
|
|
|
|
|
2020-04-03 21:18:18 +02:00
|
|
|
if (smo->F < 0.0f)
|
2018-05-30 14:36:06 +02:00
|
|
|
{
|
2020-04-03 21:18:18 +02:00
|
|
|
smo->F = 0.0f;
|
2018-05-30 14:36:06 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
/* Saturate G gain */
|
|
|
|
|
2020-04-03 21:18:18 +02:00
|
|
|
if (smo->G > 0.999f)
|
2018-05-30 14:36:06 +02:00
|
|
|
{
|
2020-04-03 21:18:18 +02:00
|
|
|
smo->G = 0.999f;
|
2018-05-30 14:36:06 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
/* 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:
|
|
|
|
*
|
2018-07-07 19:04:57 +02:00
|
|
|
* phi = -arctan(f_in/f_c) = -arctan(1) = -45deg = -PI/4
|
2018-05-30 14:36:06 +02:00
|
|
|
*
|
|
|
|
* Input signal frequency is equal to the frequency of the motor currents,
|
|
|
|
* which give us:
|
|
|
|
*
|
|
|
|
* f_c = omega_e/(2*PI)
|
2018-07-07 19:04:57 +02:00
|
|
|
* omega_m = omega_e/pole_pairs
|
|
|
|
* f_c = omega_m*pole_pairs/(2*PI)
|
2018-05-30 14:36:06 +02:00
|
|
|
*
|
|
|
|
* filter = T * (2*PI) * f_c
|
2018-07-07 19:04:57 +02:00
|
|
|
* filter = T * omega_m * pole_pairs
|
2018-05-30 14:36:06 +02:00
|
|
|
*
|
2020-04-03 21:18:18 +02:00
|
|
|
* T - [s] period at which the digital filter is being
|
|
|
|
* calculated
|
2018-07-07 19:04:57 +02:00
|
|
|
* 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
|
|
|
|
* pole_pairs - pole pairs
|
2018-05-30 14:36:06 +02:00
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
2018-07-07 19:04:57 +02:00
|
|
|
filter = o->per * o->speed * phy->p;
|
|
|
|
|
|
|
|
/* Limit SMO filters
|
|
|
|
* REVISIT: lowest filter limit should depend on minimum speed:
|
|
|
|
* filter = T * (2*PI) * f_c = T * omega0
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
|
|
|
if (filter >= 1.0f)
|
|
|
|
{
|
|
|
|
filter = 0.99f;
|
|
|
|
}
|
|
|
|
else if (filter <= 0.0f)
|
|
|
|
{
|
|
|
|
filter = 0.005f;
|
|
|
|
}
|
|
|
|
|
|
|
|
smo->emf_lp_filter1 = filter;
|
2018-05-30 14:36:06 +02:00
|
|
|
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 */
|
|
|
|
|
2020-04-03 21:18:18 +02:00
|
|
|
i_est->a = smo->F * i_est->a + smo->G * (v_err->a - z->a);
|
|
|
|
i_est->b = smo->F * i_est->b + smo->G * (v_err->b - z->b);
|
2018-05-30 14:36:06 +02:00
|
|
|
|
2019-09-20 02:19:18 +02:00
|
|
|
/* Get motor current error */
|
2018-05-30 14:36:06 +02:00
|
|
|
|
|
|
|
i_err->a = i_ab->a - i_est->a;
|
|
|
|
i_err->b = i_ab->b - i_est->b;
|
|
|
|
|
|
|
|
/* Slide-mode controller */
|
|
|
|
|
2018-07-07 19:04:57 +02:00
|
|
|
sign->a = (i_err->a > 0.0f ? 1.0f : -1.0f);
|
|
|
|
sign->b = (i_err->b > 0.0f ? 1.0f : -1.0f);
|
2018-05-30 14:36:06 +02:00
|
|
|
|
|
|
|
/* 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 */
|
|
|
|
|
2021-03-01 19:56:36 +01:00
|
|
|
z->a = i_err->a * smo->k_slide * smo->one_by_err_max;
|
2018-05-30 14:36:06 +02:00
|
|
|
}
|
|
|
|
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 */
|
|
|
|
|
2021-03-01 19:56:36 +01:00
|
|
|
z->b = i_err->b * smo->k_slide * smo->one_by_err_max;
|
2018-05-30 14:36:06 +02:00
|
|
|
}
|
|
|
|
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 */
|
|
|
|
|
2018-07-07 19:04:57 +02:00
|
|
|
LP_FILTER(emf_f->a, emf->a, smo->emf_lp_filter2);
|
|
|
|
LP_FILTER(emf_f->b, emf->b, smo->emf_lp_filter2);
|
2018-05-30 14:36:06 +02:00
|
|
|
|
|
|
|
/* Estimate phase angle according to:
|
|
|
|
* emf_a = -|emf| * sin(th)
|
|
|
|
* emf_b = |emf| * cos(th)
|
|
|
|
* th = atan2(-emf_a, emf->b)
|
2021-03-01 19:56:36 +01:00
|
|
|
*
|
|
|
|
* NOTE: bottleneck but we can't do much more to optimise this
|
2018-05-30 14:36:06 +02:00
|
|
|
*/
|
|
|
|
|
|
|
|
angle = fast_atan2(-emf->a, emf->b);
|
|
|
|
|
|
|
|
/* Angle compensation.
|
|
|
|
* Due to low pass filtering we have some delay in estimated phase angle.
|
|
|
|
*
|
2020-04-03 21:18:18 +02:00
|
|
|
* Adaptive filters introduced above cause -PI/4 phase shift for each
|
|
|
|
* filter. We use 2 times filtering which give us constant -PI/2 (-90deg)
|
|
|
|
* phase shift.
|
2018-05-30 14:36:06 +02:00
|
|
|
*/
|
|
|
|
|
2018-07-07 19:04:57 +02:00
|
|
|
angle = angle + dir * M_PI_2_F;
|
2018-05-30 14:36:06 +02:00
|
|
|
|
|
|
|
/* Normalize angle to range <0, 2PI> */
|
|
|
|
|
2018-07-07 19:04:57 +02:00
|
|
|
angle_norm_2pi(&angle, 0.0f, 2.0f*M_PI_F);
|
2018-05-30 14:36:06 +02:00
|
|
|
|
2018-08-24 13:51:44 +02:00
|
|
|
/* Store estimated angle in observer data */
|
2018-05-30 14:36:06 +02:00
|
|
|
|
2018-07-07 19:04:57 +02:00
|
|
|
o->angle = angle;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: motor_sobserver_div_init
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Initialize DIV speed observer
|
|
|
|
*
|
|
|
|
* Input Parameters:
|
|
|
|
* so - (in/out) pointer to the DIV speed observer data
|
|
|
|
* sample - (in) number of mechanical angle samples
|
|
|
|
* filter - (in) low-pass filter for final omega
|
|
|
|
* per - (in) speed observer execution period
|
|
|
|
*
|
|
|
|
* Returned Value:
|
|
|
|
* None
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2021-02-28 21:36:51 +01:00
|
|
|
void motor_sobserver_div_init(FAR struct motor_sobserver_div_f32_s *so,
|
|
|
|
uint8_t samples, float filter, float per)
|
2018-07-07 19:04:57 +02:00
|
|
|
{
|
2021-02-28 21:41:59 +01:00
|
|
|
LIBDSP_DEBUGASSERT(so != NULL);
|
|
|
|
LIBDSP_DEBUGASSERT(samples > 0);
|
|
|
|
LIBDSP_DEBUGASSERT(filter > 0.0f);
|
2018-07-07 19:04:57 +02:00
|
|
|
|
|
|
|
/* Reset observer data */
|
|
|
|
|
2021-02-28 21:36:51 +01:00
|
|
|
memset(so, 0, sizeof(struct motor_sobserver_div_f32_s));
|
2018-07-07 19:04:57 +02:00
|
|
|
|
|
|
|
/* Store number of samples for DIV observer */
|
|
|
|
|
|
|
|
so->samples = samples;
|
|
|
|
|
|
|
|
/* Store low-pass filter for DIV observer speed */
|
|
|
|
|
|
|
|
so->filter = filter;
|
|
|
|
|
2021-03-01 19:56:36 +01:00
|
|
|
/* Store inverted sampling period */
|
2018-07-07 19:04:57 +02:00
|
|
|
|
2020-04-03 21:18:18 +02:00
|
|
|
so->one_by_dt = 1.0f / (so->samples * per);
|
2018-07-07 19:04:57 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: motor_sobserver_div
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Estimate motor mechanical speed based on motor mechanical angle
|
|
|
|
* difference.
|
|
|
|
*
|
|
|
|
* Input Parameters:
|
|
|
|
* o - (in/out) pointer to the common observer data
|
|
|
|
* angle - (in) mechanical angle normalized to <0.0, 2PI>
|
|
|
|
* dir - (in) mechanical rotation direction. Valid values:
|
|
|
|
* DIR_CW (1.0f) or DIR_CCW(-1.0f)
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2021-02-28 21:36:51 +01:00
|
|
|
void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
|
|
|
|
float angle, float dir)
|
2018-07-07 19:04:57 +02:00
|
|
|
{
|
2021-02-28 21:41:59 +01:00
|
|
|
LIBDSP_DEBUGASSERT(o != NULL);
|
|
|
|
LIBDSP_DEBUGASSERT(angle >= 0.0f && angle <= 2*M_PI_F);
|
|
|
|
LIBDSP_DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW);
|
2018-07-07 19:04:57 +02:00
|
|
|
|
2021-02-28 21:36:51 +01:00
|
|
|
FAR struct motor_sobserver_div_f32_s *so =
|
|
|
|
(FAR struct motor_sobserver_div_f32_s *)o->so;
|
2018-07-07 19:04:57 +02:00
|
|
|
volatile float omega = 0.0f;
|
|
|
|
|
|
|
|
/* Get angle diff */
|
|
|
|
|
|
|
|
so->angle_diff = angle - so->angle_prev;
|
|
|
|
|
|
|
|
/* Correct angle if we crossed angle boundary
|
|
|
|
* REVISIT:
|
|
|
|
*/
|
|
|
|
|
|
|
|
if ((dir == DIR_CW && so->angle_diff < -ANGLE_DIFF_THR) ||
|
|
|
|
(dir == DIR_CCW && so->angle_diff > ANGLE_DIFF_THR))
|
|
|
|
{
|
|
|
|
/* Correction sign depends on rotation direction */
|
|
|
|
|
2020-04-03 21:18:18 +02:00
|
|
|
so->angle_diff += dir * 2 * M_PI_F;
|
2018-07-07 19:04:57 +02:00
|
|
|
}
|
|
|
|
|
2021-07-04 17:31:21 +02:00
|
|
|
/* Get absolute value */
|
2018-07-07 19:04:57 +02:00
|
|
|
|
|
|
|
if (so->angle_diff < 0.0f)
|
|
|
|
{
|
|
|
|
so->angle_diff = -so->angle_diff;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Accumulate angle only if sample is valid */
|
|
|
|
|
|
|
|
so->angle_acc += so->angle_diff;
|
|
|
|
|
|
|
|
/* Increase counter */
|
|
|
|
|
|
|
|
so->cntr += 1;
|
|
|
|
|
|
|
|
/* Accumulate angle until we get configured number of samples */
|
|
|
|
|
|
|
|
if (so->cntr >= so->samples)
|
|
|
|
{
|
|
|
|
/* Estimate omega using accumulated angle samples.
|
|
|
|
* In this case use simple estimation:
|
|
|
|
*
|
|
|
|
* omega = delta_theta/delta_time
|
|
|
|
* speed_now = low_pass(omega)
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
|
|
|
omega = so->angle_acc*so->one_by_dt;
|
|
|
|
|
|
|
|
/* Store filtered omega.
|
|
|
|
*
|
|
|
|
* REVISIT: cut-off frequency for this filter should be
|
|
|
|
* (probably) set according to minimum supported omega:
|
|
|
|
*
|
|
|
|
* filter = T * (2*PI) * f_c = T * omega0
|
|
|
|
*
|
|
|
|
* where:
|
|
|
|
* omega0 - minimum angular speed
|
|
|
|
* T - speed estimation period (samples*one_by_dt)
|
|
|
|
*/
|
|
|
|
|
|
|
|
LP_FILTER(o->speed, omega, so->filter);
|
|
|
|
|
|
|
|
/* Reset samples counter and accumulated angle */
|
|
|
|
|
|
|
|
so->cntr = 0;
|
|
|
|
so->angle_acc = 0.0f;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Store current angle as previous angle */
|
|
|
|
|
|
|
|
so->angle_prev = angle;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: motor_observer_speed_get
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Get the estmiated motor mechanical speed from the observer
|
|
|
|
*
|
|
|
|
* Input Parameters:
|
|
|
|
* o - (in/out) pointer to the common observer data
|
|
|
|
*
|
|
|
|
* Returned Value:
|
|
|
|
* Return estimated motor mechanical speed from observer
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2021-02-28 21:36:51 +01:00
|
|
|
float motor_observer_speed_get(FAR struct motor_observer_f32_s *o)
|
2018-07-07 19:04:57 +02:00
|
|
|
{
|
2021-02-28 21:41:59 +01:00
|
|
|
LIBDSP_DEBUGASSERT(o != NULL);
|
2018-07-07 19:04:57 +02:00
|
|
|
|
|
|
|
return o->speed;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: motor_observer_angle_get
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Get the estmiated motor electrical angle from the observer
|
|
|
|
*
|
|
|
|
* Input Parameters:
|
|
|
|
* o - (in/out) pointer to the common observer data
|
|
|
|
*
|
|
|
|
* Returned Value:
|
|
|
|
* Return estimated motor mechanical angle from observer
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2021-02-28 21:36:51 +01:00
|
|
|
float motor_observer_angle_get(FAR struct motor_observer_f32_s *o)
|
2018-07-07 19:04:57 +02:00
|
|
|
{
|
2021-02-28 21:41:59 +01:00
|
|
|
LIBDSP_DEBUGASSERT(o != NULL);
|
2018-07-07 19:04:57 +02:00
|
|
|
|
|
|
|
return o->angle;
|
2018-05-30 14:36:06 +02:00
|
|
|
}
|