2021-03-01 21:11:08 +01:00
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* drivers/motor/foc/foc_dummy.c
|
2021-03-01 21:11:08 +01: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
|
|
|
|
*
|
|
|
|
* 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 <stdio.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <strings.h>
|
|
|
|
#include <errno.h>
|
|
|
|
#include <assert.h>
|
|
|
|
#include <debug.h>
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
#include <nuttx/motor/foc/foc_dummy.h>
|
2021-03-01 21:11:08 +01:00
|
|
|
#include <nuttx/motor/foc/foc_lower.h>
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Pre-processor Definitions
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
#ifndef CONFIG_MOTOR_FOC
|
|
|
|
# error Upper-half FOC driver must be enabled
|
|
|
|
#endif
|
|
|
|
|
|
|
|
/* Board HW configuration */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
#define FOC_DUMMY_HW_PWM_NS (500)
|
|
|
|
#define FOC_DUMMY_HW_PWM_MAX (0.95f)
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Helper macros ************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
#define FOC_DUMMY_DATA_FROM_DEV_GET(d) \
|
|
|
|
((FAR struct foc_dummy_data_s *)(d)->lower->data)
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Private Types
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/* SIM FOC board data */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
struct foc_dummy_board_s
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
uint32_t reserved;
|
|
|
|
};
|
|
|
|
|
|
|
|
/* SIM FOC specific data */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
struct foc_dummy_data_s
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
/* Upper-half FOC controller callbacks */
|
|
|
|
|
|
|
|
FAR const struct foc_callbacks_s *cb;
|
|
|
|
|
|
|
|
/* SIM FOC board data */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
FAR struct foc_dummy_board_s *board;
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Phase currents */
|
|
|
|
|
|
|
|
foc_current_t current[CONFIG_MOTOR_FOC_PHASES];
|
|
|
|
|
2023-01-15 14:41:02 +01:00
|
|
|
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
|
|
|
|
/* BEMF voltage */
|
|
|
|
|
|
|
|
foc_voltage_t volt[CONFIG_MOTOR_FOC_PHASES];
|
|
|
|
#endif
|
|
|
|
|
2021-03-01 21:11:08 +01:00
|
|
|
/* FOC worker loop helpers */
|
|
|
|
|
|
|
|
bool state;
|
|
|
|
uint32_t notifier_cntr;
|
|
|
|
uint32_t pwm_freq;
|
|
|
|
uint32_t notifier_freq;
|
|
|
|
uint32_t notifier_div;
|
|
|
|
};
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Private Function Prototypes
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/* Lower-half FOC operations */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_configure(FAR struct foc_dev_s *dev,
|
|
|
|
FAR struct foc_cfg_s *cfg);
|
|
|
|
static int foc_dummy_setup(FAR struct foc_dev_s *dev);
|
|
|
|
static int foc_dummy_shutdown(FAR struct foc_dev_s *dev);
|
|
|
|
static int foc_dummy_start(FAR struct foc_dev_s *dev, bool state);
|
|
|
|
static int foc_dummy_pwm_duty_set(FAR struct foc_dev_s *dev,
|
|
|
|
FAR foc_duty_t *duty);
|
|
|
|
static int foc_dummy_ioctl(FAR struct foc_dev_s *dev, int cmd,
|
|
|
|
unsigned long arg);
|
|
|
|
static int foc_dummy_bind(FAR struct foc_dev_s *dev,
|
|
|
|
FAR struct foc_callbacks_s *cb);
|
|
|
|
static int foc_dummy_fault_clear(FAR struct foc_dev_s *dev);
|
2021-03-01 21:11:08 +01:00
|
|
|
#ifdef CONFIG_MOTOR_FOC_TRACE
|
2022-01-06 09:19:25 +01:00
|
|
|
static void foc_dummy_trace(FAR struct foc_dev_s *dev, int type, bool state);
|
2021-03-01 21:11:08 +01:00
|
|
|
#endif
|
|
|
|
|
|
|
|
/* Handlers */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static void foc_dummy_notifier_handler(FAR struct foc_dev_s *dev);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Helpers */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static void foc_dummy_hw_config_get(FAR struct foc_dev_s *dev);
|
|
|
|
static int foc_dummy_notifier_cfg(FAR struct foc_dev_s *dev, uint32_t freq);
|
|
|
|
static int foc_dummy_pwm_setup(FAR struct foc_dev_s *dev, uint32_t freq);
|
|
|
|
static int foc_dummy_pwm_start(FAR struct foc_dev_s *dev, bool state);
|
|
|
|
static int foc_dummy_adc_setup(FAR struct foc_dev_s *dev);
|
|
|
|
static int foc_dummy_adc_start(FAR struct foc_dev_s *dev, bool state);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Private Data
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/* SIM FOC specific data */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static struct foc_dummy_data_s g_foc_dummy_data[CONFIG_MOTOR_FOC_INST];
|
|
|
|
static struct foc_dummy_board_s g_foc_dummy_board[CONFIG_MOTOR_FOC_INST];
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* SIM specific FOC ops */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static struct foc_lower_ops_s g_foc_dummy_ops =
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
foc_dummy_configure,
|
|
|
|
foc_dummy_setup,
|
|
|
|
foc_dummy_shutdown,
|
|
|
|
foc_dummy_pwm_duty_set,
|
|
|
|
foc_dummy_start,
|
|
|
|
foc_dummy_ioctl,
|
|
|
|
foc_dummy_bind,
|
|
|
|
foc_dummy_fault_clear,
|
2021-03-01 21:11:08 +01:00
|
|
|
#ifdef CONFIG_MOTOR_FOC_TRACE
|
2022-01-06 09:19:25 +01:00
|
|
|
foc_dummy_trace
|
2021-03-01 21:11:08 +01:00
|
|
|
#endif
|
|
|
|
};
|
|
|
|
|
|
|
|
/* FOC lower-half */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static struct foc_lower_s g_foc_dummy_lower[CONFIG_MOTOR_FOC_INST];
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* FOC upper-half device data */
|
|
|
|
|
|
|
|
static struct foc_dev_s g_foc_dev[CONFIG_MOTOR_FOC_INST];
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Private Functions
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_pwm_setup
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Setup PWM for FOC controller
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_pwm_setup(FAR struct foc_dev_s *dev, uint32_t freq)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
FAR struct foc_dummy_data_s *sim = FOC_DUMMY_DATA_FROM_DEV_GET(dev);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
DEBUGASSERT(sim);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[PWM_SETUP] freq=%d\n", freq);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
DEBUGASSERT(freq > 0);
|
|
|
|
|
|
|
|
/* Store frequency */
|
|
|
|
|
|
|
|
sim->pwm_freq = freq;
|
|
|
|
|
|
|
|
return OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_start
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Start/stop PWM for the FOC controller
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_start(FAR struct foc_dev_s *dev, bool state)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
FAR struct foc_dummy_data_s *sim = FOC_DUMMY_DATA_FROM_DEV_GET(dev);
|
|
|
|
irqstate_t flags;
|
|
|
|
int ret = OK;
|
2021-03-01 21:11:08 +01:00
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[FOC_START] state=%d\n", state);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Start PWM */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
ret = foc_dummy_pwm_start(dev, state);
|
2021-03-01 21:11:08 +01:00
|
|
|
if (ret < 0)
|
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
mtrerr("foc_dummy_pwm_start failed %d\n", ret);
|
2021-03-01 21:11:08 +01:00
|
|
|
goto errout;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Start ADC */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
ret = foc_dummy_adc_start(dev, state);
|
2021-03-01 21:11:08 +01:00
|
|
|
if (ret < 0)
|
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
mtrerr("foc_dummy_adc_start failed %d\n", ret);
|
2021-03-01 21:11:08 +01:00
|
|
|
goto errout;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Store FOC worker state */
|
|
|
|
|
|
|
|
flags = enter_critical_section();
|
|
|
|
sim->state = state;
|
|
|
|
leave_critical_section(flags);
|
|
|
|
|
|
|
|
errout:
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_pwm_start
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Start/stop PWM for the FOC controller
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_pwm_start(FAR struct foc_dev_s *dev, bool state)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[PWM_START] state=%d\n", state);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
return OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_adc_setup
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Setup ADC for the FOC controller
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_adc_setup(FAR struct foc_dev_s *dev)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[ADC_SETUP]\n");
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
return OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_adc_start
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Start/stop ADC conversion for the FOC controller
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_adc_start(FAR struct foc_dev_s *dev, bool state)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[ADC_START] state=%d\n", state);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
return OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_notifier_cfg
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Configure FOC notifier
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_notifier_cfg(FAR struct foc_dev_s *dev, uint32_t freq)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
FAR struct foc_dummy_data_s *sim = FOC_DUMMY_DATA_FROM_DEV_GET(dev);
|
|
|
|
int ret = OK;
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
DEBUGASSERT(sim);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[NOTIFIER_CFG] freq=%d\n", freq);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
DEBUGASSERT(freq > 0);
|
|
|
|
|
|
|
|
if (sim->pwm_freq % freq != 0)
|
|
|
|
{
|
|
|
|
ret = -EINVAL;
|
|
|
|
goto errout;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Call FOC notifier on every update */
|
|
|
|
|
|
|
|
sim->notifier_freq = freq;
|
|
|
|
sim->notifier_div = 1;
|
|
|
|
|
|
|
|
errout:
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_configure
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Arch-specific FOC controller configuration
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_configure(FAR struct foc_dev_s *dev,
|
|
|
|
FAR struct foc_cfg_s *cfg)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
FAR struct foc_dummy_data_s *sim = FOC_DUMMY_DATA_FROM_DEV_GET(dev);
|
|
|
|
int ret = OK;
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
DEBUGASSERT(sim);
|
|
|
|
|
|
|
|
DEBUGASSERT(cfg->pwm_freq > 0);
|
|
|
|
DEBUGASSERT(cfg->notifier_freq > 0);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[FOC_SETUP]\n");
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Configure ADC */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
ret = foc_dummy_adc_setup(dev);
|
2021-03-01 21:11:08 +01:00
|
|
|
if (ret < 0)
|
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
mtrerr("foc_dummy_adc_setup failed %d\n", ret);
|
2021-03-01 21:11:08 +01:00
|
|
|
goto errout;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Configure PWM */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
ret = foc_dummy_pwm_setup(dev, cfg->pwm_freq);
|
2021-03-01 21:11:08 +01:00
|
|
|
if (ret < 0)
|
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
mtrerr("foc_dummy_pwm_setup failed %d\n", ret);
|
2021-03-01 21:11:08 +01:00
|
|
|
goto errout;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Configure notifier */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
ret = foc_dummy_notifier_cfg(dev, cfg->notifier_freq);
|
2021-03-01 21:11:08 +01:00
|
|
|
if (ret < 0)
|
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
mtrerr("foc_dummy_notifier_cfg failed %d\n", ret);
|
2021-03-01 21:11:08 +01:00
|
|
|
goto errout;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* Reset some data */
|
|
|
|
|
|
|
|
sim->notifier_cntr = 0;
|
|
|
|
|
|
|
|
errout:
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_setup
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Arch-specific FOC controller setup
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_setup(FAR struct foc_dev_s *dev)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[FOC_SETUP]\n");
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Get HW configuration */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
foc_dummy_hw_config_get(dev);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
return OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_shutdown
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Arch-specific FOC controller shutdown
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_shutdown(FAR struct foc_dev_s *dev)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[FOC_SHUTDOWN]\n");
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
return OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_ioctl
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Arch-specific FOC controller ioctl
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_ioctl(FAR struct foc_dev_s *dev, int cmd,
|
|
|
|
unsigned long arg)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
int ret = OK;
|
|
|
|
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[FOC_IOCTL]cmd=%d\n", cmd);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
switch (cmd)
|
|
|
|
{
|
|
|
|
default:
|
|
|
|
{
|
|
|
|
ret = -1;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_notifier_handler
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Handle ADC conversion and notofiy user-space
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static void foc_dummy_notifier_handler(FAR struct foc_dev_s *dev)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
FAR struct foc_dummy_data_s *sim = FOC_DUMMY_DATA_FROM_DEV_GET(dev);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
DEBUGASSERT(sim);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[FOC_NOTIFIER_HANDLER] cntr=%d\n", sim->notifier_cntr);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Call FOC notifier handler */
|
|
|
|
|
|
|
|
if (sim->notifier_cntr % sim->notifier_div == 0)
|
|
|
|
{
|
|
|
|
/* Call FOC notifier */
|
|
|
|
|
2023-01-15 14:41:02 +01:00
|
|
|
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
|
|
|
|
sim->cb->notifier(dev, sim->current, sim->voltage);
|
|
|
|
#else
|
|
|
|
sim->cb->notifier(dev, sim->current, NULL);
|
|
|
|
#endif
|
2021-03-01 21:11:08 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
/* Increase counter */
|
|
|
|
|
|
|
|
sim->notifier_cntr += 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Name: sim_duty_set
|
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Set 3-phase PWM duty cycle
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_pwm_duty_set(FAR struct foc_dev_s *dev,
|
|
|
|
FAR foc_duty_t *duty)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
int i = 0;
|
|
|
|
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
DEBUGASSERT(duty);
|
|
|
|
|
|
|
|
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
|
|
|
|
{
|
|
|
|
DEBUGASSERT(duty[i] >= 0);
|
|
|
|
}
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[PWM_DUTY_SET]\n");
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
#if CONFIG_MOTOR_FOC_PHASES == 2
|
2021-04-01 08:57:23 +02:00
|
|
|
mtrinfo("[%d %d]\n", duty[0], duty[1]);
|
2021-03-01 21:11:08 +01:00
|
|
|
#elif CONFIG_MOTOR_FOC_PHASES == 3
|
2021-04-01 08:57:23 +02:00
|
|
|
mtrinfo("[%d %d %d]\n", duty[0], duty[1], duty[2]);
|
2021-03-01 21:11:08 +01:00
|
|
|
#elif CONFIG_MOTOR_FOC_PHASES == 4
|
2021-04-01 08:57:23 +02:00
|
|
|
mtrinfo("[%d %d %d %d]\n", duty[0], duty[1], duty[2], duty[3]);
|
2021-03-01 21:11:08 +01:00
|
|
|
#else
|
|
|
|
# error
|
|
|
|
#endif
|
|
|
|
|
|
|
|
return OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_hw_config_get
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Get HW configuration for FOC controller
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static void foc_dummy_hw_config_get(FAR struct foc_dev_s *dev)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
|
|
|
|
/* Get HW configuration */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
dev->info.hw_cfg.pwm_dt_ns = FOC_DUMMY_HW_PWM_NS;
|
|
|
|
dev->info.hw_cfg.pwm_max = ftob16(FOC_DUMMY_HW_PWM_MAX);
|
2021-03-01 21:11:08 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_bind
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Bind lower-half FOC controller with upper-half FOC logic
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_bind(FAR struct foc_dev_s *dev,
|
|
|
|
FAR struct foc_callbacks_s *cb)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
FAR struct foc_dummy_data_s *sim = FOC_DUMMY_DATA_FROM_DEV_GET(dev);
|
|
|
|
int ret = OK;
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
DEBUGASSERT(cb);
|
|
|
|
DEBUGASSERT(sim);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[FOC_BIND]\n");
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Bind upper-half FOC controller callbacks */
|
|
|
|
|
|
|
|
sim->cb = cb;
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_fault_clear
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Arch-specific fault clear
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static int foc_dummy_fault_clear(FAR struct foc_dev_s *dev)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[FAULT_CLEAR]\n");
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
return OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
#ifdef CONFIG_MOTOR_FOC_TRACE
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_trace
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* SIM FOC trace
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
static void foc_dummy_trace(FAR struct foc_dev_s *dev, int type, bool state)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
DEBUGASSERT(dev);
|
|
|
|
|
2021-11-15 15:32:55 +01:00
|
|
|
mtrinfo("[FOC_TRACE] type=%d state=%d\n", type, state);
|
2021-03-01 21:11:08 +01:00
|
|
|
}
|
|
|
|
#endif /* CONFIG_MOTOR_FOC_TRACE */
|
|
|
|
|
|
|
|
/****************************************************************************
|
|
|
|
* Public Functions
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_initialize
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Initialize the FOC controller lower-half support.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
FAR struct foc_dev_s *foc_dummy_initialize(int inst)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
|
|
|
FAR struct foc_lower_s *foc_lower = NULL;
|
|
|
|
FAR struct foc_dev_s *dev = NULL;
|
|
|
|
|
2021-04-01 08:57:23 +02:00
|
|
|
mtrinfo("[FOC_INITIALIZE] inst=%d\n", inst);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Reset data */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
memset(&g_foc_dummy_data[inst], 0, sizeof(struct foc_dummy_data_s));
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Get SIM FOC arch-specific driver */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
foc_lower = &g_foc_dummy_lower[inst];
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Connect ops, data and dev with arch-specific FOC driver */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
foc_lower->data = &g_foc_dummy_data[inst];
|
|
|
|
foc_lower->ops = &g_foc_dummy_ops;
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Connect board data */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
g_foc_dummy_data[inst].board = &g_foc_dummy_board[inst];
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
/* Get FOC device */
|
|
|
|
|
|
|
|
dev = &g_foc_dev[inst];
|
|
|
|
|
2022-07-07 18:01:22 +02:00
|
|
|
/* Connect lower half FOC with FOC device */
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
dev->lower = (FAR void *)foc_lower;
|
|
|
|
|
|
|
|
/* Return lower-half driver instance */
|
|
|
|
|
|
|
|
return dev;
|
|
|
|
}
|
|
|
|
|
|
|
|
/****************************************************************************
|
2022-01-06 09:19:25 +01:00
|
|
|
* Name: foc_dummy_update
|
2021-03-01 21:11:08 +01:00
|
|
|
*
|
|
|
|
* Description:
|
|
|
|
* Called periodically from the IDLE loop to simulate FOC driver interrupts
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
void foc_dummy_update(void)
|
2021-03-01 21:11:08 +01:00
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
FAR struct foc_dev_s *dev = NULL;
|
|
|
|
FAR struct foc_dummy_data_s *sim = NULL;
|
|
|
|
int i = 0;
|
|
|
|
irqstate_t flags;
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
flags = enter_critical_section();
|
|
|
|
|
|
|
|
/* Update all FOC instances */
|
|
|
|
|
|
|
|
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
|
|
|
|
{
|
|
|
|
/* Get FOC device */
|
|
|
|
|
|
|
|
dev = &g_foc_dev[i];
|
|
|
|
|
|
|
|
/* Get SIM data */
|
|
|
|
|
2022-01-06 09:19:25 +01:00
|
|
|
sim = FOC_DUMMY_DATA_FROM_DEV_GET(dev);
|
2021-03-01 21:11:08 +01:00
|
|
|
|
|
|
|
if (sim->state == true)
|
|
|
|
{
|
2022-01-06 09:19:25 +01:00
|
|
|
foc_dummy_notifier_handler(dev);
|
2021-03-01 21:11:08 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
leave_critical_section(flags);
|
|
|
|
}
|