sergiotarxz/nuttx-apps
sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

industry/foc: add support for CORDIC

Browse Source
This commit is contained in:
raiden00pl 2021-07-28 08:49:35 +02:00 committed by Alan Carvalho de Assis
parent a1026c9f23
commit a0fbd104fa
9 changed files with 282 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/industry/foc/fixed16/foc_cordic.h
View File

@ -33,16 +33,20 @@
* Public Function Prototypes
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
/****************************************************************************
* Name: foc_cordic_dqsat_b16
****************************************************************************/
int foc_cordic_dqsat_b16(int fd, FAR dq_frame_b16_t *dq_ref, b16_t mag_max);
#endif
#ifdef CONFIG_INDUSTRY_FOC_CORDIC_ANGLE
/****************************************************************************
* Name: foc_cordic_angle_b16
****************************************************************************/
int foc_cordic_angle_b16(int fd, FAR phase_angle_b16_t *angle, b16_t a);
#endif
#endif /* __INDUSTRY_FOC_FIXED16_FOC_CORDIC_H */

4
include/industry/foc/float/foc_cordic.h
View File

@ -33,16 +33,20 @@
* Public Function Prototypes
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
/****************************************************************************
* Name: foc_cordic_dqsat_f32
****************************************************************************/
int foc_cordic_dqsat_f32(int fd, FAR dq_frame_f32_t *dq_ref, float mag_max);
#endif
#ifdef CONFIG_INDUSTRY_FOC_CORDIC_ANGLE
/****************************************************************************
* Name: foc_cordic_angle_f32
****************************************************************************/
int foc_cordic_angle_f32(int fd, FAR phase_angle_f32_t *angle, float a);
#endif
#endif /* __INDUSTRY_FOC_FLOAT_FOC_CORDIC_H */

17
industry/foc/Kconfig
View File

@ -14,11 +14,26 @@ if INDUSTRY_FOC
config INDUSTRY_FOC_CORDIC
bool "Enable CORDIC support"
depends on EXPERIMENTAL
default n
---help---
Enable support for CORDIC hardware acceleration
if INDUSTRY_FOC_CORDIC
config INDUSTRY_FOC_CORDIC_DEVPATH
string "CORDIC device path"
default "/dev/cordic0"
config INDUSTRY_FOC_CORDIC_ANGLE
bool "Enable CORDIC for phase angle"
default n
config INDUSTRY_FOC_CORDIC_DQSAT
bool "Enable CORDIC for dq saturation"
default n
endif # INDUSTRY_FOC_CORDIC
config INDUSTRY_FOC_FIXED16
bool "Enable support for fixed16"
default n

112
industry/foc/fixed16/foc_cordic.c
View File

@ -24,14 +24,25 @@
#include <nuttx/config.h>
#include <assert.h>
#include <sys/ioctl.h>
#include <assert.h>
#include <fcntl.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/math/cordic.h>
#include <nuttx/math/math_ioctl.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/fixed16/foc_cordic.h"
/****************************************************************************
* Public Functions
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
/****************************************************************************
* Name: foc_cordic_dqsat_b16
*
@ -47,14 +58,70 @@
int foc_cordic_dqsat_b16(int fd, FAR dq_frame_b16_t *dq, b16_t mag_max)
{
struct cordic_calc_s io;
b16_t dnorm = 0;
b16_t qnorm = 0;
b16_t dqscale = 0;
b16_t mag = 0;
b16_t tmp = 0;
int ret = OK;
DEBUGASSERT(dq);
/* TODO: */
/* Normalize DQ to [-1, 1] */
ASSERT(0);
if (dq->d > dq->q)
{
dqscale = b16divb16(b16ONE, dq->d);
}
else
{
dqscale = b16divb16(b16ONE, dq->q);
}
dnorm = b16mulb16(dq->d, dqscale);
qnorm = b16mulb16(dq->d, dqscale);
/* Get modulus */
io.func = CORDIC_CALC_FUNC_MOD;
io.res2_incl = false;
io.arg1 = b16toq31(dnorm);
io.arg2 = b16toq31(qnorm);
io.res1 = 0;
io.res2 = 0;
ret = ioctl(fd, MATHIOC_CORDIC_CALC, (unsigned long)((uintptr_t)&io));
if (ret < 0)
{
FOCLIBERR("ERROR: MATHIOC_CORDIC_CALC failed, errno=%d\n", errno);
}
/* Get real magnitude */
mag = b16mulb16(q31tof(io.res1), dqscale);
/* Magnitude bottom limit */
if (mag < 1)
{
mag = 1;
}
if (mag > mag_max)
{
/* Saturate vector */
tmp = b16divb16(mag_max, mag);
dq->d = b16mulb16(dq->d, tmp);
dq->q = b16mulb16(dq->q, tmp);
}
return OK;
}
#endif /* CONFIG_INDUSTRY_FOC_CORDIC_DQSAT */
#ifdef CONFIG_INDUSTRY_FOC_CORDIC_ANGLE
/****************************************************************************
* Name: foc_cordic_angle_b16
@ -71,11 +138,46 @@ int foc_cordic_dqsat_b16(int fd, FAR dq_frame_b16_t *dq, b16_t mag_max)
int foc_cordic_angle_b16(int fd, FAR phase_angle_b16_t *angle, b16_t a)
{
const b16_t onebypi = b16divb16(b16ONE, b16PI);
struct cordic_calc_s io;
b16_t anorm = 0;
int ret = OK;
DEBUGASSERT(angle);
/* TODO: */
/* Copy angle */
ASSERT(0);
anorm = a;
/* Normalize angle to [-PI, PI] */
angle_norm_2pi_b16(&anorm, -b16PI, b16PI);
/* Normalize angle to [-1, 1] */
anorm = b16mulb16(anorm, onebypi);
/* Get cosine and sine from single call */
io.func = CORDIC_CALC_FUNC_COS;
io.res2_incl = true;
io.arg1 = b16toq31(anorm);
io.arg2 = b16toq31(b16ONE);
io.res1 = 0;
io.res2 = 0;
ret = ioctl(fd, MATHIOC_CORDIC_CALC, (unsigned long)((uintptr_t)&io));
if (ret < 0)
{
FOCLIBERR("ERROR: MATHIOC_CORDIC_CALC failed, errno=%d\n", errno);
}
/* Fill phase angle struct */
angle->angle = a;
angle->cos = q31tob16(io.res1);
angle->sin = q31tob16(io.res2);
return OK;
}
#endif /* CONFIG_INDUSTRY_FOC_CORDIC_ANGLE */

19
industry/foc/fixed16/foc_handler.c
View File

@ -26,6 +26,7 @@
#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/foc_common.h"
@ -104,6 +105,18 @@ int foc_handler_init_b16(FAR foc_handler_b16_t *h,
goto errout;
}
#ifdef CONFIG_INDUSTRY_FOC_CORDIC
/* Open CORDIC device */
h->fd = open(CONFIG_INDUSTRY_FOC_CORDIC_DEVPATH, O_RDWR);
if (h->fd < 0)
{
FOCLIBERR("ERROR: failed to open CORDIC device %s %d\n",
CONFIG_INDUSTRY_FOC_CORDIC_DEVPATH, errno);
goto errout;
}
#endif
errout:
return ret;
}
@ -133,6 +146,12 @@ int foc_handler_deinit_b16(FAR foc_handler_b16_t *h)
h->ops.ctrl->deinit(h);
#ifdef CONFIG_INDUSTRY_FOC_CORDIC
/* Close CORDIC device */
close(h->fd);
#endif
/* Reset data */
memset(h, 0, sizeof(foc_handler_b16_t));

6
industry/foc/fixed16/foc_picontrol.c
View File

@ -235,7 +235,7 @@ static void foc_control_input_set_b16(FAR foc_handler_b16_t *h,
/* Update phase angle */
#ifndef CONFIG_INDUSTRY_FOC_CORDIC
#ifndef CONFIG_INDUSTRY_FOC_CORDIC_ANGLE
phase_angle_update_b16(&foc->angle, angle);
#else
foc_cordic_angle_b16(h->fd, &foc->angle, angle);
@ -281,7 +281,7 @@ static void foc_control_voltage_run_b16(FAR foc_handler_b16_t *h,
/* Saturate voltage DQ vector */
#ifndef CONFIG_INDUSTRY_FOC_CORDIC
#ifndef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
dq_saturate_b16(dq_ref, mag_max);
#else
foc_cordic_dqsat_b16(h->fd, dq_ref, mag_max);
@ -343,7 +343,7 @@ static void foc_control_current_run_b16(FAR foc_handler_b16_t *h,
/* Saturate voltage DQ vector */
#ifndef CONFIG_INDUSTRY_FOC_CORDIC
#ifndef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
dq_saturate_b16(dq_ref, mag_max);
#else
foc_cordic_dqsat_b16(h->fd, dq_ref, mag_max);

112
industry/foc/float/foc_cordic.c
View File

@ -24,14 +24,25 @@
#include <nuttx/config.h>
#include <assert.h>
#include <sys/ioctl.h>
#include <assert.h>
#include <fcntl.h>
#include <debug.h>
#include <errno.h>
#include <nuttx/math/cordic.h>
#include <nuttx/math/math_ioctl.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/float/foc_cordic.h"
/****************************************************************************
* Public Functions
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
/****************************************************************************
* Name: foc_cordic_dqsat_f32
*
@ -47,14 +58,70 @@
int foc_cordic_dqsat_f32(int fd, FAR dq_frame_f32_t *dq, float mag_max)
{
struct cordic_calc_s io;
float dnorm = 0.0f;
float qnorm = 0.0f;
float dqscale = 0.0f;
float mag = 0.0f;
float tmp = 0.0f;
int ret = OK;
DEBUGASSERT(dq);
/* TODO: */
/* Normalize DQ to [-1, 1] */
ASSERT(0);
if (dq->d > dq->q)
{
dqscale = (1.0f / dq->d);
}
else
{
dqscale = (1.0f / dq->q);
}
dnorm = dq->d * dqscale;
qnorm = dq->d * dqscale;
/* Get modulus */
io.func = CORDIC_CALC_FUNC_MOD;
io.res2_incl = false;
io.arg1 = ftoq31(dnorm);
io.arg2 = ftoq31(qnorm);
io.res1 = 0;
io.res2 = 0;
ret = ioctl(fd, MATHIOC_CORDIC_CALC, (unsigned long)((uintptr_t)&io));
if (ret < 0)
{
FOCLIBERR("ERROR: MATHIOC_CORDIC_CALC failed, errno=%d\n", errno);
}
/* Get real magnitude */
mag = q31tof(io.res1) * dqscale;
/* Magnitude bottom limit */
if (mag < 1e-10f)
{
mag = 1e-10f;
}
if (mag > mag_max)
{
/* Saturate vector */
tmp = mag_max / mag;
dq->d *= tmp;
dq->q *= tmp;
}
return OK;
}
#endif /* CONFIG_INDUSTRY_FOC_CORDIC_DQSAT */
#ifdef CONFIG_INDUSTRY_FOC_CORDIC_ANGLE
/****************************************************************************
* Name: foc_cordic_angle_f32
@ -71,11 +138,46 @@ int foc_cordic_dqsat_f32(int fd, FAR dq_frame_f32_t *dq, float mag_max)
int foc_cordic_angle_f32(int fd, FAR phase_angle_f32_t *angle, float a)
{
const float onebypi = (1.0f / M_PI_F);
struct cordic_calc_s io;
float anorm = 0.0f;
int ret = OK;
DEBUGASSERT(angle);
/* TODO: */
/* Copy angle */
ASSERT(0);
anorm = a;
/* Normalize angle to [-PI, PI] */
angle_norm_2pi(&anorm, -M_PI_F, M_PI_F);
/* Normalize angle to [-1, 1] */
anorm = anorm * onebypi;
/* Get cosine and sine from single call */
io.func = CORDIC_CALC_FUNC_COS;
io.res2_incl = true;
io.arg1 = ftoq31(anorm);
io.arg2 = ftoq31(1.0f);
io.res1 = 0;
io.res2 = 0;
ret = ioctl(fd, MATHIOC_CORDIC_CALC, (unsigned long)((uintptr_t)&io));
if (ret < 0)
{
FOCLIBERR("ERROR: MATHIOC_CORDIC_CALC failed, errno=%d\n", errno);
}
/* Fill phase angle struct */
angle->angle = a;
angle->cos = q31tof(io.res1);
angle->sin = q31tof(io.res2);
return OK;
}
#endif /* CONFIG_INDUSTRY_FOC_CORDIC_ANGLE */

19
industry/foc/float/foc_handler.c
View File

@ -26,6 +26,7 @@
#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/foc_common.h"
@ -104,6 +105,18 @@ int foc_handler_init_f32(FAR foc_handler_f32_t *h,
goto errout;
}
#ifdef CONFIG_INDUSTRY_FOC_CORDIC
/* Open CORDIC device */
h->fd = open(CONFIG_INDUSTRY_FOC_CORDIC_DEVPATH, O_RDWR);
if (h->fd < 0)
{
FOCLIBERR("ERROR: failed to open CORDIC device %s %d\n",
CONFIG_INDUSTRY_FOC_CORDIC_DEVPATH, errno);
goto errout;
}
#endif
errout:
return ret;
}
@ -133,6 +146,12 @@ int foc_handler_deinit_f32(FAR foc_handler_f32_t *h)
h->ops.ctrl->deinit(h);
#ifdef CONFIG_INDUSTRY_FOC_CORDIC
/* Close CORDIC device */
close(h->fd);
#endif
/* Reset data */
memset(h, 0, sizeof(foc_handler_f32_t));

6
industry/foc/float/foc_picontrol.c
View File

@ -235,7 +235,7 @@ static void foc_control_input_set_f32(FAR foc_handler_f32_t *h,
/* Update phase angle */
#ifndef CONFIG_INDUSTRY_FOC_CORDIC
#ifndef CONFIG_INDUSTRY_FOC_CORDIC_ANGLE
phase_angle_update(&foc->angle, angle);
#else
foc_cordic_angle_f32(h->fd, &foc->angle, angle);
@ -281,7 +281,7 @@ static void foc_control_voltage_run_f32(FAR foc_handler_f32_t *h,
/* Saturate voltage DQ vector */
#ifndef CONFIG_INDUSTRY_FOC_CORDIC
#ifndef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
dq_saturate(dq_ref, mag_max);
#else
foc_cordic_dqsat_f32(h->fd, dq_ref, mag_max);
@ -344,7 +344,7 @@ static void foc_control_current_run_f32(FAR foc_handler_f32_t *h,
/* Saturate voltage DQ vector */
#ifndef CONFIG_INDUSTRY_FOC_CORDIC
#ifndef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
dq_saturate(dq_ref, mag_max);
#else
foc_cordic_dqsat_f32(h->fd, dq_ref, mag_max);