631 lines
16 KiB
C
631 lines
16 KiB
C
/****************************************************************************
|
|
* apps/examples/foc/foc_main.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 <unistd.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
#include <errno.h>
|
|
|
|
#include <sys/types.h>
|
|
#include <sys/boardctl.h>
|
|
|
|
#include "foc_mq.h"
|
|
#include "foc_thr.h"
|
|
#include "foc_cfg.h"
|
|
#include "foc_debug.h"
|
|
#include "foc_parseargs.h"
|
|
#include "foc_intf.h"
|
|
|
|
#include "industry/foc/foc_common.h"
|
|
|
|
/****************************************************************************
|
|
* Pre-processor Definitions
|
|
****************************************************************************/
|
|
|
|
/* Main loop sleep */
|
|
|
|
#define MAIN_LOOP_USLEEP (200000)
|
|
|
|
/* Enabled instances default state */
|
|
|
|
#define INST_EN_DEFAULT (0xff)
|
|
|
|
/****************************************************************************
|
|
* Private Functions
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Name: init_args
|
|
****************************************************************************/
|
|
|
|
static void init_args(FAR struct args_s *args)
|
|
{
|
|
args->time =
|
|
(args->time == 0 ? CONFIG_EXAMPLES_FOC_TIME_DEFAULT : args->time);
|
|
args->fmode =
|
|
(args->fmode == 0 ? CONFIG_EXAMPLES_FOC_FMODE : args->fmode);
|
|
args->mmode =
|
|
(args->mmode == 0 ? CONFIG_EXAMPLES_FOC_MMODE : args->mmode);
|
|
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
|
|
args->qparam =
|
|
(args->qparam == 0 ? CONFIG_EXAMPLES_FOC_OPENLOOP_Q : args->qparam);
|
|
#endif
|
|
|
|
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
|
|
args->foc_pi_kp =
|
|
(args->foc_pi_kp == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KP : args->foc_pi_kp);
|
|
args->foc_pi_ki =
|
|
(args->foc_pi_ki == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KI : args->foc_pi_ki);
|
|
#endif
|
|
|
|
/* Setpoint configuration */
|
|
|
|
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
|
|
#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_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_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
|
|
|
|
args->state =
|
|
(args->state == 0 ? CONFIG_EXAMPLES_FOC_STATE_INIT : args->state);
|
|
args->en = (args->en == -1 ? INST_EN_DEFAULT : args->en);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: validate_args
|
|
****************************************************************************/
|
|
|
|
static int validate_args(FAR struct args_s *args)
|
|
{
|
|
int ret = -EINVAL;
|
|
|
|
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
|
|
/* Current PI controller */
|
|
|
|
if (args->foc_pi_kp == 0 && args->foc_pi_ki == 0)
|
|
{
|
|
PRINTF("ERROR: missing FOC Kp/Ki configuration\n");
|
|
goto errout;
|
|
}
|
|
#endif
|
|
|
|
/* FOC operation mode */
|
|
|
|
if (args->fmode != FOC_FMODE_IDLE &&
|
|
args->fmode != FOC_FMODE_VOLTAGE &&
|
|
args->fmode != FOC_FMODE_CURRENT)
|
|
{
|
|
PRINTF("Invalid op mode value %d s\n", args->fmode);
|
|
goto errout;
|
|
}
|
|
|
|
/* Example control mode */
|
|
|
|
if (
|
|
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
|
|
args->mmode != FOC_MMODE_TORQ &&
|
|
#endif
|
|
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
|
|
args->mmode != FOC_MMODE_VEL &&
|
|
#endif
|
|
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
|
|
args->mmode != FOC_MMODE_POS &&
|
|
#endif
|
|
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
|
|
args->mmode != FOC_MMODE_ALIGN_ONLY &&
|
|
#endif
|
|
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
|
|
args->mmode != FOC_MMODE_IDENT_ONLY &&
|
|
#endif
|
|
1)
|
|
{
|
|
PRINTF("Invalid ctrl mode value %d s\n", args->mmode);
|
|
goto errout;
|
|
}
|
|
|
|
/* Example state */
|
|
|
|
if (args->state != FOC_EXAMPLE_STATE_FREE &&
|
|
args->state != FOC_EXAMPLE_STATE_STOP &&
|
|
args->state != FOC_EXAMPLE_STATE_CW &&
|
|
args->state != FOC_EXAMPLE_STATE_CCW)
|
|
{
|
|
PRINTF("Invalid state value %d s\n", args->state);
|
|
goto errout;
|
|
}
|
|
|
|
/* Time parameter */
|
|
|
|
if (args->time <= 0 && args->time != -1)
|
|
{
|
|
PRINTF("Invalid time value %d s\n", args->time);
|
|
goto errout;
|
|
}
|
|
|
|
/* Otherwise OK */
|
|
|
|
ret = OK;
|
|
|
|
errout:
|
|
return ret;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: foc_mq_send
|
|
****************************************************************************/
|
|
|
|
static int foc_mq_send(mqd_t mqd, uint8_t msg, FAR void *data)
|
|
{
|
|
int ret = OK;
|
|
uint8_t buffer[5];
|
|
uint32_t tmp = 0;
|
|
|
|
DEBUGASSERT(data);
|
|
|
|
/* Data max 4B */
|
|
|
|
tmp = *((FAR uint32_t *) data);
|
|
|
|
buffer[0] = msg;
|
|
buffer[1] = ((tmp & 0x000000ff) >> 0);
|
|
buffer[2] = ((tmp & 0x0000ff00) >> 8);
|
|
buffer[3] = ((tmp & 0x00ff0000) >> 16);
|
|
buffer[4] = ((tmp & 0xff000000) >> 24);
|
|
|
|
ret = mq_send(mqd, (FAR char *)buffer, 5, 42);
|
|
if (ret < 0)
|
|
{
|
|
PRINTF("foc_main: mq_send failed %d\n", errno);
|
|
ret = -errno;
|
|
goto errout;
|
|
}
|
|
|
|
errout:
|
|
return ret;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: foc_vbus_send
|
|
****************************************************************************/
|
|
|
|
static int foc_vbus_send(mqd_t mqd, uint32_t vbus)
|
|
{
|
|
return foc_mq_send(mqd, CONTROL_MQ_MSG_VBUS, (FAR void *)&vbus);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: foc_setpoint_send
|
|
****************************************************************************/
|
|
|
|
static int foc_setpoint_send(mqd_t mqd, uint32_t setpoint)
|
|
{
|
|
return foc_mq_send(mqd, CONTROL_MQ_MSG_SETPOINT, (FAR void *)&setpoint);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: foc_state_send
|
|
****************************************************************************/
|
|
|
|
static int foc_state_send(mqd_t mqd, uint32_t state)
|
|
{
|
|
return foc_mq_send(mqd, CONTROL_MQ_MSG_APPSTATE, (FAR void *)&state);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: foc_start_send
|
|
****************************************************************************/
|
|
|
|
static int foc_start_send(mqd_t mqd)
|
|
{
|
|
int tmp = 0;
|
|
return foc_mq_send(mqd, CONTROL_MQ_MSG_START, (FAR void *)&tmp);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: foc_kill_send
|
|
****************************************************************************/
|
|
|
|
static int foc_kill_send(mqd_t mqd)
|
|
{
|
|
int tmp = 0;
|
|
return foc_mq_send(mqd, CONTROL_MQ_MSG_KILL, (FAR void *)&tmp);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Public Functions
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Name: main
|
|
****************************************************************************/
|
|
|
|
int main(int argc, char *argv[])
|
|
{
|
|
struct foc_ctrl_env_s foc[CONFIG_MOTOR_FOC_INST];
|
|
pthread_t threads[CONFIG_MOTOR_FOC_INST];
|
|
mqd_t mqd[CONFIG_MOTOR_FOC_INST];
|
|
struct args_s args;
|
|
struct foc_intf_data_s data;
|
|
int ret = OK;
|
|
int i = 0;
|
|
int time = 0;
|
|
|
|
/* Reset some data */
|
|
|
|
memset(&args, 0, sizeof(struct args_s));
|
|
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 */
|
|
|
|
args.en = -1;
|
|
|
|
#ifdef CONFIG_BUILTIN
|
|
/* Parse the command line */
|
|
|
|
parse_args(&args, argc, argv);
|
|
#endif
|
|
|
|
/* Initialize args */
|
|
|
|
init_args(&args);
|
|
|
|
/* Validate arguments */
|
|
|
|
ret = validate_args(&args);
|
|
if (ret < 0)
|
|
{
|
|
PRINTF("ERROR: validate args failed\n");
|
|
goto errout_no_mutex;
|
|
}
|
|
|
|
#ifndef CONFIG_NSH_ARCHINIT
|
|
/* Perform architecture-specific initialization (if configured) */
|
|
|
|
boardctl(BOARDIOC_INIT, 0);
|
|
|
|
# ifdef CONFIG_BOARDCTL_FINALINIT
|
|
/* Perform architecture-specific final-initialization (if configured) */
|
|
|
|
boardctl(BOARDIOC_FINALINIT, 0);
|
|
# endif
|
|
#endif
|
|
|
|
PRINTF("\nStart foc_main application!\n\n");
|
|
|
|
/* Initialize threads */
|
|
|
|
ret = foc_threads_init();
|
|
if (ret < 0)
|
|
{
|
|
PRINTF("ERROR: failed to initialize threads %d\n", ret);
|
|
goto errout_no_mutex;
|
|
}
|
|
|
|
/* Initialize control interface */
|
|
|
|
ret = foc_intf_init();
|
|
if (ret < 0)
|
|
{
|
|
PRINTF("ERROR: failed to initialize control interface %d\n", ret);
|
|
goto errout;
|
|
}
|
|
|
|
/* Initialzie FOC controllers */
|
|
|
|
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
|
|
{
|
|
/* Get configuration */
|
|
|
|
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
|
|
foc[i].qparam = args.qparam;
|
|
#endif
|
|
foc[i].fmode = args.fmode;
|
|
foc[i].mmode = args.mmode;
|
|
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
|
|
foc[i].foc_pi_kp = args.foc_pi_kp;
|
|
foc[i].foc_pi_ki = args.foc_pi_ki;
|
|
#endif
|
|
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
|
|
foc[i].torqmax = args.torqmax;
|
|
#endif
|
|
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
|
|
foc[i].velmax = args.velmax;
|
|
#endif
|
|
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
|
|
foc[i].posmax = args.posmax;
|
|
#endif
|
|
|
|
if (args.en & (1 << i))
|
|
{
|
|
/* Initialize controller thread if enabled */
|
|
|
|
ret = foc_ctrlthr_init(&foc[i], i, &mqd[i], &threads[i]);
|
|
if (ret < 0)
|
|
{
|
|
PRINTF("ERROR: foc_ctrlthr_init failed %d!\n", ret);
|
|
goto errout;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Wait some time to finish all controllers initialziation */
|
|
|
|
usleep(10000);
|
|
|
|
/* Initial update for VBUS and SETPOINT */
|
|
|
|
#ifndef CONFIG_EXAMPLES_FOC_VBUS_ADC
|
|
data.vbus_update = true;
|
|
data.vbus_raw = VBUS_CONST_VALUE;
|
|
#endif
|
|
#ifndef CONFIG_EXAMPLES_FOC_SETPOINT_ADC
|
|
data.sp_update = true;
|
|
data.sp_raw = 1;
|
|
#endif
|
|
data.state_update = true;
|
|
|
|
/* Controller state */
|
|
|
|
data.state = args.state;
|
|
|
|
/* Auxliary control loop */
|
|
|
|
while (data.terminate != true)
|
|
{
|
|
PRINTFV("foc_main loop %d\n", time);
|
|
|
|
/* Update control interface */
|
|
|
|
ret = foc_intf_update(&data);
|
|
if (ret < 0)
|
|
{
|
|
PRINTF("ERROR: foc_intf_update failed: %d\n", ret);
|
|
goto errout;
|
|
}
|
|
|
|
/* 1. Update VBUS */
|
|
|
|
if (data.vbus_update == true)
|
|
{
|
|
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
|
|
{
|
|
if (args.en & (1 << i))
|
|
{
|
|
PRINTFV("Send vbus to %d\n", i);
|
|
|
|
/* Send VBUS to thread */
|
|
|
|
ret = foc_vbus_send(mqd[i], data.vbus_raw);
|
|
if (ret < 0)
|
|
{
|
|
PRINTF("ERROR: foc_vbus_send failed %d\n", ret);
|
|
goto errout;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Reset flag */
|
|
|
|
data.vbus_update = false;
|
|
}
|
|
|
|
/* 2. Update motor state */
|
|
|
|
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", data.state, i);
|
|
|
|
/* Send STATE to thread */
|
|
|
|
ret = foc_state_send(mqd[i], data.state);
|
|
if (ret < 0)
|
|
{
|
|
PRINTF("ERROR: foc_state_send failed %d\n", ret);
|
|
goto errout;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Reset flag */
|
|
|
|
data.state_update = false;
|
|
}
|
|
|
|
/* 3. Update motor velocity */
|
|
|
|
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",
|
|
data.sp_raw, i);
|
|
|
|
/* Send setpoint to threads */
|
|
|
|
ret = foc_setpoint_send(mqd[i], data.sp_raw);
|
|
if (ret < 0)
|
|
{
|
|
PRINTF("ERROR: foc_setpoint_send failed %d\n", ret);
|
|
goto errout;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Reset flag */
|
|
|
|
data.sp_update = false;
|
|
}
|
|
|
|
/* 4. One time start */
|
|
|
|
if (data.started == false)
|
|
{
|
|
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
|
|
{
|
|
if (args.en & (1 << i))
|
|
{
|
|
PRINTFV("Send start to %d\n", i);
|
|
|
|
/* Send START to threads */
|
|
|
|
ret = foc_start_send(mqd[i]);
|
|
if (ret < 0)
|
|
{
|
|
PRINTF("ERROR: foc_start_send failed %d\n", ret);
|
|
goto errout;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Set flag */
|
|
|
|
data.started = true;
|
|
}
|
|
|
|
/* Handle run time */
|
|
|
|
time += 1;
|
|
|
|
if (args.time != -1)
|
|
{
|
|
if (time >= (args.time * (1000000 / MAIN_LOOP_USLEEP)))
|
|
{
|
|
/* Exit loop */
|
|
|
|
data.terminate = true;
|
|
}
|
|
}
|
|
|
|
/* Terminate main loop if threads terminated */
|
|
|
|
if (foc_threads_terminated() == true)
|
|
{
|
|
data.terminate = true;
|
|
}
|
|
|
|
usleep(MAIN_LOOP_USLEEP);
|
|
}
|
|
|
|
errout:
|
|
|
|
/* Stop FOC control threads */
|
|
|
|
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
|
|
{
|
|
if (args.en & (1 << i))
|
|
{
|
|
if (mqd[i] != (mqd_t)-1)
|
|
{
|
|
/* Stop thread message */
|
|
|
|
ret = foc_kill_send(mqd[i]);
|
|
if (ret < 0)
|
|
{
|
|
PRINTF("ERROR: foc_kill_send failed %d\n", ret);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Wait for threads termination */
|
|
|
|
while (foc_threads_terminated() == false)
|
|
{
|
|
usleep(100000);
|
|
}
|
|
|
|
/* De-initialize all mq */
|
|
|
|
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
|
|
{
|
|
if (args.en & (1 << i))
|
|
{
|
|
/* Close FOC control thread queue */
|
|
|
|
if (mqd[i] != (mqd_t)-1)
|
|
{
|
|
mq_close(mqd[i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
/* 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();
|
|
|
|
PRINTF("foc_main exit\n");
|
|
|
|
return 0;
|
|
}
|