industry/foc: add support for Hall sensor
This commit is contained in:
raiden00pl
Xiang Xiao
parent
5b52247625
commit
4001d27571
@ -116,6 +116,16 @@ struct foc_qenco_cfg_b16_s
|
||||
};
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_HALL
|
||||
/* Hall configuration data */
|
||||
|
||||
struct foc_hall_cfg_b16_s
|
||||
{
|
||||
FAR char *devpath;
|
||||
b16_t per;
|
||||
};
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
@ -132,6 +142,12 @@ extern struct foc_angle_ops_b16_s g_foc_angle_ol_b16;
|
||||
extern struct foc_angle_ops_b16_s g_foc_angle_qe_b16;
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_HALL
|
||||
/* Hall angle operations (fixed16) */
|
||||
|
||||
extern struct foc_angle_ops_b16_s g_foc_angle_hl_b16;
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
@ -116,6 +116,16 @@ struct foc_qenco_cfg_f32_s
|
||||
};
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_HALL
|
||||
/* Hall configuration data */
|
||||
|
||||
struct foc_hall_cfg_f32_s
|
||||
{
|
||||
FAR char *devpath;
|
||||
float per;
|
||||
};
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
@ -132,6 +142,12 @@ extern struct foc_angle_ops_f32_s g_foc_angle_ol_f32;
|
||||
extern struct foc_angle_ops_f32_s g_foc_angle_qe_f32;
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_HALL
|
||||
/* Hall angle operations (float) */
|
||||
|
||||
extern struct foc_angle_ops_f32_s g_foc_angle_hl_f32;
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
@ -65,6 +65,33 @@ config INDUSTRY_FOC_ANGLE_QENCO
|
||||
---help---
|
||||
Enable support for quadrature encoder angle handler
|
||||
|
||||
config INDUSTRY_FOC_ANGLE_HALL
|
||||
bool "FOC angle 3-phase hall handler"
|
||||
default n
|
||||
depends on SENSORS_HALL3PHASE
|
||||
---help---
|
||||
Enable support for 3-phase hall sensor angle handler
|
||||
|
||||
if INDUSTRY_FOC_ANGLE_HALL
|
||||
|
||||
choice
|
||||
prompt "FOC Hall sensor type"
|
||||
default INDUSTRY_FOC_ANGLE_HALL_120DEG
|
||||
|
||||
config INDUSTRY_FOC_ANGLE_HALL_120DEG
|
||||
bool "Hall sensor 120 deg"
|
||||
|
||||
config INDUSTRY_FOC_ANGLE_HALL_60DEG
|
||||
bool "Hall sensor 60 deg (not tested)"
|
||||
|
||||
endchoice # FOC Hall sensor type
|
||||
|
||||
config INDUSTRY_FOC_ANGLE_HALL_EST
|
||||
bool "FOC Hall angle estimation"
|
||||
default n
|
||||
|
||||
endif # INDUSTRY_FOC_ANGLE_HALL
|
||||
|
||||
config INDUSTRY_FOC_CONTROL_PI
|
||||
bool "FOC PI controller"
|
||||
default y
|
||||
|
||||
@ -39,6 +39,9 @@ endif
|
||||
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_QENCO),y)
|
||||
CSRCS += float/foc_ang_qenco.c
|
||||
endif
|
||||
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_HALL),y)
|
||||
CSRCS += float/foc_ang_hall.c
|
||||
endif
|
||||
ifeq ($(CONFIG_INDUSTRY_FOC_CORDIC),y)
|
||||
CSRCS += float/foc_cordic.c
|
||||
endif
|
||||
@ -78,6 +81,9 @@ endif
|
||||
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_QENCO),y)
|
||||
CSRCS += fixed16/foc_ang_qenco.c
|
||||
endif
|
||||
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_HALL),y)
|
||||
CSRCS += fixed16/foc_ang_hall.c
|
||||
endif
|
||||
ifeq ($(CONFIG_INDUSTRY_FOC_CORDIC),y)
|
||||
CSRCS += fixed16/foc_cordic.c
|
||||
endif
|
||||
|
||||
592
industry/foc/fixed16/foc_ang_hall.c
Normal file
592
industry/foc/fixed16/foc_ang_hall.c
Normal file
@ -0,0 +1,592 @@
|
||||
/****************************************************************************
|
||||
* apps/industry/foc/fixed16/foc_ang_hall.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 <nuttx/config.h>
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <nuttx/sensors/hall3ph.h>
|
||||
|
||||
#include "industry/foc/foc_log.h"
|
||||
#include "industry/foc/fixed16/foc_angle.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#define HALL_MAX (6)
|
||||
#define ONE_BY_HALL_MAX (b16idiv(1, HALL_MAX))
|
||||
#define HALL_ANGLE_STEP (b16mulb16(ONE_BY_HALL_MAX, MOTOR_ANGLE_E_MAX_B16))
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data Types
|
||||
****************************************************************************/
|
||||
|
||||
/* Hall private data */
|
||||
|
||||
struct foc_hall_b16_s
|
||||
{
|
||||
int fd;
|
||||
int8_t sector;
|
||||
int8_t offset;
|
||||
b16_t sensor_dir;
|
||||
b16_t angle;
|
||||
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_HALL_EST
|
||||
int8_t sector_last;
|
||||
int8_t sector_diff;
|
||||
int8_t sector_diff_last;
|
||||
b16_t per_acc;
|
||||
b16_t vel_est;
|
||||
b16_t angle_diff;
|
||||
#endif
|
||||
struct foc_hall_cfg_b16_s cfg;
|
||||
};
|
||||
|
||||
/****************************************************************************
|
||||
* Private Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
static int foc_angle_hl_init_b16(FAR foc_angle_b16_t *h);
|
||||
static void foc_angle_hl_deinit_b16(FAR foc_angle_b16_t *h);
|
||||
static int foc_angle_hl_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg);
|
||||
static int foc_angle_hl_zero_b16(FAR foc_angle_b16_t *h);
|
||||
static int foc_angle_hl_dir_b16(FAR foc_angle_b16_t *h, b16_t dir);
|
||||
static int foc_angle_hl_run_b16(FAR foc_angle_b16_t *h,
|
||||
FAR struct foc_angle_in_b16_s *in,
|
||||
FAR struct foc_angle_out_b16_s *out);
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/* FOC angle fixed16 interface */
|
||||
|
||||
struct foc_angle_ops_b16_s g_foc_angle_hl_b16 =
|
||||
{
|
||||
.init = foc_angle_hl_init_b16,
|
||||
.deinit = foc_angle_hl_deinit_b16,
|
||||
.cfg = foc_angle_hl_cfg_b16,
|
||||
.zero = foc_angle_hl_zero_b16,
|
||||
.dir = foc_angle_hl_dir_b16,
|
||||
.run = foc_angle_hl_run_b16,
|
||||
};
|
||||
|
||||
/* Sector angles */
|
||||
|
||||
static b16_t g_sector_angle[7] =
|
||||
{
|
||||
b16muli(HALL_ANGLE_STEP, 0),
|
||||
b16muli(HALL_ANGLE_STEP, 1),
|
||||
b16muli(HALL_ANGLE_STEP, 2),
|
||||
b16muli(HALL_ANGLE_STEP, 3),
|
||||
b16muli(HALL_ANGLE_STEP, 4),
|
||||
b16muli(HALL_ANGLE_STEP, 5),
|
||||
b16muli(HALL_ANGLE_STEP, 6)
|
||||
};
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_decode
|
||||
****************************************************************************/
|
||||
|
||||
static int8_t foc_angle_hl_decode_sector(uint8_t hall)
|
||||
{
|
||||
int8_t sector = -1;
|
||||
|
||||
#if defined(CONFIG_INDUSTRY_FOC_ANGLE_HALL_120DEG)
|
||||
switch (hall)
|
||||
{
|
||||
case HALL3_120DEG_POS_1:
|
||||
{
|
||||
sector = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_120DEG_POS_2:
|
||||
{
|
||||
sector = 1;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_120DEG_POS_3:
|
||||
{
|
||||
sector = 2;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_120DEG_POS_4:
|
||||
{
|
||||
sector = 3;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_120DEG_POS_5:
|
||||
{
|
||||
sector = 4;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_120DEG_POS_6:
|
||||
{
|
||||
sector = 5;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
sector = -1;
|
||||
}
|
||||
}
|
||||
#elif defined(CONFIG_INDUSTRY_FOC_ANGLE_HALL_60DEG)
|
||||
switch (hall)
|
||||
{
|
||||
case HALL3_60DEG_POS_1:
|
||||
{
|
||||
sector = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_60DEG_POS_2:
|
||||
{
|
||||
sector = 1;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_60DEG_POS_3:
|
||||
{
|
||||
sector = 2;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_60DEG_POS_4:
|
||||
{
|
||||
sector = 3;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_60DEG_POS_5:
|
||||
{
|
||||
sector = 4;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_60DEG_POS_6:
|
||||
{
|
||||
sector = 5;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
sector = -1;
|
||||
}
|
||||
}
|
||||
#else
|
||||
# error Invalid configuration
|
||||
#endif
|
||||
|
||||
return sector;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_sector_get()
|
||||
****************************************************************************/
|
||||
|
||||
static uint8_t foc_angle_hl_sector_get(FAR foc_angle_b16_t *h)
|
||||
{
|
||||
FAR struct foc_hall_b16_s *hl = NULL;
|
||||
int ret = OK;
|
||||
int8_t sector = -1;
|
||||
uint8_t hall = 0;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Get hall data */
|
||||
|
||||
DEBUGASSERT(h->data);
|
||||
hl = h->data;
|
||||
|
||||
/* Get the positions from hall */
|
||||
|
||||
ret = ioctl(hl->fd, SNIOC_GET_POSITION, (unsigned long)((uintptr_t)&hall));
|
||||
if (ret < 0)
|
||||
{
|
||||
FOCLIBERR("ERROR: H3PH_POSITION failed, errno=%d\n", errno);
|
||||
goto errout;
|
||||
}
|
||||
|
||||
/* Decode hall sector */
|
||||
|
||||
sector = foc_angle_hl_decode_sector(hall);
|
||||
if (sector == -1)
|
||||
{
|
||||
goto errout;
|
||||
}
|
||||
|
||||
errout:
|
||||
return sector;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_init_b16
|
||||
*
|
||||
* Description:
|
||||
* Initialize hall the FOC angle handler (fixed16)
|
||||
*
|
||||
* Input Parameter:
|
||||
* h - pointer to FOC angle handler
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static int foc_angle_hl_init_b16(FAR foc_angle_b16_t *h)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Connect angle data */
|
||||
|
||||
h->data = zalloc(sizeof(struct foc_hall_b16_s));
|
||||
if (h->data == NULL)
|
||||
{
|
||||
ret = -ENOMEM;
|
||||
goto errout;
|
||||
}
|
||||
|
||||
errout:
|
||||
return ret;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_deinit_b16
|
||||
*
|
||||
* Description:
|
||||
* De-initialize hall the FOC angle handler (fixed16)
|
||||
*
|
||||
* Input Parameter:
|
||||
* h - pointer to FOC angle handler
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static void foc_angle_hl_deinit_b16(FAR foc_angle_b16_t *h)
|
||||
{
|
||||
FAR struct foc_hall_b16_s *hl = NULL;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Get hall data */
|
||||
|
||||
DEBUGASSERT(h->data);
|
||||
hl = h->data;
|
||||
|
||||
if (h->data)
|
||||
{
|
||||
/* Close file */
|
||||
|
||||
if (hl->fd > 0)
|
||||
{
|
||||
close(hl->fd);
|
||||
}
|
||||
|
||||
/* Free angle data */
|
||||
|
||||
free(h->data);
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_cfg_b16
|
||||
*
|
||||
* Description:
|
||||
* Configure the hall FOC angle handler (fixed16)
|
||||
*
|
||||
* Input Parameter:
|
||||
* h - pointer to FOC angle handler
|
||||
* cfg - pointer to angle handler configuration data
|
||||
* (struct foc_hall_b16_s)
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static int foc_angle_hl_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg)
|
||||
{
|
||||
FAR struct foc_hall_b16_s *hl = NULL;
|
||||
int ret = OK;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Get hall data */
|
||||
|
||||
DEBUGASSERT(h->data);
|
||||
hl = h->data;
|
||||
|
||||
/* Copy configuration */
|
||||
|
||||
memcpy(&hl->cfg, cfg, sizeof(struct foc_hall_cfg_b16_s));
|
||||
|
||||
/* Open hall device */
|
||||
|
||||
hl->fd = open(hl->cfg.devpath, O_RDONLY);
|
||||
if (hl->fd <= 0)
|
||||
{
|
||||
FOCLIBERR("ERROR: failed to open %s, errno=%d\n",
|
||||
hl->cfg.devpath, errno);
|
||||
ret = -errno;
|
||||
goto errout;
|
||||
}
|
||||
|
||||
/* Initialize with CW direction */
|
||||
|
||||
hl->sensor_dir = DIR_CW_B16;
|
||||
|
||||
errout:
|
||||
return ret;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_zero_b16
|
||||
*
|
||||
* Description:
|
||||
* Zero the hl FOC angle handler (fixed16)
|
||||
*
|
||||
* Input Parameter:
|
||||
* h - pointer to FOC angle handler
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static int foc_angle_hl_zero_b16(FAR foc_angle_b16_t *h)
|
||||
{
|
||||
FAR struct foc_hall_b16_s *hl = NULL;
|
||||
int ret = OK;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Get hall data */
|
||||
|
||||
DEBUGASSERT(h->data);
|
||||
hl = h->data;
|
||||
|
||||
/* Get hall offset */
|
||||
|
||||
hl->offset = foc_angle_hl_sector_get(h);
|
||||
if (hl->offset == -1)
|
||||
{
|
||||
ret = -EINVAL;
|
||||
goto errout;
|
||||
}
|
||||
|
||||
/* Reset data */
|
||||
|
||||
hl->angle = 0;
|
||||
hl->sector = 0;
|
||||
|
||||
errout:
|
||||
return ret;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_dir_b16
|
||||
*
|
||||
* Description:
|
||||
* Set the hl FOC angle handler direction (fixed16)
|
||||
*
|
||||
* Input Parameter:
|
||||
* h - pointer to FOC angle handler
|
||||
* dir - sensor direction (1 if normal -1 if inverted)
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static int foc_angle_hl_dir_b16(FAR foc_angle_b16_t *h, b16_t dir)
|
||||
{
|
||||
FAR struct foc_hall_b16_s *hl = NULL;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Get hall data */
|
||||
|
||||
DEBUGASSERT(h->data);
|
||||
hl = h->data;
|
||||
|
||||
/* Configure direction */
|
||||
|
||||
hl->sensor_dir = dir;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_run_b16
|
||||
*
|
||||
* Description:
|
||||
* Process the hall FOC angle data (fixed16)
|
||||
*
|
||||
* Input Parameter:
|
||||
* h - pointer to FOC angle handler
|
||||
* in - pointer to FOC angle handler input data
|
||||
* out - pointer to FOC angle handler output data
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static int foc_angle_hl_run_b16(FAR foc_angle_b16_t *h,
|
||||
FAR struct foc_angle_in_b16_s *in,
|
||||
FAR struct foc_angle_out_b16_s *out)
|
||||
{
|
||||
FAR struct foc_hall_b16_s *hl = NULL;
|
||||
int ret = OK;
|
||||
int8_t sector = -1;
|
||||
b16_t tmp1 = 0;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Get hall data */
|
||||
|
||||
DEBUGASSERT(h->data);
|
||||
hl = h->data;
|
||||
|
||||
/* Get hall sector now */
|
||||
|
||||
sector = foc_angle_hl_sector_get(h);
|
||||
if (sector == -1)
|
||||
{
|
||||
ret = -EINVAL;
|
||||
goto errout;
|
||||
}
|
||||
|
||||
/* Correct sector with offset */
|
||||
|
||||
sector = sector - hl->offset;
|
||||
if (sector < 0)
|
||||
{
|
||||
sector = sector + HALL_MAX;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_HALL_EST
|
||||
/* Store previous state */
|
||||
|
||||
hl->sector_last = hl->sector;
|
||||
#endif
|
||||
|
||||
/* Store current sector */
|
||||
|
||||
hl->sector = sector;
|
||||
|
||||
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_HALL_EST
|
||||
/* Sector diff */
|
||||
|
||||
hl->sector_diff = hl->sector - hl->sector_last;
|
||||
|
||||
/* Handle next sector or estimate angle between sectors */
|
||||
|
||||
if (hl->sector_diff != 0)
|
||||
{
|
||||
/* Only if per_acc is valid */
|
||||
|
||||
if (hl->per_acc > 0)
|
||||
{
|
||||
/* Do not update vel_est on boundaries */
|
||||
|
||||
if (hl->sector * hl->sector_last != 0)
|
||||
{
|
||||
/* Only if velocity direction not changed */
|
||||
|
||||
if (hl->sector_diff * hl->sector_diff_last > 0)
|
||||
{
|
||||
tmp1 = b16muli(hl->sector_diff, HALL_ANGLE_STEP);
|
||||
|
||||
/* Next sector - estimate velocity */
|
||||
|
||||
hl->vel_est = b16divb16(tmp1, hl->per_acc);
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Velocity dir changed */
|
||||
|
||||
hl->vel_est = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* Reser accumulators */
|
||||
|
||||
hl->per_acc = 0;
|
||||
hl->angle_diff = 0;
|
||||
}
|
||||
|
||||
/* Store last sector diff */
|
||||
|
||||
hl->sector_diff_last = hl->sector_diff;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Accumulate period for velocity estimation */
|
||||
|
||||
hl->per_acc += hl->cfg.per;
|
||||
|
||||
/* Accumulate angle diff */
|
||||
|
||||
hl->angle_diff += b16mulb16(hl->vel_est, hl->cfg.per);
|
||||
|
||||
/* Saturate angle diff */
|
||||
|
||||
if (hl->angle_diff > HALL_ANGLE_STEP)
|
||||
{
|
||||
hl->angle_diff = HALL_ANGLE_STEP;
|
||||
}
|
||||
else if (hl->angle_diff < -HALL_ANGLE_STEP)
|
||||
{
|
||||
hl->angle_diff = -HALL_ANGLE_STEP;
|
||||
}
|
||||
}
|
||||
|
||||
/* Get corrected electrical angle */
|
||||
|
||||
hl->angle = b16mulb16(hl->sensor_dir,
|
||||
g_sector_angle[hl->sector]) + hl->angle_diff;
|
||||
|
||||
#else
|
||||
/* Get electrical angle */
|
||||
|
||||
hl->angle = b16mulb16(hl->sensor_dir, g_sector_angle[hl->sector]);
|
||||
|
||||
#endif /* CONFIG_INDUSTRY_FOC_ANGLE_HALL_EST */
|
||||
|
||||
/* Normalize angle */
|
||||
|
||||
angle_norm_2pi_b16(&hl->angle, MOTOR_ANGLE_E_MIN_B16,
|
||||
MOTOR_ANGLE_E_MAX_B16);
|
||||
|
||||
/* Copy data */
|
||||
|
||||
out->type = FOC_ANGLE_TYPE_ELE;
|
||||
out->angle = hl->angle;
|
||||
|
||||
errout:
|
||||
return ret;
|
||||
}
|
||||
588
industry/foc/float/foc_ang_hall.c
Normal file
588
industry/foc/float/foc_ang_hall.c
Normal file
@ -0,0 +1,588 @@
|
||||
/****************************************************************************
|
||||
* apps/industry/foc/float/foc_ang_hall.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 <nuttx/config.h>
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <nuttx/sensors/hall3ph.h>
|
||||
|
||||
#include "industry/foc/foc_log.h"
|
||||
#include "industry/foc/float/foc_angle.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#define HALL_MAX (6)
|
||||
#define ONE_BY_HALL_MAX (1.0f / HALL_MAX)
|
||||
#define HALL_ANGLE_STEP (ONE_BY_HALL_MAX * MOTOR_ANGLE_E_MAX)
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data Types
|
||||
****************************************************************************/
|
||||
|
||||
/* Hall private data */
|
||||
|
||||
struct foc_hall_f32_s
|
||||
{
|
||||
int fd;
|
||||
int8_t sector;
|
||||
int8_t offset;
|
||||
float sensor_dir;
|
||||
float angle;
|
||||
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_HALL_EST
|
||||
int8_t sector_last;
|
||||
int8_t sector_diff;
|
||||
int8_t sector_diff_last;
|
||||
float per_acc;
|
||||
float vel_est;
|
||||
float angle_diff;
|
||||
#endif
|
||||
struct foc_hall_cfg_f32_s cfg;
|
||||
};
|
||||
|
||||
/****************************************************************************
|
||||
* Private Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
static int foc_angle_hl_init_f32(FAR foc_angle_f32_t *h);
|
||||
static void foc_angle_hl_deinit_f32(FAR foc_angle_f32_t *h);
|
||||
static int foc_angle_hl_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg);
|
||||
static int foc_angle_hl_zero_f32(FAR foc_angle_f32_t *h);
|
||||
static int foc_angle_hl_dir_f32(FAR foc_angle_f32_t *h, float dir);
|
||||
static int foc_angle_hl_run_f32(FAR foc_angle_f32_t *h,
|
||||
FAR struct foc_angle_in_f32_s *in,
|
||||
FAR struct foc_angle_out_f32_s *out);
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/* FOC angle float interface */
|
||||
|
||||
struct foc_angle_ops_f32_s g_foc_angle_hl_f32 =
|
||||
{
|
||||
.init = foc_angle_hl_init_f32,
|
||||
.deinit = foc_angle_hl_deinit_f32,
|
||||
.cfg = foc_angle_hl_cfg_f32,
|
||||
.zero = foc_angle_hl_zero_f32,
|
||||
.dir = foc_angle_hl_dir_f32,
|
||||
.run = foc_angle_hl_run_f32,
|
||||
};
|
||||
|
||||
/* Sector angles */
|
||||
|
||||
static float g_sector_angle[7] =
|
||||
{
|
||||
0 * HALL_ANGLE_STEP,
|
||||
1 * HALL_ANGLE_STEP,
|
||||
2 * HALL_ANGLE_STEP,
|
||||
3 * HALL_ANGLE_STEP,
|
||||
4 * HALL_ANGLE_STEP,
|
||||
5 * HALL_ANGLE_STEP,
|
||||
6 * HALL_ANGLE_STEP
|
||||
};
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_decode
|
||||
****************************************************************************/
|
||||
|
||||
static int8_t foc_angle_hl_decode_sector(uint8_t hall)
|
||||
{
|
||||
int8_t sector = -1;
|
||||
|
||||
#if defined(CONFIG_INDUSTRY_FOC_ANGLE_HALL_120DEG)
|
||||
switch (hall)
|
||||
{
|
||||
case HALL3_120DEG_POS_1:
|
||||
{
|
||||
sector = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_120DEG_POS_2:
|
||||
{
|
||||
sector = 1;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_120DEG_POS_3:
|
||||
{
|
||||
sector = 2;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_120DEG_POS_4:
|
||||
{
|
||||
sector = 3;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_120DEG_POS_5:
|
||||
{
|
||||
sector = 4;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_120DEG_POS_6:
|
||||
{
|
||||
sector = 5;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
sector = -1;
|
||||
}
|
||||
}
|
||||
#elif defined(CONFIG_INDUSTRY_FOC_ANGLE_HALL_60DEG)
|
||||
switch (hall)
|
||||
{
|
||||
case HALL3_60DEG_POS_1:
|
||||
{
|
||||
sector = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_60DEG_POS_2:
|
||||
{
|
||||
sector = 1;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_60DEG_POS_3:
|
||||
{
|
||||
sector = 2;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_60DEG_POS_4:
|
||||
{
|
||||
sector = 3;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_60DEG_POS_5:
|
||||
{
|
||||
sector = 4;
|
||||
break;
|
||||
}
|
||||
|
||||
case HALL3_60DEG_POS_6:
|
||||
{
|
||||
sector = 5;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
sector = -1;
|
||||
}
|
||||
}
|
||||
#else
|
||||
# error Invalid configuration
|
||||
#endif
|
||||
|
||||
return sector;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_sector_get()
|
||||
****************************************************************************/
|
||||
|
||||
static uint8_t foc_angle_hl_sector_get(FAR foc_angle_f32_t *h)
|
||||
{
|
||||
FAR struct foc_hall_f32_s *hl = NULL;
|
||||
int ret = OK;
|
||||
int8_t sector = -1;
|
||||
uint8_t hall = 0;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Get hall data */
|
||||
|
||||
DEBUGASSERT(h->data);
|
||||
hl = h->data;
|
||||
|
||||
/* Get the positions from hall */
|
||||
|
||||
ret = ioctl(hl->fd, SNIOC_GET_POSITION, (unsigned long)((uintptr_t)&hall));
|
||||
if (ret < 0)
|
||||
{
|
||||
FOCLIBERR("ERROR: H3PH_POSITION failed, errno=%d\n", errno);
|
||||
goto errout;
|
||||
}
|
||||
|
||||
/* Decode hall sector */
|
||||
|
||||
sector = foc_angle_hl_decode_sector(hall);
|
||||
if (sector == -1)
|
||||
{
|
||||
goto errout;
|
||||
}
|
||||
|
||||
errout:
|
||||
return sector;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_init_f32
|
||||
*
|
||||
* Description:
|
||||
* Initialize hall the FOC angle handler (float32)
|
||||
*
|
||||
* Input Parameter:
|
||||
* h - pointer to FOC angle handler
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static int foc_angle_hl_init_f32(FAR foc_angle_f32_t *h)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Connect angle data */
|
||||
|
||||
h->data = zalloc(sizeof(struct foc_hall_f32_s));
|
||||
if (h->data == NULL)
|
||||
{
|
||||
ret = -ENOMEM;
|
||||
goto errout;
|
||||
}
|
||||
|
||||
errout:
|
||||
return ret;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_deinit_f32
|
||||
*
|
||||
* Description:
|
||||
* De-initialize hall the FOC angle handler (float32)
|
||||
*
|
||||
* Input Parameter:
|
||||
* h - pointer to FOC angle handler
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static void foc_angle_hl_deinit_f32(FAR foc_angle_f32_t *h)
|
||||
{
|
||||
FAR struct foc_hall_f32_s *hl = NULL;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Get hall data */
|
||||
|
||||
DEBUGASSERT(h->data);
|
||||
hl = h->data;
|
||||
|
||||
if (h->data)
|
||||
{
|
||||
/* Close file */
|
||||
|
||||
if (hl->fd > 0)
|
||||
{
|
||||
close(hl->fd);
|
||||
}
|
||||
|
||||
/* Free angle data */
|
||||
|
||||
free(h->data);
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_cfg_f32
|
||||
*
|
||||
* Description:
|
||||
* Configure the hall FOC angle handler (float32)
|
||||
*
|
||||
* Input Parameter:
|
||||
* h - pointer to FOC angle handler
|
||||
* cfg - pointer to angle handler configuration data
|
||||
* (struct foc_hall_f32_s)
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static int foc_angle_hl_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg)
|
||||
{
|
||||
FAR struct foc_hall_f32_s *hl = NULL;
|
||||
int ret = OK;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Get hall data */
|
||||
|
||||
DEBUGASSERT(h->data);
|
||||
hl = h->data;
|
||||
|
||||
/* Copy configuration */
|
||||
|
||||
memcpy(&hl->cfg, cfg, sizeof(struct foc_hall_cfg_f32_s));
|
||||
|
||||
/* Open hall device */
|
||||
|
||||
hl->fd = open(hl->cfg.devpath, O_RDONLY);
|
||||
if (hl->fd <= 0)
|
||||
{
|
||||
FOCLIBERR("ERROR: failed to open %s, errno=%d\n",
|
||||
hl->cfg.devpath, errno);
|
||||
ret = -errno;
|
||||
goto errout;
|
||||
}
|
||||
|
||||
/* Initialize with CW direction */
|
||||
|
||||
hl->sensor_dir = DIR_CW;
|
||||
|
||||
errout:
|
||||
return ret;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_zero_f32
|
||||
*
|
||||
* Description:
|
||||
* Zero the hl FOC angle handler (float32)
|
||||
*
|
||||
* Input Parameter:
|
||||
* h - pointer to FOC angle handler
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static int foc_angle_hl_zero_f32(FAR foc_angle_f32_t *h)
|
||||
{
|
||||
FAR struct foc_hall_f32_s *hl = NULL;
|
||||
int ret = OK;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Get hall data */
|
||||
|
||||
DEBUGASSERT(h->data);
|
||||
hl = h->data;
|
||||
|
||||
/* Get hall offset */
|
||||
|
||||
hl->offset = foc_angle_hl_sector_get(h);
|
||||
if (hl->offset == -1)
|
||||
{
|
||||
ret = -EINVAL;
|
||||
goto errout;
|
||||
}
|
||||
|
||||
/* Reset data */
|
||||
|
||||
hl->angle = 0.0f;
|
||||
hl->sector = 0;
|
||||
|
||||
errout:
|
||||
return ret;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_dir_f32
|
||||
*
|
||||
* Description:
|
||||
* Set the hl FOC angle handler direction (float32)
|
||||
*
|
||||
* Input Parameter:
|
||||
* h - pointer to FOC angle handler
|
||||
* dir - sensor direction (1 if normal -1 if inverted)
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static int foc_angle_hl_dir_f32(FAR foc_angle_f32_t *h, float dir)
|
||||
{
|
||||
FAR struct foc_hall_f32_s *hl = NULL;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Get hall data */
|
||||
|
||||
DEBUGASSERT(h->data);
|
||||
hl = h->data;
|
||||
|
||||
/* Configure direction */
|
||||
|
||||
hl->sensor_dir = dir;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_hl_run_f32
|
||||
*
|
||||
* Description:
|
||||
* Process the hall FOC angle data (float32)
|
||||
*
|
||||
* Input Parameter:
|
||||
* h - pointer to FOC angle handler
|
||||
* in - pointer to FOC angle handler input data
|
||||
* out - pointer to FOC angle handler output data
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static int foc_angle_hl_run_f32(FAR foc_angle_f32_t *h,
|
||||
FAR struct foc_angle_in_f32_s *in,
|
||||
FAR struct foc_angle_out_f32_s *out)
|
||||
{
|
||||
FAR struct foc_hall_f32_s *hl = NULL;
|
||||
int ret = OK;
|
||||
int8_t sector = -1;
|
||||
|
||||
DEBUGASSERT(h);
|
||||
|
||||
/* Get hall data */
|
||||
|
||||
DEBUGASSERT(h->data);
|
||||
hl = h->data;
|
||||
|
||||
/* Get hall sector now */
|
||||
|
||||
sector = foc_angle_hl_sector_get(h);
|
||||
if (sector == -1)
|
||||
{
|
||||
ret = -EINVAL;
|
||||
goto errout;
|
||||
}
|
||||
|
||||
/* Correct sector with offset */
|
||||
|
||||
sector = sector - hl->offset;
|
||||
if (sector < 0)
|
||||
{
|
||||
sector = sector + HALL_MAX;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_HALL_EST
|
||||
/* Store previous state */
|
||||
|
||||
hl->sector_last = hl->sector;
|
||||
#endif
|
||||
|
||||
/* Store current sector */
|
||||
|
||||
hl->sector = sector;
|
||||
|
||||
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_HALL_EST
|
||||
/* Sector diff */
|
||||
|
||||
hl->sector_diff = hl->sector - hl->sector_last;
|
||||
|
||||
/* Handle next sector or estimate angle between sectors */
|
||||
|
||||
if (hl->sector_diff != 0)
|
||||
{
|
||||
/* Only if per_acc is valid */
|
||||
|
||||
if (hl->per_acc > 0.0f)
|
||||
{
|
||||
/* Do not update vel_est on boundaries */
|
||||
|
||||
if (hl->sector * hl->sector_last != 0)
|
||||
{
|
||||
/* Only if velocity direction not changed */
|
||||
|
||||
if (hl->sector_diff * hl->sector_diff_last > 0)
|
||||
{
|
||||
/* Next sector - estimate velocity */
|
||||
|
||||
hl->vel_est = (hl->sector_diff * HALL_ANGLE_STEP /
|
||||
hl->per_acc);
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Velocity dir changed */
|
||||
|
||||
hl->vel_est = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/* Reser accumulators */
|
||||
|
||||
hl->per_acc = 0.0f;
|
||||
hl->angle_diff = 0.0f;
|
||||
}
|
||||
|
||||
/* Store last sector diff */
|
||||
|
||||
hl->sector_diff_last = hl->sector_diff;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Accumulate period for velocity estimation */
|
||||
|
||||
hl->per_acc += hl->cfg.per;
|
||||
|
||||
/* Accumulate angle diff */
|
||||
|
||||
hl->angle_diff += hl->vel_est * hl->cfg.per;
|
||||
|
||||
/* Saturate angle diff */
|
||||
|
||||
if (hl->angle_diff > HALL_ANGLE_STEP)
|
||||
{
|
||||
hl->angle_diff = HALL_ANGLE_STEP;
|
||||
}
|
||||
else if (hl->angle_diff < -HALL_ANGLE_STEP)
|
||||
{
|
||||
hl->angle_diff = -HALL_ANGLE_STEP;
|
||||
}
|
||||
}
|
||||
|
||||
/* Get corrected electrical angle */
|
||||
|
||||
hl->angle = hl->sensor_dir * g_sector_angle[hl->sector] + hl->angle_diff;
|
||||
|
||||
#else
|
||||
/* Get electrical angle */
|
||||
|
||||
hl->angle = hl->sensor_dir * g_sector_angle[hl->sector];
|
||||
|
||||
#endif /* CONFIG_INDUSTRY_FOC_ANGLE_HALL_EST */
|
||||
|
||||
/* Normalize angle */
|
||||
|
||||
angle_norm_2pi(&hl->angle, MOTOR_ANGLE_E_MIN, MOTOR_ANGLE_E_MAX);
|
||||
|
||||
/* Copy data */
|
||||
|
||||
out->type = FOC_ANGLE_TYPE_ELE;
|
||||
out->angle = hl->angle;
|
||||
|
||||
errout:
|
||||
return ret;
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user