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

examples/foc: move FOC device data to foc_device_s

Browse Source
This commit is contained in:
raiden00pl 2021-10-31 10:54:22 +01:00 committed by Xiang Xiao
parent 33b34f8852
commit e0ef3cecb9
4 changed files with 48 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
examples/foc/foc_device.c
View File

@ -60,6 +60,15 @@ int foc_device_open(FAR struct foc_device_s *dev, int id)
goto errout; goto errout;
} }
/* Get device info */
ret = foc_dev_getinfo(dev->fd, &dev->info);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_getinfo failed %d!\n", ret);
goto errout;
}
errout: errout:
return ret; return ret;
} }
@ -74,6 +83,14 @@ int foc_device_close(FAR struct foc_device_s *dev)
DEBUGASSERT(dev); DEBUGASSERT(dev);
/* Stop FOC device */
ret = foc_dev_stop(dev->fd);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_stop %d!\n", ret);
}
if (dev->fd > 0) if (dev->fd > 0)
{ {
close(dev->fd); close(dev->fd);

7
examples/foc/foc_device.h
View File

@ -27,6 +27,8 @@
#include <nuttx/config.h> #include <nuttx/config.h>
#include "industry/foc/foc_utils.h"
/**************************************************************************** /****************************************************************************
* Public Type Definition * Public Type Definition
****************************************************************************/ ****************************************************************************/
@ -35,7 +37,10 @@
struct foc_device_s struct foc_device_s
{ {
int fd; /* FOC device */ int fd; /* FOC device */
struct foc_info_s info; /* FOC dev info */
struct foc_state_s state; /* FOC dev state */
struct foc_params_s params; /* FOC dev params */
}; };
/**************************************************************************** /****************************************************************************

45
examples/foc/foc_fixed16_thr.c
View File

@ -84,10 +84,7 @@ struct foc_motor_b16_s
dq_frame_b16_t vdq_comp; /* DQ voltage compensation */ dq_frame_b16_t vdq_comp; /* DQ voltage compensation */
foc_handler_b16_t handler; /* FOC controller */ foc_handler_b16_t handler; /* FOC controller */
struct foc_mq_s mq; /* MQ data */ struct foc_mq_s mq; /* MQ data */
struct foc_info_s info; /* Device info */
struct foc_state_b16_s foc_state; /* FOC controller sate */ struct foc_state_b16_s foc_state; /* FOC controller sate */
struct foc_state_s dev_state; /* FOC dev state */
struct foc_params_s dev_params; /* FOC dev params */
struct foc_ramp_b16_s ramp; /* Velocity ramp data */ struct foc_ramp_b16_s ramp; /* Velocity ramp data */
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
struct foc_model_b16_s model; /* Model handler */ struct foc_model_b16_s model; /* Model handler */
@ -139,7 +136,6 @@ static int foc_motor_init(FAR struct foc_motor_b16_s *motor,
foc_angle_cfg_b16(&motor->openloop, &ol_cfg); foc_angle_cfg_b16(&motor->openloop, &ol_cfg);
#endif #endif
errout:
return ret; return ret;
} }
@ -243,7 +239,7 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 #ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
/* Get SVM3 modulation configuration */ /* Get SVM3 modulation configuration */
mod_cfg.pwm_duty_max = FOCDUTY_TO_FIXED16(motor->info.hw_cfg.pwm_max); mod_cfg.pwm_duty_max = FOCDUTY_TO_FIXED16(motor->dev.info.hw_cfg.pwm_max);
#endif #endif
/* Configure FOC handler */ /* Configure FOC handler */
@ -610,7 +606,7 @@ static int foc_handler_run(FAR struct foc_motor_b16_s *motor)
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{ {
current[i] = b16muli(motor->iphase_adc, motor->dev_state.curr[i]); current[i] = b16muli(motor->iphase_adc, motor->dev.state.curr[i]);
} }
/* Get input for FOC handler */ /* Get input for FOC handler */
@ -630,7 +626,7 @@ static int foc_handler_run(FAR struct foc_motor_b16_s *motor)
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{ {
motor->dev_params.duty[i] = FOCDUTY_FROM_FIXED16(output.duty[i]); motor->dev.params.duty[i] = FOCDUTY_FROM_FIXED16(output.duty[i]);
} }
/* Get FOC handler state */ /* Get FOC handler state */
@ -655,7 +651,7 @@ static int foc_dev_state_get(FAR struct foc_motor_b16_s *motor)
/* Get FOC state - blocking */ /* Get FOC state - blocking */
ret = foc_dev_getstate(motor->dev.fd, &motor->dev_state); ret = foc_dev_getstate(motor->dev.fd, &motor->dev.state);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_dev_getstate failed %d!\n", ret); PRINTF("ERROR: foc_dev_getstate failed %d!\n", ret);
@ -671,7 +667,7 @@ static int foc_dev_state_get(FAR struct foc_motor_b16_s *motor)
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{ {
motor->dev_state.curr[i] = motor->model_state.curr_raw[i]; motor->dev.state.curr[i] = motor->model_state.curr_raw[i];
} }
#endif #endif
@ -691,7 +687,7 @@ static int foc_dev_params_set(FAR struct foc_motor_b16_s *motor)
/* Write FOC parameters */ /* Write FOC parameters */
ret = foc_dev_setparams(motor->dev.fd, &motor->dev_params); ret = foc_dev_setparams(motor->dev.fd, &motor->dev.params);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_dev_setparams failed %d!\n", ret); PRINTF("ERROR: foc_dev_setparams failed %d!\n", ret);
@ -703,16 +699,16 @@ errout:
} }
/**************************************************************************** /****************************************************************************
* Name: foc_state_handle * Name: foc_dev_state_handle
****************************************************************************/ ****************************************************************************/
static int foc_state_handle(FAR struct foc_motor_b16_s *motor) static int foc_dev_state_handle(FAR struct foc_motor_b16_s *motor)
{ {
DEBUGASSERT(motor); DEBUGASSERT(motor);
if (motor->dev_state.fault > 0) if (motor->dev.state.fault > 0)
{ {
PRINTF("FAULT = %d\n", motor->dev_state.fault); PRINTF("FAULT = %d\n", motor->dev.state.fault);
motor->fault = true; motor->fault = true;
} }
else else
@ -872,15 +868,6 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
goto errout; goto errout;
} }
/* Get device info */
ret = foc_dev_getinfo(motor.dev.fd, &motor.info);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_getinfo failed %d!\n", ret);
goto errout;
}
/* Initialize controller mode */ /* Initialize controller mode */
ret = foc_mode_init(&motor); ret = foc_mode_init(&motor);
@ -937,14 +924,14 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
/* Handle controller state */ /* Handle controller state */
ret = foc_state_handle(&motor); ret = foc_dev_state_handle(&motor);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret); PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
goto errout; goto errout;
} }
if (motor.dev_state.fault > 0) if (motor.dev.state.fault > 0)
{ {
/* Clear fault state */ /* Clear fault state */
@ -1031,14 +1018,6 @@ errout:
PRINTF("Stop FOC device %d!\n", envp->id); PRINTF("Stop FOC device %d!\n", envp->id);
/* Stop FOC device */
ret = foc_dev_stop(motor.dev.fd);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_stop %d!\n", ret);
}
/* Close FOC control device */ /* Close FOC control device */
ret = foc_device_close(&motor.dev); ret = foc_device_close(&motor.dev);

47
examples/foc/foc_float_thr.c
View File

@ -84,10 +84,7 @@ struct foc_motor_f32_s
dq_frame_f32_t vdq_comp; /* DQ voltage compensation */ dq_frame_f32_t vdq_comp; /* DQ voltage compensation */
foc_handler_f32_t handler; /* FOC controller */ foc_handler_f32_t handler; /* FOC controller */
struct foc_mq_s mq; /* MQ data */ struct foc_mq_s mq; /* MQ data */
struct foc_info_s info; /* Device info */
struct foc_state_f32_s foc_state; /* FOC controller sate */ struct foc_state_f32_s foc_state; /* FOC controller sate */
struct foc_state_s dev_state; /* FOC dev state */
struct foc_params_s dev_params; /* FOC dev params */
struct foc_ramp_f32_s ramp; /* Velocity ramp data */ struct foc_ramp_f32_s ramp; /* Velocity ramp data */
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
struct foc_model_f32_s model; /* Model handler */ struct foc_model_f32_s model; /* Model handler */
@ -139,7 +136,6 @@ static int foc_motor_init(FAR struct foc_motor_f32_s *motor,
foc_angle_cfg_f32(&motor->openloop, &ol_cfg); foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
#endif #endif
errout:
return ret; return ret;
} }
@ -243,7 +239,7 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 #ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
/* Get SVM3 modulation configuration */ /* Get SVM3 modulation configuration */
mod_cfg.pwm_duty_max = FOCDUTY_TO_FLOAT(motor->info.hw_cfg.pwm_max); mod_cfg.pwm_duty_max = FOCDUTY_TO_FLOAT(motor->dev.info.hw_cfg.pwm_max);
#endif #endif
/* Configure FOC handler */ /* Configure FOC handler */
@ -610,7 +606,7 @@ static int foc_handler_run(FAR struct foc_motor_f32_s *motor)
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{ {
current[i] = (motor->iphase_adc * motor->dev_state.curr[i]); current[i] = (motor->iphase_adc * motor->dev.state.curr[i]);
} }
/* Get input for FOC handler */ /* Get input for FOC handler */
@ -630,7 +626,7 @@ static int foc_handler_run(FAR struct foc_motor_f32_s *motor)
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{ {
motor->dev_params.duty[i] = FOCDUTY_FROM_FLOAT(output.duty[i]); motor->dev.params.duty[i] = FOCDUTY_FROM_FLOAT(output.duty[i]);
} }
/* Get FOC handler state */ /* Get FOC handler state */
@ -655,7 +651,7 @@ static int foc_dev_state_get(FAR struct foc_motor_f32_s *motor)
/* Get FOC state - blocking */ /* Get FOC state - blocking */
ret = foc_dev_getstate(motor->dev.fd, &motor->dev_state); ret = foc_dev_getstate(motor->dev.fd, &motor->dev.state);
if (ret < 0) if (ret < 0)
{ {
PRINTFV("ERROR: foc_dev_getstate failed %d!\n", ret); PRINTFV("ERROR: foc_dev_getstate failed %d!\n", ret);
@ -671,7 +667,7 @@ static int foc_dev_state_get(FAR struct foc_motor_f32_s *motor)
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{ {
motor->dev_state.curr[i] = motor->model_state.curr_raw[i]; motor->dev.state.curr[i] = motor->model_state.curr_raw[i];
} }
#endif #endif
@ -691,7 +687,7 @@ static int foc_dev_params_set(FAR struct foc_motor_f32_s *motor)
/* Write FOC parameters */ /* Write FOC parameters */
ret = foc_dev_setparams(motor->dev.fd, &motor->dev_params); ret = foc_dev_setparams(motor->dev.fd, &motor->dev.params);
if (ret < 0) if (ret < 0)
{ {
PRINTFV("ERROR: foc_dev_setparams failed %d!\n", ret); PRINTFV("ERROR: foc_dev_setparams failed %d!\n", ret);
@ -703,16 +699,16 @@ errout:
} }
/**************************************************************************** /****************************************************************************
* Name: foc_state_handle * Name: foc_dev_state_handle
****************************************************************************/ ****************************************************************************/
static int foc_state_handle(FAR struct foc_motor_f32_s *motor) static int foc_dev_state_handle(FAR struct foc_motor_f32_s *motor)
{ {
DEBUGASSERT(motor); DEBUGASSERT(motor);
if (motor->dev_state.fault > 0) if (motor->dev.state.fault > 0)
{ {
PRINTF("FAULT = %d\n", motor->dev_state.fault); PRINTF("FAULT = %d\n", motor->dev.state.fault);
motor->fault = true; motor->fault = true;
} }
else else
@ -875,15 +871,6 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
goto errout; goto errout;
} }
/* Get device info */
ret = foc_dev_getinfo(motor.dev.fd, &motor.info);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_getinfo failed %d!\n", ret);
goto errout;
}
/* Initialize controller mode */ /* Initialize controller mode */
ret = foc_mode_init(&motor); ret = foc_mode_init(&motor);
@ -940,14 +927,14 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
/* Handle controller state */ /* Handle controller state */
ret = foc_state_handle(&motor); ret = foc_dev_state_handle(&motor);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_state_handle failed %d!\n", ret); PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
goto errout; goto errout;
} }
if (motor.dev_state.fault > 0) if (motor.dev.state.fault > 0)
{ {
/* Clear fault state */ /* Clear fault state */
@ -1033,14 +1020,6 @@ errout:
PRINTF("Stop FOC device %d!\n", envp->id); PRINTF("Stop FOC device %d!\n", envp->id);
/* Stop FOC device */
ret = foc_dev_stop(motor.dev.fd);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_stop failed %d!\n", ret);
}
/* Close FOC control device */ /* Close FOC control device */
ret = foc_device_close(&motor.dev); ret = foc_device_close(&motor.dev);