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

examples/foc: move adc and button interfaces logic to a separate file

Browse Source
This commit is contained in:
raiden00pl 2021-12-05 16:19:15 +01:00 committed by Xiang Xiao
parent ea27aacbd2
commit 37aa562f03
5 changed files with 620 additions and 219 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
examples/foc/Kconfig
View File

@ -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)"

2
examples/foc/Makefile
View File

@ -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

485
examples/foc/foc_intf.c Normal file
View File

@ -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;
}

60
examples/foc/foc_intf.h Normal file
View File

@ -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 */

279
examples/foc/foc_main.c
View File

@ -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
args->torqmax =
(args->torqmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_ADC_MAX : args->torqmax);
#else
#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST
args->torqmax =
(args->torqmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->torqmax);
#else
args->torqmax =
(args->torqmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_MAX : args->torqmax);
#endif
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
args->velmax =
(args->velmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_ADC_MAX : args->velmax);
#else
#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST
args->velmax =
(args->velmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->velmax);
#else
args->velmax =
(args->velmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_MAX : args->velmax);
#endif
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
args->posmax =
(args->posmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_ADC_MAX : args->posmax);
#else
#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST
args->posmax =
(args->posmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->posmax);
#else
args->posmax =
(args->posmax == 0 ?
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 */
/* Initialize control interface */
adc_fd = open(CONFIG_EXAMPLES_FOC_ADC_DEVPATH, (O_RDONLY | O_NONBLOCK));
if (adc_fd <= 0)
ret = foc_intf_init();
if (ret < 0)
{
PRINTF("ERROR: failed to open %s %d\n",
CONFIG_EXAMPLES_FOC_ADC_DEVPATH, errno);
ret = -errno;
PRINTF("ERROR: failed to initialize control interface %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
/* Open button driver */
b_fd = open(CONFIG_EXAMPLES_FOC_BUTTON_DEVPATH, (O_RDONLY | O_NONBLOCK));
if (b_fd < 0)
{
PRINTF("ERROR: failed to open %s %d\n",
CONFIG_EXAMPLES_FOC_BUTTON_DEVPATH, errno);
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();