/****************************************************************************
* control/lib_misc.c
*
* 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
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* 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.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <dsp.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define VECTOR2D_SATURATE_MAG_MIN (1e-10f)
#define FAST_ATAN2_SMALLNUM (1e-10f)
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: f_saturate
*
* Description:
* Saturate float number
*
* Input Parameters:
* val - pointer to float number
* min - lower limit
* max - upper limit
*
* Returned Value:
* None
*
****************************************************************************/
void f_saturate(FAR float *val, float min, float max)
{
if (*val < min)
{
*val = min;
}
else if (*val > max)
{
*val = max;
}
}
/****************************************************************************
* Name: vector2d_mag
*
* Description:
* Get 2D vector magnitude.
*
* Input Parameters:
* x - (in) vector x component
* y - (in) vector y component
*
* Returned Value:
* Return 2D vector magnitude
*
****************************************************************************/
float vector2d_mag(float x, float y)
{
return sqrtf(x * x + y * y);
}
/****************************************************************************
* Name: vector2d_saturate
*
* Description:
* Saturate 2D vector magnitude.
*
* Input Parameters:
* x - (in/out) pointer to the vector x component
* y - (in/out) pointer to the vector y component
* max - (in) maximum vector magnitude
*
* Returned Value:
* None
*
****************************************************************************/
void vector2d_saturate(FAR float *x, FAR float *y, float max)
{
float mag = 0.0f;
float tmp = 0.0f;
/* Get vector magnitude */
mag = vector2d_mag(*x, *y);
if (mag < VECTOR2D_SATURATE_MAG_MIN)
{
mag = VECTOR2D_SATURATE_MAG_MIN;
}
if (mag > max)
{
/* Saturate vector */
tmp = max / mag;
*x *= tmp;
*y *= tmp;
}
}
/****************************************************************************
* Name: dq_mag
*
* Description:
* Get DQ vector magnitude.
*
* Input Parameters:
* dq - (in/out) dq frame vector
*
* Returned Value:
* Return dq vector magnitude
*
****************************************************************************/
float dq_mag(FAR dq_frame_t *dq)
{
return vector2d_mag(dq->d, dq->q);
}
/****************************************************************************
* Name: dq_saturate
*
* Description:
* Saturate dq frame vector magnitude.
*
* Input Parameters:
* dq - (in/out) dq frame vector
* max - (in) maximum vector magnitude
*
* Returned Value:
* None
*
****************************************************************************/
void dq_saturate(FAR dq_frame_t *dq, float max)
{
vector2d_saturate(&dq->d, &dq->q, max);
}
/****************************************************************************
* Name: fast_sin
*
* Description:
* Fast sin calculation
*
* Reference: http://lab.polygonal.de/?p=205
*
* Input Parameters:
* angle - (in)
*
* Returned Value:
* Return estimated sine value
*
****************************************************************************/
float fast_sin(float angle)
{
float sin = 0.0f;
float n1 = 1.27323954f;
float n2 = 0.405284735f;
/* Normalize angle */
angle_norm_2pi(&angle, -M_PI_F, M_PI_F);
/* Get estiamte sine value from quadratic equation */
if (angle < 0.0f)
{
sin = n1 * angle + n2 * angle * angle;
}
else
{
sin = n1 * angle - n2 * angle * angle;
}
return sin;
}
/****************************************************************************
* Name:fast_cos
*
* Description:
* Fast cos calculation
*
* Input Parameters:
* angle - (in)
*
* Returned Value:
* Return estimated cosine value
*
****************************************************************************/
float fast_cos(float angle)
{
/* Get cosine value from sine sin(x + PI/2) = cos(x) */
return fast_sin(angle + M_PI_2_F);
}
/****************************************************************************
* Name: fast_sin2
*
* Description:
* Fast sin calculation with better accuracy
*
* Reference: http://lab.polygonal.de/?p=205
*
* Input Parameters:
* angle
*
* Returned Value:
* Return estimated sine value
*
****************************************************************************/
float fast_sin2(float angle)
{
float sin = 0.0f;
float n1 = 1.27323954f;
float n2 = 0.405284735f;
float n3 = 0.225f;
/* Normalize angle */
angle_norm_2pi(&angle, -M_PI_F, M_PI_F);
/* Get estiamte sine value from quadratic equation and do more */
if (angle < 0.0f)
{
sin = n1 * angle + n2 * angle * angle;
if (sin < 0.0f)
{
sin = n3 * (sin *(-sin) - sin) + sin;
}
else
{
sin = n3 * (sin * sin - sin) + sin;
}
}
else
{
sin = n1 * angle - n2 * angle * angle;
if (sin < 0.0f)
{
sin = n3 * (sin *(-sin) - sin) + sin;
}
else
{
sin = n3 * (sin * sin - sin) + sin;
}
}
return sin;
}
/****************************************************************************
* Name:fast_cos2
*
* Description:
* Fast cos calculation with better accuracy
*
* Input Parameters:
* angle - (in)
*
* Returned Value:
* Return estimated cosine value
*
****************************************************************************/
float fast_cos2(float angle)
{
/* Get cosine value from sine sin(x + PI/2) = cos(x) */
return fast_sin2(angle + M_PI_2_F);
}
/****************************************************************************
* Name: fast_atan2
*
* Description:
* Fast atan2 calculation
*
* REFERENCE:
* https://dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization/
*
* Input Parameters:
* x - (in)
* y - (in)
*
* Returned Value:
* Return estimated angle
*
****************************************************************************/
float fast_atan2(float y, float x)
{
float angle = 0.0f;
float abs_y = 0.0f;
float rsq = 0.0f;
float r = 0.0f;
float n1 = 0.1963f;
float n2 = 0.9817f;
/* Get absolute value of y and add some small number to prevent 0/0 */
abs_y = fabsf(y) + FAST_ATAN2_SMALLNUM;
/* Calculate angle */
if (x >= 0.0f)
{
r = (x - abs_y) / (x + abs_y);
rsq = r * r;
angle = ((n1 * rsq) - n2) * r + (M_PI_F / 4.0f);
}
else
{
r = (x + abs_y) / (abs_y - x);
rsq = r * r;
angle = ((n1 * rsq) - n2) * r + (3.0f * M_PI_F / 4.0f);
}
/* Get angle sign */
if (y < 0.0f)
{
angle = -angle;
}
return angle;
}
/****************************************************************************
* Name: angle_norm
*
* Description:
* Normalize radians angle to a given boundary and a given period.
*
* Input Parameters:
* angle - (in/out) pointer to the angle data
* per - (in) angle period
* bottom - (in) lower limit
* top - (in) upper limit
*
* Returned Value:
* None
*
****************************************************************************/
void angle_norm(FAR float *angle, float per, float bottom, float top)
{
while (*angle > top)
{
/* Move the angle backwards by given period */
*angle = *angle - per;
}
while (*angle < bottom)
{
/* Move the angle forwards by given period */
*angle = *angle + per;
}
}
/****************************************************************************
* Name: angle_norm_2pi
*
* Description:
* Normalize radians angle with period 2*PI to a given boundary.
*
* Input Parameters:
* angle - (in/out) pointer to the angle data
* bottom - (in) lower limit
* top - (in) upper limit
*
* Returned Value:
* None
*
****************************************************************************/
void angle_norm_2pi(FAR float *angle, float bottom, float top)
{
angle_norm(angle, 2.0f*M_PI_F, bottom, top);
}
/****************************************************************************
* Name: phase_angle_update
*
* Description:
* Update phase_angle_s structure:
* 1. normalize angle value to <0.0, 2PI> range
* 2. update angle value
* 3. update sin/cos value for given angle
*
* Input Parameters:
* angle - (in/out) pointer to the angle data
* val - (in) angle radian value
*
* Returned Value:
* None
*
****************************************************************************/
void phase_angle_update(FAR struct phase_angle_s *angle, float val)
{
DEBUGASSERT(angle != NULL);
/* Normalize angle to <0.0, 2PI> */
angle_norm_2pi(&val, 0.0f, 2.0f*M_PI_F);
/* Update structure */
angle->angle = val;
#if CONFIG_LIBDSP_PRECISION == 1
angle->sin = fast_sin2(val);
angle->cos = fast_cos2(val);
#elif CONFIG_LIBDSP_PRECISION == 2
angle->sin = sin(val);
angle->cos = cos(val);
#else
angle->sin = fast_sin(val);
angle->cos = fast_cos(val);
#endif
}