This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx-apps.git
The following commit(s) were added to refs/heads/master by this push:
new 2fb79db examples/foc: replace the requested velocity input with a
more general setpoint input.
2fb79db is described below
commit 2fb79db8b41a988c27a80fdbdd832b02b228772c
Author: raiden00pl <[email protected]>
AuthorDate: Sat Oct 30 13:53:32 2021 +0200
examples/foc: replace the requested velocity input with a more general
setpoint input.
This is the inital step to support torque/velocity/position control
---
examples/foc/Kconfig | 34 +++++++++++++++----------------
examples/foc/foc_adc.h | 30 +++++++++++++--------------
examples/foc/foc_fixed16_thr.c | 12 +++++------
examples/foc/foc_float_thr.c | 17 +++++++++-------
examples/foc/foc_main.c | 46 ++++++++++++++++++++++++------------------
examples/foc/foc_mq.c | 4 ++--
examples/foc/foc_mq.h | 4 ++--
7 files changed, 78 insertions(+), 69 deletions(-)
diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index 1bd955c..c3cf592 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -146,33 +146,33 @@ config EXAMPLES_FOC_VBUS_SCALE
endif # EXAMPLES_FOC_VBUS_ADC
choice
- prompt "FOC velocity source"
- default EXAMPLES_FOC_VEL_CONST
+ prompt "FOC setpoint source"
+ default EXAMPLES_FOC_SETPOINT_CONST
-config EXAMPLES_FOC_VEL_CONST
- bool "Use hardcoded constant velocity value"
+config EXAMPLES_FOC_SETPOINT_CONST
+ bool "Use hardcoded constant setpiont value"
-config EXAMPLES_FOC_VEL_ADC
- bool "Use ADC to control velocity"
+config EXAMPLES_FOC_SETPOINT_ADC
+ bool "Use ADC to control setpoint"
select EXAMPLES_FOC_HAVE_ADC
-endchoice # FOC velocity interface
+endchoice # FOC setpoint interface
-if EXAMPLES_FOC_VEL_CONST
+if EXAMPLES_FOC_SETPOINT_CONST
-config EXAMPLES_FOC_VEL_CONST_VALUE
- int "FOC hardoced velocity value"
- default 100000
+config EXAMPLES_FOC_SETPOINT_CONST_VALUE
+ int "FOC hardoced setpoint value"
+ default 0
-endif # EXAMPLES_FOC_VEL_CONST
+endif # EXAMPLES_FOC_SETPOINT_CONST
-if EXAMPLES_FOC_VEL_ADC
+if EXAMPLES_FOC_SETPOINT_ADC
-config EXAMPLES_FOC_VEL_ADC_MAX
- int "FOC maximum velocity from ADC [x1000]"
- default 100000
+config EXAMPLES_FOC_SETPOINT_ADC_MAX
+ int "FOC maximum setpoint from ADC [x1000]"
+ default 0
-endif # EXAMPLES_FOC_VEL_ADC
+endif # EXAMPLES_FOC_SETPOINT_ADC
config EXAMPLES_FOC_TIME_DEFAULT
int "FOC run time default (sec)"
diff --git a/examples/foc/foc_adc.h b/examples/foc/foc_adc.h
index 38cc822..28fcc04 100644
--- a/examples/foc/foc_adc.h
+++ b/examples/foc/foc_adc.h
@@ -40,8 +40,8 @@
/* Velocity source must be specified */
-#if defined(CONFIG_EXAMPLES_FOC_VEL_CONST) && \
- defined(CONFIG_EXAMPLES_FOC_VEL_ADC)
+#if defined(CONFIG_EXAMPLES_FOC_SETPOINT_CONST) && \
+ defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC)
# error
#endif
@@ -57,15 +57,15 @@
/* Velocity ADC scale factor */
-#ifdef CONFIG_EXAMPLES_FOC_VEL_ADC
-# define VEL_ADC_SCALE (1.0f / CONFIG_EXAMPLES_FOC_ADC_MAX)
+#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
+# define SETPOINT_ADC_SCALE (1.0f / CONFIG_EXAMPLES_FOC_ADC_MAX)
#endif
/* If constant velocity is selected, velocity value must be provided */
-#ifdef CONFIG_EXAMPLES_FOC_VEL_CONST
-# define VEL_ADC_SCALE (1)
-# if CONFIG_EXAMPLES_FOC_VEL_CONST_VALUE == 0
+#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST
+# define SETPOINT_ADC_SCALE (1)
+# if CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE == 0
# error
# endif
#endif
@@ -86,10 +86,10 @@
/* AUX ADC samples support */
-# if defined(CONFIG_EXAMPLES_FOC_VEL_ADC) && \
+# if defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC) && \
defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
# define ADC_SAMPLES (2)
-# elif defined(CONFIG_EXAMPLES_FOC_VEL_ADC) || \
+# elif defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC) || \
defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
# define ADC_SAMPLES (1)
# else
@@ -104,14 +104,14 @@
/* Numerate ADC samples */
-# if defined(CONFIG_EXAMPLES_FOC_VEL_ADC) && \
+# if defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC) && \
defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
-# define VBUS_ADC_SAMPLE (0)
-# define VEL_ADC_SAMPLE (1)
+# define VBUS_ADC_SAMPLE (0)
+# define SETPOINT_ADC_SAMPLE (1)
# elif defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
-# define VBUS_ADC_SAMPLE (0)
-# elif defined(CONFIG_EXAMPLES_FOC_VEL_ADC)
-# define VEL_ADC_SAMPLE (0)
+# define VBUS_ADC_SAMPLE (0)
+# elif defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC)
+# define SETPOINT_ADC_SAMPLE (0)
# endif
#endif /* CONFIG_EXAMPLES_FOC_HAVE_ADC */
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index ccff7e9..6b6fc98 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -434,7 +434,7 @@ static int foc_motor_vel(FAR struct foc_motor_b16_s *motor,
uint32_t vel)
*/
tmp1 = itob16(motor->envp->velmax / 1000);
- tmp2 = b16mulb16(ftob16(VEL_ADC_SCALE), tmp1);
+ tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1);
motor->vel_des = b16muli(tmp2, vel);
@@ -773,19 +773,19 @@ static int foc_motor_handle(FAR struct foc_motor_b16_s
*motor,
/* Update motor velocity destination */
- if (motor->mq.vel != handle->vel)
+ if (motor->mq.setpoint != handle->setpoint)
{
- PRINTFV("Set vel=%" PRIu32 " for FOC driver %d!\n",
- handle->vel, motor->envp->id);
+ PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n",
+ handle->setpoint, motor->envp->id);
- ret = foc_motor_vel(motor, handle->vel);
+ ret = foc_motor_vel(motor, handle->setpoint);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
goto errout;
}
- motor->mq.vel = handle->vel;
+ motor->mq.setpoint = handle->setpoint;
}
/* Update motor state */
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index c091211..e1a7eed 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -435,7 +435,8 @@ static int foc_motor_vel(FAR struct foc_motor_f32_s *motor,
uint32_t vel)
/* Update motor velocity destination */
- motor->vel_des = (vel * VEL_ADC_SCALE * motor->envp->velmax / 1000.0f);
+ motor->vel_des = (vel * SETPOINT_ADC_SCALE *
+ motor->envp->velmax / 1000.0f);
return OK;
}
@@ -771,21 +772,23 @@ static int foc_motor_handle(FAR struct foc_motor_f32_s
*motor,
motor->mq.vbus = handle->vbus;
}
- /* Update motor velocity destination */
+ /* Update motor velocity destination
+ * NOTE: only velocity control supported for now
+ */
- if (motor->mq.vel != handle->vel)
+ if (motor->mq.setpoint != handle->setpoint)
{
- PRINTFV("Set vel=%" PRIu32 " for FOC driver %d!\n",
- handle->vel, motor->envp->id);
+ PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n",
+ handle->setpoint, motor->envp->id);
- ret = foc_motor_vel(motor, handle->vel);
+ ret = foc_motor_vel(motor, handle->setpoint);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
goto errout;
}
- motor->mq.vel = handle->vel;
+ motor->mq.setpoint = handle->setpoint;
}
/* Update motor state */
diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c
index 66e6bbb..cfe72e1 100644
--- a/examples/foc/foc_main.c
+++ b/examples/foc/foc_main.c
@@ -139,13 +139,19 @@ static void init_args(FAR struct args_s *args)
(args->pi_kp == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KP : args->pi_kp);
args->pi_ki =
(args->pi_ki == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KI : args->pi_ki);
-#ifdef CONFIG_EXAMPLES_FOC_VEL_ADC
+
+ /* For now only velocity control supported */
+
+#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
args->velmax =
- (args->velmax == 0 ? CONFIG_EXAMPLES_FOC_VEL_ADC_MAX : args->velmax);
+ (args->velmax == 0 ?
+ CONFIG_EXAMPLES_FOC_SETPOINT_ADC_MAX : args->velmax);
#else
args->velmax =
- (args->velmax == 0 ? CONFIG_EXAMPLES_FOC_VEL_CONST_VALUE : args->velmax);
+ (args->velmax == 0 ?
+ CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->velmax);
#endif
+
args->state =
(args->state == 0 ? CONFIG_EXAMPLES_FOC_STATE_INIT : args->state);
args->en = (args->en == -1 ? INST_EN_DEAFULT : args->en);
@@ -248,12 +254,12 @@ static int foc_vbus_send(mqd_t mqd, uint32_t vbus)
}
/****************************************************************************
- * Name: foc_vel_send
+ * Name: foc_setpoint_send
****************************************************************************/
-static int foc_vel_send(mqd_t mqd, uint32_t vel)
+static int foc_setpoint_send(mqd_t mqd, uint32_t setpoint)
{
- return foc_mq_send(mqd, CONTROL_MQ_MSG_VEL, (FAR void *)&vel);
+ return foc_mq_send(mqd, CONTROL_MQ_MSG_SETPOINT, (FAR void *)&setpoint);
}
/****************************************************************************
@@ -513,10 +519,10 @@ int main(int argc, char *argv[])
#endif
uint32_t state = 0;
uint32_t vbus_raw = 0;
- int32_t vel_raw = 0;
+ int32_t sp_raw = 0;
bool vbus_update = false;
bool state_update = false;
- bool vel_update = false;
+ bool sp_update = false;
bool terminate = false;
bool started = false;
int ret = OK;
@@ -639,9 +645,9 @@ int main(int argc, char *argv[])
vbus_update = true;
vbus_raw = VBUS_CONST_VALUE;
#endif
-#ifndef CONFIG_EXAMPLES_FOC_VEL_ADC
- vel_update = true;
- vel_raw = 1;
+#ifndef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
+ sp_update = true;
+ sp_raw = 1;
#endif
state_update = true;
@@ -756,12 +762,12 @@ int main(int argc, char *argv[])
vbus_update = true;
# endif
-# ifdef CONFIG_EXAMPLES_FOC_VEL_ADC
+# ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
/* Get raw VEL */
- vel_raw = adc_sample[VEL_ADC_SAMPLE].am_data;
+ sp_raw = adc_sample[SETPOINT_ADC_SAMPLE].am_data;
- vel_update = true;
+ sp_update = true;
# endif
/* ADC trigger next cycle */
@@ -825,20 +831,20 @@ int main(int argc, char *argv[])
/* 3. Update motor velocity */
- if (vel_update == true)
+ if (sp_update == true)
{
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{
if (args.en & (1 << i))
{
- PRINTFV("Send velocity to %d\n", i);
+ PRINTFV("Send setpoint = %" PRIu32 "to %d\n", sp_raw, i);
- /* Send VELOCITY to threads */
+ /* Send setpoint to threads */
- ret = foc_vel_send(mqd[i], vel_raw);
+ ret = foc_setpoint_send(mqd[i], sp_raw);
if (ret < 0)
{
- PRINTF("ERROR: foc_vel_send failed %d\n", ret);
+ PRINTF("ERROR: foc_setpoint_send failed %d\n", ret);
goto errout;
}
}
@@ -846,7 +852,7 @@ int main(int argc, char *argv[])
/* Reset flag */
- vel_update = false;
+ sp_update = false;
}
/* 4. One time start */
diff --git a/examples/foc/foc_mq.c b/examples/foc/foc_mq.c
index a28116f..e336170 100644
--- a/examples/foc/foc_mq.c
+++ b/examples/foc/foc_mq.c
@@ -87,9 +87,9 @@ int foc_mq_handle(mqd_t mq, FAR struct foc_mq_s *h)
break;
}
- case CONTROL_MQ_MSG_VEL:
+ case CONTROL_MQ_MSG_SETPOINT:
{
- memcpy(&h->vel, &buffer[1], 4);
+ memcpy(&h->setpoint, &buffer[1], 4);
break;
}
diff --git a/examples/foc/foc_mq.h b/examples/foc/foc_mq.h
index 319cc94..4c94c93 100644
--- a/examples/foc/foc_mq.h
+++ b/examples/foc/foc_mq.h
@@ -50,7 +50,7 @@ enum foc_thr_msg_e
CONTROL_MQ_MSG_INVALID = 0,
CONTROL_MQ_MSG_VBUS = 1,
CONTROL_MQ_MSG_APPSTATE = 2,
- CONTROL_MQ_MSG_VEL = 3,
+ CONTROL_MQ_MSG_SETPOINT = 3,
CONTROL_MQ_MSG_START = 4,
CONTROL_MQ_MSG_KILL = 5
};
@@ -63,7 +63,7 @@ struct foc_mq_s
bool start;
int app_state;
uint32_t vbus;
- uint32_t vel;
+ uint32_t setpoint;
};
/****************************************************************************
