From e0ef3cecb9bd64558e603a83dd47718d51b662b5 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sun, 31 Oct 2021 10:54:22 +0100 Subject: [PATCH] examples/foc: move FOC device data to foc_device_s --- examples/foc/foc_device.c | 17 ++++++++++++ examples/foc/foc_device.h | 7 ++++- examples/foc/foc_fixed16_thr.c | 45 +++++++++----------------------- examples/foc/foc_float_thr.c | 47 ++++++++++------------------------ 4 files changed, 48 insertions(+), 68 deletions(-) diff --git a/examples/foc/foc_device.c b/examples/foc/foc_device.c index 605ec67ee..3e3570d8e 100644 --- a/examples/foc/foc_device.c +++ b/examples/foc/foc_device.c @@ -60,6 +60,15 @@ int foc_device_open(FAR struct foc_device_s *dev, int id) 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: return ret; } @@ -74,6 +83,14 @@ int foc_device_close(FAR struct foc_device_s *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) { close(dev->fd); diff --git a/examples/foc/foc_device.h b/examples/foc/foc_device.h index be5be9256..fc4d42b01 100644 --- a/examples/foc/foc_device.h +++ b/examples/foc/foc_device.h @@ -27,6 +27,8 @@ #include +#include "industry/foc/foc_utils.h" + /**************************************************************************** * Public Type Definition ****************************************************************************/ @@ -35,7 +37,10 @@ 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 */ }; /**************************************************************************** diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c index 44fb1e6d1..7009464b2 100644 --- a/examples/foc/foc_fixed16_thr.c +++ b/examples/foc/foc_fixed16_thr.c @@ -84,10 +84,7 @@ struct foc_motor_b16_s dq_frame_b16_t vdq_comp; /* DQ voltage compensation */ foc_handler_b16_t handler; /* FOC controller */ 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_s dev_state; /* FOC dev state */ - struct foc_params_s dev_params; /* FOC dev params */ struct foc_ramp_b16_s ramp; /* Velocity ramp data */ #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM 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); #endif -errout: return ret; } @@ -243,7 +239,7 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor) #ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 /* 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 /* 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) { - 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 */ @@ -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) { - 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 */ @@ -655,7 +651,7 @@ static int foc_dev_state_get(FAR struct foc_motor_b16_s *motor) /* 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) { 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) { - motor->dev_state.curr[i] = motor->model_state.curr_raw[i]; + motor->dev.state.curr[i] = motor->model_state.curr_raw[i]; } #endif @@ -691,7 +687,7 @@ static int foc_dev_params_set(FAR struct foc_motor_b16_s *motor) /* 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) { 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); - 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; } else @@ -872,15 +868,6 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp) 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 */ ret = foc_mode_init(&motor); @@ -937,14 +924,14 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp) /* Handle controller state */ - ret = foc_state_handle(&motor); + ret = foc_dev_state_handle(&motor); if (ret < 0) { PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret); goto errout; } - if (motor.dev_state.fault > 0) + if (motor.dev.state.fault > 0) { /* Clear fault state */ @@ -1031,14 +1018,6 @@ errout: 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 */ ret = foc_device_close(&motor.dev); diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c index 1386ae874..5f4765209 100644 --- a/examples/foc/foc_float_thr.c +++ b/examples/foc/foc_float_thr.c @@ -84,10 +84,7 @@ struct foc_motor_f32_s dq_frame_f32_t vdq_comp; /* DQ voltage compensation */ foc_handler_f32_t handler; /* FOC controller */ 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_s dev_state; /* FOC dev state */ - struct foc_params_s dev_params; /* FOC dev params */ struct foc_ramp_f32_s ramp; /* Velocity ramp data */ #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM 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); #endif -errout: return ret; } @@ -243,7 +239,7 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor) #ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 /* 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 /* 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) { - 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 */ @@ -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) { - 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 */ @@ -655,7 +651,7 @@ static int foc_dev_state_get(FAR struct foc_motor_f32_s *motor) /* 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) { 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) { - motor->dev_state.curr[i] = motor->model_state.curr_raw[i]; + motor->dev.state.curr[i] = motor->model_state.curr_raw[i]; } #endif @@ -691,7 +687,7 @@ static int foc_dev_params_set(FAR struct foc_motor_f32_s *motor) /* 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) { 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); - 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; } else @@ -875,15 +871,6 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp) 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 */ ret = foc_mode_init(&motor); @@ -940,14 +927,14 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp) /* Handle controller state */ - ret = foc_state_handle(&motor); + ret = foc_dev_state_handle(&motor); if (ret < 0) { - PRINTF("ERROR: foc_state_handle failed %d!\n", ret); + PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret); goto errout; } - if (motor.dev_state.fault > 0) + if (motor.dev.state.fault > 0) { /* Clear fault state */ @@ -1033,14 +1020,6 @@ errout: 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 */ ret = foc_device_close(&motor.dev);