You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by xi...@apache.org on 2021/12/07 13:52:58 UTC
[incubator-nuttx-apps] 04/04: examples/foc: move adc and button interfaces logic to a separate file
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
commit 37aa562f0322560b3d437cfe4fe681b28548337c
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sun Dec 5 16:19:15 2021 +0100
examples/foc: move adc and button interfaces logic to a separate file
---
examples/foc/Kconfig | 13 +-
examples/foc/Makefile | 2 +-
examples/foc/foc_intf.c | 485 ++++++++++++++++++++++++++++++++++++++++++++++++
examples/foc/foc_intf.h | 60 ++++++
examples/foc/foc_main.c | 267 ++++++--------------------
5 files changed, 614 insertions(+), 213 deletions(-)
diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index e0cf16d..b8a69ee 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -216,9 +216,14 @@ config EXAMPLES_FOC_SETPOINT_CONST
config EXAMPLES_FOC_SETPOINT_ADC
bool "Use ADC to control setpoint"
select EXAMPLES_FOC_HAVE_ADC
+ select EXAMPLES_FOC_HAVE_SETPOINT_VAR
endchoice # FOC setpoint interface
+config EXAMPLES_FOC_HAVE_SETPOINT_VAR
+ bool
+ default n
+
if EXAMPLES_FOC_SETPOINT_CONST
config EXAMPLES_FOC_SETPOINT_CONST_VALUE
@@ -227,13 +232,13 @@ config EXAMPLES_FOC_SETPOINT_CONST_VALUE
endif # EXAMPLES_FOC_SETPOINT_CONST
-if EXAMPLES_FOC_SETPOINT_ADC
+if EXAMPLES_FOC_HAVE_SETPOINT_VAR
-config EXAMPLES_FOC_SETPOINT_ADC_MAX
- int "FOC maximum setpoint from ADC [x1000]"
+config EXAMPLES_FOC_SETPOINT_MAX
+ int "FOC maximum setpoint [x1000]"
default 0
-endif # EXAMPLES_FOC_SETPOINT_ADC
+endif # EXAMPLES_FOC_HAVE_SETPOINT_VAR
config EXAMPLES_FOC_TIME_DEFAULT
int "FOC run time default (sec)"
diff --git a/examples/foc/Makefile b/examples/foc/Makefile
index 6b2b791..68b0d61 100644
--- a/examples/foc/Makefile
+++ b/examples/foc/Makefile
@@ -32,7 +32,7 @@ MODULE = $(CONFIG_EXAMPLES_FOC)
MAINSRC = foc_main.c
ASRCS =
-CSRCS = foc_device.c foc_mq.c foc_thr.c
+CSRCS = foc_device.c foc_mq.c foc_thr.c foc_intf.c
ifeq ($(CONFIG_BUILTIN),y)
CSRCS += foc_parseargs.c
diff --git a/examples/foc/foc_intf.c b/examples/foc/foc_intf.c
new file mode 100644
index 0000000..7bc0581
--- /dev/null
+++ b/examples/foc/foc_intf.c
@@ -0,0 +1,485 @@
+/****************************************************************************
+ * apps/examples/foc/foc_intf.c
+ *
+ * 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 <fcntl.h>
+#include <assert.h>
+#include <errno.h>
+
+#include <sys/ioctl.h>
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+# include <nuttx/input/buttons.h>
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+# include <nuttx/analog/adc.h>
+# include <nuttx/analog/ioctl.h>
+#endif
+
+#include "foc_thr.h"
+#include "foc_adc.h"
+#include "foc_intf.h"
+#include "foc_debug.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#if defined(CONFIG_EXAMPLES_FOC_HAVE_ADC) || \
+ defined(CONFIG_EXAMPLES_FOC_HAVE_BUTTON)
+# define FOC_HAVE_INTF
+#endif
+
+/* Button init state */
+
+#if CONFIG_EXAMPLES_FOC_STATE_INIT == 1
+# define STATE_BUTTON_I (0)
+#elif CONFIG_EXAMPLES_FOC_STATE_INIT == 2
+# define STATE_BUTTON_I (2)
+#elif CONFIG_EXAMPLES_FOC_STATE_INIT == 3
+# define STATE_BUTTON_I (1)
+#elif CONFIG_EXAMPLES_FOC_STATE_INIT == 4
+# define STATE_BUTTON_I (3)
+#else
+# error
+#endif
+
+/****************************************************************************
+ * Private Type Definition
+ ****************************************************************************/
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+/* Button interface data */
+
+struct foc_intf_btn_s
+{
+ int fd;
+};
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+/* ADC interface data */
+
+struct foc_intf_adc_s
+{
+ int fd;
+ bool adc_trigger;
+};
+#endif
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+/* Example state */
+
+static const int g_state_list[5] =
+{
+ FOC_EXAMPLE_STATE_FREE,
+ FOC_EXAMPLE_STATE_CW,
+ FOC_EXAMPLE_STATE_STOP,
+ FOC_EXAMPLE_STATE_CCW,
+ 0
+};
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+/* Button interface data */
+
+static struct foc_intf_btn_s g_btn_intf;
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+/* ADC interface data */
+
+static struct foc_intf_adc_s g_adc_intf;
+#endif
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+/****************************************************************************
+ * Name: foc_button_init
+ ****************************************************************************/
+
+static int foc_button_init(FAR struct foc_intf_btn_s *intf)
+{
+ int ret = 0;
+
+ DEBUGASSERT(intf);
+
+ /* Open button driver */
+
+ intf->fd = open(CONFIG_EXAMPLES_FOC_BUTTON_DEVPATH,
+ (O_RDONLY | O_NONBLOCK));
+ if (intf->fd < 0)
+ {
+ PRINTF("ERROR: failed to open %s %d\n",
+ CONFIG_EXAMPLES_FOC_BUTTON_DEVPATH, errno);
+ ret = -errno;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_button_deinit
+ ****************************************************************************/
+
+static int foc_button_deinit(FAR struct foc_intf_btn_s *intf)
+{
+ DEBUGASSERT(intf);
+
+ if (intf->fd > 0)
+ {
+ close(intf->fd);
+ }
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_button_update
+ ****************************************************************************/
+
+static int foc_button_update(FAR struct foc_intf_btn_s *intf,
+ FAR struct foc_intf_data_s *data)
+{
+ static int state_i = STATE_BUTTON_I;
+ btn_buttonset_t b_sample = 0;
+ int ret = OK;
+
+ DEBUGASSERT(intf);
+ DEBUGASSERT(data);
+
+ /* Get button state */
+
+ ret = read(intf->fd, &b_sample, sizeof(btn_buttonset_t));
+ if (ret < 0)
+ {
+ if (errno != EAGAIN)
+ {
+ PRINTF("ERROR: read button failed %d\n", errno);
+ ret = -errno;
+ goto errout;
+ }
+ else
+ {
+ ret = OK;
+ }
+ }
+
+ /* Next state */
+
+ if (b_sample & (1 << 0))
+ {
+ state_i += 1;
+
+ if (g_state_list[state_i] == 0)
+ {
+ state_i = 0;
+ }
+
+ data->state = g_state_list[state_i];
+ data->state_update = true;
+
+ PRINTF("BUTTON STATE %" PRIu32 "\n", data->state);
+ }
+
+errout:
+ return ret;
+}
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+/****************************************************************************
+ * Name: foc_adc_init
+ ****************************************************************************/
+
+static int foc_adc_init(FAR struct foc_intf_adc_s *intf)
+{
+ int ret = OK;
+
+ DEBUGASSERT(intf);
+
+ /* Open ADC device */
+
+ intf->fd = open(CONFIG_EXAMPLES_FOC_ADC_DEVPATH, (O_RDONLY | O_NONBLOCK));
+ if (intf->fd <= 0)
+ {
+ PRINTF("ERROR: failed to open %s %d\n",
+ CONFIG_EXAMPLES_FOC_ADC_DEVPATH, errno);
+ ret = -errno;
+ goto errout;
+ }
+
+ /* Initial ADC trigger */
+
+ ret = ioctl(intf->fd, ANIOC_TRIGGER, 0);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: ANIOC_TRIGGER ioctl failed: %d\n", errno);
+ ret = -errno;
+ goto errout;
+ }
+
+ /* Make sure that conversion is done before first read form ADC device */
+
+ usleep(10000);
+
+ /* Read ADC data if the first loop cylce */
+
+ intf->adc_trigger = false;
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_adc_deinit
+ ****************************************************************************/
+
+static int foc_adc_deinit(FAR struct foc_intf_adc_s *intf)
+{
+ DEBUGASSERT(intf);
+
+ if (intf->fd > 0)
+ {
+ close(intf->fd);
+ }
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_adc_update
+ ****************************************************************************/
+
+static int foc_adc_update(FAR struct foc_intf_adc_s *intf,
+ FAR struct foc_intf_data_s *data)
+{
+ struct adc_msg_s adc_sample[ADC_SAMPLES];
+ int ret = OK;
+
+ DEBUGASSERT(intf);
+ DEBUGASSERT(data);
+
+ if (intf->adc_trigger == true)
+ {
+ /* Issue the software trigger to start ADC conversion */
+
+ ret = ioctl(intf->fd, ANIOC_TRIGGER, 0);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: ANIOC_TRIGGER ioctl failed: %d\n", errno);
+ ret = -errno;
+ goto errout;
+ }
+
+ /* No ADC trigger next cycle */
+
+ intf->adc_trigger = false;
+ }
+ else
+ {
+ /* Get ADC samples */
+
+ ret = read(intf->fd, adc_sample,
+ (ADC_SAMPLES * sizeof(struct adc_msg_s)));
+ if (ret < 0)
+ {
+ if (errno != EAGAIN)
+ {
+ PRINTF("ERROR: adc read failed %d\n", errno);
+ ret = -errno;
+ goto errout;
+ }
+ else
+ {
+ ret = OK;
+ }
+ }
+ else
+ {
+ /* Verify we have received the configured number of samples */
+
+ if (ret != ADC_SAMPLES * sizeof(struct adc_msg_s))
+ {
+ PRINTF("ERROR: adc read invalid read %d != %d\n", ret,
+ ADC_SAMPLES * sizeof(struct adc_msg_s));
+ ret = -EINVAL;
+ goto errout;
+ }
+
+#ifdef CONFIG_EXAMPLES_FOC_VBUS_ADC
+ /* Get raw VBUS */
+
+ data->vbus_raw = adc_sample[VBUS_ADC_SAMPLE].am_data;
+
+ data->vbus_update = true;
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
+ /* Get raw VEL */
+
+ data->sp_raw = adc_sample[SETPOINT_ADC_SAMPLE].am_data;
+
+ data->sp_update = true;
+#endif
+
+ /* ADC trigger next cycle */
+
+ intf->adc_trigger = true;
+ }
+ }
+
+errout:
+ return ret;
+}
+#endif
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_intf_init
+ ****************************************************************************/
+
+int foc_intf_init(void)
+{
+ int ret = OK;
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+ /* Initialize ADC interface */
+
+ ret = foc_adc_init(&g_adc_intf);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: failed to initialize adc interface %d\n", ret);
+ goto errout;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+ /* Initialize button interface */
+
+ ret = foc_button_init(&g_btn_intf);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: failed to initialize button interface %d\n", ret);
+ goto errout;
+ }
+#endif
+
+#ifdef FOC_HAVE_INTF
+ errout:
+#endif
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_intf_deinit
+ ****************************************************************************/
+
+int foc_intf_deinit(void)
+{
+ int ret = OK;
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+ /* De-initialize adc interface */
+
+ ret = foc_adc_deinit(&g_adc_intf);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_adc_deinit failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+ /* De-initialize button interface */
+
+ ret = foc_button_deinit(&g_btn_intf);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_button_deinit failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
+#ifdef FOC_HAVE_INTF
+ errout:
+#endif
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_intf_update
+ ****************************************************************************/
+
+int foc_intf_update(FAR struct foc_intf_data_s *data)
+{
+ int ret = OK;
+
+ DEBUGASSERT(data);
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+ /* Update button */
+
+ ret = foc_button_update(&g_btn_intf, data);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: button update failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+ /* Update ADC */
+
+ ret = foc_adc_update(&g_adc_intf, data);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: adc update failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
+#ifdef FOC_HAVE_INTF
+ errout:
+#endif
+
+ return ret;
+}
diff --git a/examples/foc/foc_intf.h b/examples/foc/foc_intf.h
new file mode 100644
index 0000000..fb9e7e0
--- /dev/null
+++ b/examples/foc/foc_intf.h
@@ -0,0 +1,60 @@
+/****************************************************************************
+ * apps/examples/foc/foc_intf.h
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef __EXAMPLES_FOC_FOC_INTF_H
+#define __EXAMPLES_FOC_FOC_INTF_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* FOC control interface data */
+
+struct foc_intf_data_s
+{
+ uint32_t state;
+ uint32_t vbus_raw;
+ int32_t sp_raw;
+ bool vbus_update;
+ bool state_update;
+ bool sp_update;
+ bool terminate;
+ bool started;
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+int foc_intf_init(void);
+int foc_intf_deinit(void);
+int foc_intf_update(FAR struct foc_intf_data_s *data);
+
+#endif /* __EXAMPLES_FOC_FOC_INTF_H */
diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c
index fd1deec..8dbedd0 100644
--- a/examples/foc/foc_main.c
+++ b/examples/foc/foc_main.c
@@ -39,15 +39,7 @@
#include "foc_cfg.h"
#include "foc_debug.h"
#include "foc_parseargs.h"
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
-# include <nuttx/input/buttons.h>
-#endif
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
-# include <nuttx/analog/adc.h>
-# include <nuttx/analog/ioctl.h>
-#endif
+#include "foc_intf.h"
#include "industry/foc/foc_common.h"
@@ -59,20 +51,6 @@
#define MAIN_LOOP_USLEEP (200000)
-/* Button init state */
-
-#if CONFIG_EXAMPLES_FOC_STATE_INIT == 1
-# define STATE_BUTTON_I (0)
-#elif CONFIG_EXAMPLES_FOC_STATE_INIT == 2
-# define STATE_BUTTON_I (2)
-#elif CONFIG_EXAMPLES_FOC_STATE_INIT == 3
-# define STATE_BUTTON_I (1)
-#elif CONFIG_EXAMPLES_FOC_STATE_INIT == 4
-# define STATE_BUTTON_I (3)
-#else
-# error
-#endif
-
/* Enabled instnaces default state */
#define INST_EN_DEAFULT (0xff)
@@ -105,36 +83,36 @@ static void init_args(FAR struct args_s *args)
/* Setpoint configuration */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
-#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
+#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST
args->torqmax =
(args->torqmax == 0 ?
- CONFIG_EXAMPLES_FOC_SETPOINT_ADC_MAX : args->torqmax);
+ CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->torqmax);
#else
args->torqmax =
(args->torqmax == 0 ?
- CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->torqmax);
+ CONFIG_EXAMPLES_FOC_SETPOINT_MAX : args->torqmax);
#endif
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
-#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
+#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST
args->velmax =
(args->velmax == 0 ?
- CONFIG_EXAMPLES_FOC_SETPOINT_ADC_MAX : args->velmax);
+ CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->velmax);
#else
args->velmax =
(args->velmax == 0 ?
- CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->velmax);
+ CONFIG_EXAMPLES_FOC_SETPOINT_MAX : args->velmax);
#endif
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
-#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
+#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST
args->posmax =
(args->posmax == 0 ?
- CONFIG_EXAMPLES_FOC_SETPOINT_ADC_MAX : args->posmax);
+ CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->posmax);
#else
args->posmax =
(args->posmax == 0 ?
- CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->posmax);
+ CONFIG_EXAMPLES_FOC_SETPOINT_MAX : args->posmax);
#endif
#endif
@@ -309,24 +287,7 @@ int main(int argc, char *argv[])
pthread_t threads[CONFIG_MOTOR_FOC_INST];
mqd_t mqd[CONFIG_MOTOR_FOC_INST];
struct args_s args;
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
- int adc_fd = 0;
- bool adc_trigger = false;
- struct adc_msg_s adc_sample[ADC_SAMPLES];
-#endif
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
- btn_buttonset_t b_sample = 0;
- int b_fd = 0;
- int state_i = 0;
-#endif
- uint32_t state = 0;
- uint32_t vbus_raw = 0;
- int32_t sp_raw = 0;
- bool vbus_update = false;
- bool state_update = false;
- bool sp_update = false;
- bool terminate = false;
- bool started = false;
+ struct foc_intf_data_s data;
int ret = OK;
int i = 0;
int time = 0;
@@ -337,6 +298,7 @@ int main(int argc, char *argv[])
memset(mqd, 0, sizeof(mqd_t) * CONFIG_MOTOR_FOC_INST);
memset(foc, 0, sizeof(struct foc_ctrl_env_s) * CONFIG_MOTOR_FOC_INST);
memset(threads, 0, sizeof(pthread_t) * CONFIG_MOTOR_FOC_INST);
+ memset(&data, 0, sizeof(struct foc_intf_data_s));
/* Initialize args before parse */
@@ -384,31 +346,14 @@ int main(int argc, char *argv[])
goto errout_no_mutex;
}
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
- /* Open ADC */
-
- adc_fd = open(CONFIG_EXAMPLES_FOC_ADC_DEVPATH, (O_RDONLY | O_NONBLOCK));
- if (adc_fd <= 0)
- {
- PRINTF("ERROR: failed to open %s %d\n",
- CONFIG_EXAMPLES_FOC_ADC_DEVPATH, errno);
-
- ret = -errno;
- goto errout;
- }
-#endif
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
- /* Open button driver */
+ /* Initialize control interface */
- b_fd = open(CONFIG_EXAMPLES_FOC_BUTTON_DEVPATH, (O_RDONLY | O_NONBLOCK));
- if (b_fd < 0)
+ ret = foc_intf_init();
+ if (ret < 0)
{
- PRINTF("ERROR: failed to open %s %d\n",
- CONFIG_EXAMPLES_FOC_BUTTON_DEVPATH, errno);
+ PRINTF("ERROR: failed to initialize control interface %d\n", ret);
goto errout;
}
-#endif
/* Initialzie FOC controllers */
@@ -450,147 +395,40 @@ int main(int argc, char *argv[])
usleep(10000);
- /* Initial update for VBUS and VEL */
+ /* Initial update for VBUS and SETPOINT */
#ifndef CONFIG_EXAMPLES_FOC_VBUS_ADC
- vbus_update = true;
- vbus_raw = VBUS_CONST_VALUE;
+ data.vbus_update = true;
+ data.vbus_raw = VBUS_CONST_VALUE;
#endif
#ifndef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
- sp_update = true;
- sp_raw = 1;
-#endif
- state_update = true;
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
- /* Initial ADC trigger */
-
- ret = ioctl(adc_fd, ANIOC_TRIGGER, 0);
- if (ret < 0)
- {
- PRINTF("ERROR: ANIOC_TRIGGER ioctl failed: %d\n", errno);
- goto errout;
- }
-
- /* Make sure that conversion is done before first read form ADC device */
-
- usleep(10000);
-
- /* Read ADC data if the first loop cylce */
-
- adc_trigger = false;
+ data.sp_update = true;
+ data.sp_raw = 1;
#endif
+ data.state_update = true;
/* Controller state */
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
- state_i = STATE_BUTTON_I;
-#endif
- state = args.state;
+ data.state = args.state;
/* Auxliary control loop */
- while (terminate != true)
+ while (data.terminate != true)
{
PRINTFV("foc_main loop %d\n", time);
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
- /* Get button state */
+ /* Update control interface */
- ret = read(b_fd, &b_sample, sizeof(btn_buttonset_t));
+ ret = foc_intf_update(&data);
if (ret < 0)
{
- if (errno != EAGAIN)
- {
- PRINTF("ERROR: read button failed %d\n", errno);
- }
+ PRINTF("ERROR: foc_intf_update failed: %d\n", ret);
+ goto errout;
}
- /* Next state */
-
- if (b_sample & (1 << 0))
- {
- state_i += 1;
-
- if (g_state_list[state_i] == 0)
- {
- state_i = 0;
- }
-
- state = g_state_list[state_i];
- state_update = true;
-
- PRINTF("BUTTON STATE %" PRIu32 "\n", state);
- }
-#endif
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
- if (adc_trigger == true)
- {
- /* Issue the software trigger to start ADC conversion */
-
- ret = ioctl(adc_fd, ANIOC_TRIGGER, 0);
- if (ret < 0)
- {
- PRINTF("ERROR: ANIOC_TRIGGER ioctl failed: %d\n", errno);
- goto errout;
- }
-
- /* No ADC trigger next cycle */
-
- adc_trigger = false;
- }
- else
- {
- /* Get ADC samples */
-
- ret = read(adc_fd, adc_sample,
- (ADC_SAMPLES * sizeof(struct adc_msg_s)));
- if (ret < 0)
- {
- if (errno != EAGAIN)
- {
- PRINTF("ERROR: adc read failed %d\n", errno);
- }
- }
- else
- {
- /* Verify we have received the configured number of samples */
-
- if (ret != ADC_SAMPLES * sizeof(struct adc_msg_s))
- {
- PRINTF("ERROR: adc read invalid read %d != %d\n",
- ret, ADC_SAMPLES * sizeof(struct adc_msg_s));
- ret = -EINVAL;
- goto errout;
- }
-
-# ifdef CONFIG_EXAMPLES_FOC_VBUS_ADC
- /* Get raw VBUS */
-
- vbus_raw = adc_sample[VBUS_ADC_SAMPLE].am_data;
-
- vbus_update = true;
-# endif
-
-# ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
- /* Get raw VEL */
-
- sp_raw = adc_sample[SETPOINT_ADC_SAMPLE].am_data;
-
- sp_update = true;
-# endif
-
- /* ADC trigger next cycle */
-
- adc_trigger = true;
- }
- }
-#endif
-
/* 1. Update VBUS */
- if (vbus_update == true)
+ if (data.vbus_update == true)
{
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{
@@ -600,7 +438,7 @@ int main(int argc, char *argv[])
/* Send VBUS to thread */
- ret = foc_vbus_send(mqd[i], vbus_raw);
+ ret = foc_vbus_send(mqd[i], data.vbus_raw);
if (ret < 0)
{
PRINTF("ERROR: foc_vbus_send failed %d\n", ret);
@@ -611,22 +449,22 @@ int main(int argc, char *argv[])
/* Reset flag */
- vbus_update = false;
+ data.vbus_update = false;
}
/* 2. Update motor state */
- if (state_update == true)
+ if (data.state_update == true)
{
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{
if (args.en & (1 << i))
{
- PRINTFV("Send state %" PRIu32 " to %d\n", state, i);
+ PRINTFV("Send state %" PRIu32 " to %d\n", data.state, i);
/* Send STATE to thread */
- ret = foc_state_send(mqd[i], state);
+ ret = foc_state_send(mqd[i], data.state);
if (ret < 0)
{
PRINTF("ERROR: foc_state_send failed %d\n", ret);
@@ -637,22 +475,23 @@ int main(int argc, char *argv[])
/* Reset flag */
- state_update = false;
+ data.state_update = false;
}
/* 3. Update motor velocity */
- if (sp_update == true)
+ if (data.sp_update == true)
{
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{
if (args.en & (1 << i))
{
- PRINTFV("Send setpoint = %" PRIu32 "to %d\n", sp_raw, i);
+ PRINTFV("Send setpoint = %" PRIu32 "to %d\n",
+ data.sp_raw, i);
/* Send setpoint to threads */
- ret = foc_setpoint_send(mqd[i], sp_raw);
+ ret = foc_setpoint_send(mqd[i], data.sp_raw);
if (ret < 0)
{
PRINTF("ERROR: foc_setpoint_send failed %d\n", ret);
@@ -663,12 +502,12 @@ int main(int argc, char *argv[])
/* Reset flag */
- sp_update = false;
+ data.sp_update = false;
}
/* 4. One time start */
- if (started == false)
+ if (data.started == false)
{
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{
@@ -689,7 +528,7 @@ int main(int argc, char *argv[])
/* Set flag */
- started = true;
+ data.started = true;
}
/* Handle run time */
@@ -702,7 +541,7 @@ int main(int argc, char *argv[])
{
/* Exit loop */
- terminate = true;
+ data.terminate = true;
}
}
@@ -710,7 +549,7 @@ int main(int argc, char *argv[])
if (foc_threads_terminated() == true)
{
- terminate = true;
+ data.terminate = true;
}
usleep(MAIN_LOOP_USLEEP);
@@ -737,11 +576,14 @@ errout:
}
}
- /* Wait some time */
+ /* Wait for threads termination */
- usleep(100000);
+ while (foc_threads_terminated() == false)
+ {
+ usleep(100000);
+ }
- /* De-initialize all FOC control threads */
+ /* De-initialize all mq */
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{
@@ -756,6 +598,15 @@ errout:
}
}
+ /* De-initialize control interface */
+
+ ret = foc_intf_deinit();
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_inf_deinit failed %d\n", ret);
+ goto errout;
+ }
+
errout_no_mutex:
foc_threads_deinit();