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

examples/foc: replace the requested velocity input with a more general setpoint input.

Browse Source 
This is the inital step to support torque/velocity/position control
This commit is contained in:
raiden00pl 2021-10-30 13:53:32 +02:00 committed by Xiang Xiao
parent 4001d27571
commit 2fb79db8b4
7 changed files with 78 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
examples/foc/Kconfig
View File

@ -146,33 +146,33 @@ config EXAMPLES_FOC_VBUS_SCALE
endif # EXAMPLES_FOC_VBUS_ADC endif # EXAMPLES_FOC_VBUS_ADC
choice choice
prompt "FOC velocity source" prompt "FOC setpoint source"
default EXAMPLES_FOC_VEL_CONST default EXAMPLES_FOC_SETPOINT_CONST
config EXAMPLES_FOC_VEL_CONST config EXAMPLES_FOC_SETPOINT_CONST
bool "Use hardcoded constant velocity value" bool "Use hardcoded constant setpiont value"
config EXAMPLES_FOC_VEL_ADC config EXAMPLES_FOC_SETPOINT_ADC
bool "Use ADC to control velocity" bool "Use ADC to control setpoint"
select EXAMPLES_FOC_HAVE_ADC 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 config EXAMPLES_FOC_SETPOINT_CONST_VALUE
int "FOC hardoced velocity value" int "FOC hardoced setpoint value"
default 100000 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 config EXAMPLES_FOC_SETPOINT_ADC_MAX
int "FOC maximum velocity from ADC [x1000]" int "FOC maximum setpoint from ADC [x1000]"
default 100000 default 0
endif # EXAMPLES_FOC_VEL_ADC endif # EXAMPLES_FOC_SETPOINT_ADC
config EXAMPLES_FOC_TIME_DEFAULT config EXAMPLES_FOC_TIME_DEFAULT
int "FOC run time default (sec)" int "FOC run time default (sec)"

26
examples/foc/foc_adc.h
View File

@ -40,8 +40,8 @@
/* Velocity source must be specified */ /* Velocity source must be specified */
#if defined(CONFIG_EXAMPLES_FOC_VEL_CONST) && \ #if defined(CONFIG_EXAMPLES_FOC_SETPOINT_CONST) && \
defined(CONFIG_EXAMPLES_FOC_VEL_ADC) defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC)
# error # error
#endif #endif
@ -57,15 +57,15 @@
/* Velocity ADC scale factor */ /* Velocity ADC scale factor */
#ifdef CONFIG_EXAMPLES_FOC_VEL_ADC #ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
# define VEL_ADC_SCALE (1.0f / CONFIG_EXAMPLES_FOC_ADC_MAX) # define SETPOINT_ADC_SCALE (1.0f / CONFIG_EXAMPLES_FOC_ADC_MAX)
#endif #endif
/* If constant velocity is selected, velocity value must be provided */ /* If constant velocity is selected, velocity value must be provided */
#ifdef CONFIG_EXAMPLES_FOC_VEL_CONST #ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST
# define VEL_ADC_SCALE (1) # define SETPOINT_ADC_SCALE (1)
# if CONFIG_EXAMPLES_FOC_VEL_CONST_VALUE == 0 # if CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE == 0
# error # error
# endif # endif
#endif #endif
@ -86,10 +86,10 @@
/* AUX ADC samples support */ /* AUX ADC samples support */
# if defined(CONFIG_EXAMPLES_FOC_VEL_ADC) && \ # if defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC) && \
defined(CONFIG_EXAMPLES_FOC_VBUS_ADC) defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
# define ADC_SAMPLES (2) # define ADC_SAMPLES (2)
# elif defined(CONFIG_EXAMPLES_FOC_VEL_ADC) || \ # elif defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC) || \
defined(CONFIG_EXAMPLES_FOC_VBUS_ADC) defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
# define ADC_SAMPLES (1) # define ADC_SAMPLES (1)
# else # else
@ -104,14 +104,14 @@
/* Numerate ADC samples */ /* Numerate ADC samples */
# if defined(CONFIG_EXAMPLES_FOC_VEL_ADC) && \ # if defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC) && \
defined(CONFIG_EXAMPLES_FOC_VBUS_ADC) defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
# define VBUS_ADC_SAMPLE (0) # define VBUS_ADC_SAMPLE (0)
# define VEL_ADC_SAMPLE (1) # define SETPOINT_ADC_SAMPLE (1)
# elif defined(CONFIG_EXAMPLES_FOC_VBUS_ADC) # elif defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
# define VBUS_ADC_SAMPLE (0) # define VBUS_ADC_SAMPLE (0)
# elif defined(CONFIG_EXAMPLES_FOC_VEL_ADC) # elif defined(CONFIG_EXAMPLES_FOC_SETPOINT_ADC)
# define VEL_ADC_SAMPLE (0) # define SETPOINT_ADC_SAMPLE (0)
# endif # endif
#endif /* CONFIG_EXAMPLES_FOC_HAVE_ADC */ #endif /* CONFIG_EXAMPLES_FOC_HAVE_ADC */

12
examples/foc/foc_fixed16_thr.c
View File

@ -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); 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); 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 */ /* 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", PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n",
handle->vel, motor->envp->id); handle->setpoint, motor->envp->id);
ret = foc_motor_vel(motor, handle->vel); ret = foc_motor_vel(motor, handle->setpoint);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_motor_vel failed %d!\n", ret); PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
goto errout; goto errout;
} }
motor->mq.vel = handle->vel; motor->mq.setpoint = handle->setpoint;
} }
/* Update motor state */ /* Update motor state */

17
examples/foc/foc_float_thr.c
View File

@ -435,7 +435,8 @@ static int foc_motor_vel(FAR struct foc_motor_f32_s *motor, uint32_t vel)
/* Update motor velocity destination */ /* 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; return OK;
} }
@ -771,21 +772,23 @@ static int foc_motor_handle(FAR struct foc_motor_f32_s *motor,
motor->mq.vbus = handle->vbus; 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", PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n",
handle->vel, motor->envp->id); handle->setpoint, motor->envp->id);
ret = foc_motor_vel(motor, handle->vel); ret = foc_motor_vel(motor, handle->setpoint);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_motor_vel failed %d!\n", ret); PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
goto errout; goto errout;
} }
motor->mq.vel = handle->vel; motor->mq.setpoint = handle->setpoint;
} }
/* Update motor state */ /* Update motor state */

46
examples/foc/foc_main.c
View File

@ -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_kp == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KP : args->pi_kp);
args->pi_ki = args->pi_ki =
(args->pi_ki == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KI : 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 =
(args->velmax == 0 ? CONFIG_EXAMPLES_FOC_VEL_ADC_MAX : args->velmax); (args->velmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_ADC_MAX : args->velmax);
#else #else
args->velmax = 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 #endif
args->state = args->state =
(args->state == 0 ? CONFIG_EXAMPLES_FOC_STATE_INIT : args->state); (args->state == 0 ? CONFIG_EXAMPLES_FOC_STATE_INIT : args->state);
args->en = (args->en == -1 ? INST_EN_DEAFULT : args->en); 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 #endif
uint32_t state = 0; uint32_t state = 0;
uint32_t vbus_raw = 0; uint32_t vbus_raw = 0;
int32_t vel_raw = 0; int32_t sp_raw = 0;
bool vbus_update = false; bool vbus_update = false;
bool state_update = false; bool state_update = false;
bool vel_update = false; bool sp_update = false;
bool terminate = false; bool terminate = false;
bool started = false; bool started = false;
int ret = OK; int ret = OK;
@ -639,9 +645,9 @@ int main(int argc, char *argv[])
vbus_update = true; vbus_update = true;
vbus_raw = VBUS_CONST_VALUE; vbus_raw = VBUS_CONST_VALUE;
#endif #endif
#ifndef CONFIG_EXAMPLES_FOC_VEL_ADC #ifndef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
vel_update = true; sp_update = true;
vel_raw = 1; sp_raw = 1;
#endif #endif
state_update = true; state_update = true;
@ -756,12 +762,12 @@ int main(int argc, char *argv[])
vbus_update = true; vbus_update = true;
# endif # endif
# ifdef CONFIG_EXAMPLES_FOC_VEL_ADC # ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
/* Get raw VEL */ /* 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 # endif
/* ADC trigger next cycle */ /* ADC trigger next cycle */
@ -825,20 +831,20 @@ int main(int argc, char *argv[])
/* 3. Update motor velocity */ /* 3. Update motor velocity */
if (vel_update == true) if (sp_update == true)
{ {
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{ {
if (args.en & (1 << i)) 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) if (ret < 0)
{ {
PRINTF("ERROR: foc_vel_send failed %d\n", ret); PRINTF("ERROR: foc_setpoint_send failed %d\n", ret);
goto errout; goto errout;
} }
} }
@ -846,7 +852,7 @@ int main(int argc, char *argv[])
/* Reset flag */ /* Reset flag */
vel_update = false; sp_update = false;
} }
/* 4. One time start */ /* 4. One time start */

4
examples/foc/foc_mq.c
View File

@ -87,9 +87,9 @@ int foc_mq_handle(mqd_t mq, FAR struct foc_mq_s *h)
break; break;
} }
case CONTROL_MQ_MSG_VEL: case CONTROL_MQ_MSG_SETPOINT:
{ {
memcpy(&h->vel, &buffer[1], 4); memcpy(&h->setpoint, &buffer[1], 4);
break; break;
} }

4
examples/foc/foc_mq.h
View File

@ -50,7 +50,7 @@ enum foc_thr_msg_e
CONTROL_MQ_MSG_INVALID = 0, CONTROL_MQ_MSG_INVALID = 0,
CONTROL_MQ_MSG_VBUS = 1, CONTROL_MQ_MSG_VBUS = 1,
CONTROL_MQ_MSG_APPSTATE = 2, CONTROL_MQ_MSG_APPSTATE = 2,
CONTROL_MQ_MSG_VEL = 3, CONTROL_MQ_MSG_SETPOINT = 3,
CONTROL_MQ_MSG_START = 4, CONTROL_MQ_MSG_START = 4,
CONTROL_MQ_MSG_KILL = 5 CONTROL_MQ_MSG_KILL = 5
}; };
@ -63,7 +63,7 @@ struct foc_mq_s
bool start; bool start;
int app_state; int app_state;
uint32_t vbus; uint32_t vbus;
uint32_t vel; uint32_t setpoint;
}; };
/**************************************************************************** /****************************************************************************