/**************************************************************************** * 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 #include #include #include #include #include #include #include #include #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" #ifdef CONFIG_EXAMPLES_FOC_NXSCOPE # include "foc_nxscope.h" #endif /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ /* Main loop sleep */ #define MAIN_LOOP_USLEEP (200000) /* Enabled instances default state */ #define INST_EN_DEFAULT (0xff) /**************************************************************************** * Private Data ****************************************************************************/ /* Default configuration */ struct args_s g_args = { .time = CONFIG_EXAMPLES_FOC_TIME_DEFAULT, .state = CONFIG_EXAMPLES_FOC_STATE_INIT, .en = INST_EN_DEFAULT, .cfg = { .fmode = CONFIG_EXAMPLES_FOC_FMODE, .mmode = CONFIG_EXAMPLES_FOC_MMODE, #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP .qparam = CONFIG_EXAMPLES_FOC_OPENLOOP_Q, .ol_force = false, # ifdef CONFIG_EXAMPLES_FOC_ANGOBS .ol_hys = CONFIG_EXAMPLES_FOC_ANGOBS_HYS, .ol_thr = CONFIG_EXAMPLES_FOC_ANGOBS_THR, # endif #endif #ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI .foc_pi_kp = CONFIG_EXAMPLES_FOC_IDQ_KP, .foc_pi_ki = CONFIG_EXAMPLES_FOC_IDQ_KI, #endif #ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST # ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ .torqmax = CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE, # endif # ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL .velmax = CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE, # endif # ifdef CONFIG_EXAMPLES_FOC_HAVE_POS .posmax = CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE, # endif #else # ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ .torqmax = CONFIG_EXAMPLES_FOC_SETPOINT_MAX, # endif # ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL .velmax = CONFIG_EXAMPLES_FOC_SETPOINT_MAX, # endif # ifdef CONFIG_EXAMPLES_FOC_HAVE_POS .posmax = CONFIG_EXAMPLES_FOC_SETPOINT_MAX, # endif #endif #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL .acc = CONFIG_EXAMPLES_FOC_RAMP_ACC, .dec = CONFIG_EXAMPLES_FOC_RAMP_DEC, #endif #ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT .ident_res_ki = CONFIG_EXAMPLES_FOC_IDENT_RES_KI, .ident_res_curr = CONFIG_EXAMPLES_FOC_IDENT_RES_CURRENT, .ident_res_sec = CONFIG_EXAMPLES_FOC_IDENT_RES_SEC, .ident_ind_volt = CONFIG_EXAMPLES_FOC_IDENT_IND_VOLTAGE, .ident_ind_sec = CONFIG_EXAMPLES_FOC_IDENT_IND_SEC, #endif #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL .vel_filter = CONFIG_EXAMPLES_FOC_VELNOW_FILTER, #endif #ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL .vel_pll_kp = CONFIG_EXAMPLES_FOC_VELOBS_PLL_KP, .vel_pll_ki = CONFIG_EXAMPLES_FOC_VELOBS_PLL_KI, #endif #ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV .vel_div_samples = CONFIG_EXAMPLES_FOC_VELOBS_DIV_SAMPLES, .vel_div_filter = CONFIG_EXAMPLES_FOC_VELOBS_DIV_FILTER, #endif #ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI .vel_pi_kp = CONFIG_EXAMPLES_FOC_VELCTRL_PI_KP, .vel_pi_ki = CONFIG_EXAMPLES_FOC_VELCTRL_PI_KI, #endif #ifdef CONFIG_INDUSTRY_FOC_ANGLE_ONFO .ang_nfo_slow = CONFIG_EXAMPLES_FOC_ANGOBS_NFO_GAINSLOW, .ang_nfo_gain = CONFIG_EXAMPLES_FOC_ANGOBS_NFO_GAIN, #endif } }; /* Start allowed at defaule */ static bool g_start_allowed = true; static pthread_mutex_t g_start_allowed_lock = PTHREAD_MUTEX_INITIALIZER; /**************************************************************************** * Private Functions ****************************************************************************/ /**************************************************************************** * 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); } #ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_START /**************************************************************************** * Name: foc_nxscope_cb_start ****************************************************************************/ static int foc_nxscope_cb_start(FAR void *priv, bool start) { pthread_mutex_lock(&g_start_allowed_lock); g_start_allowed = start; pthread_mutex_unlock(&g_start_allowed_lock); return OK; } #endif /**************************************************************************** * 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 foc_intf_data_s data; uint32_t thrs_active = 0; int ret = OK; int i = 0; int time = 0; bool startallowed = false; #ifdef CONFIG_EXAMPLES_FOC_NXSCOPE struct foc_nxscope_s nxs; #endif /* Reset some data */ 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)); #ifdef CONFIG_EXAMPLES_FOC_NXSCOPE memset(&nxs, 0, sizeof(struct foc_nxscope_s)); #endif #ifdef CONFIG_BUILTIN /* Parse the command line */ parse_args(&g_args, argc, argv); #endif /* Validate arguments */ ret = validate_args(&g_args); if (ret < 0) { PRINTF("ERROR: validate args failed\n"); goto errout_no_nxscope; } #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"); #ifdef CONFIG_EXAMPLES_FOC_NXSCOPE # ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_START /* Wait for nxscope */ g_start_allowed = false; /* Connect start callback */ nxs.cb.start = foc_nxscope_cb_start; # endif /* Initialize nxscope */ ret = foc_nxscope_init(&nxs); if (ret < 0) { PRINTF("ERROR: failed to initialize nxscope %d\n", ret); goto errout_no_threads; } #endif /* Initialize threads */ ret = foc_threads_init(); if (ret < 0) { PRINTF("ERROR: failed to initialize threads %d\n", ret); goto errout_no_intf; } /* 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 */ foc[i].cfg = &g_args.cfg; #ifdef CONFIG_EXAMPLES_FOC_NXSCOPE foc[i].nxs = &nxs; #endif if (g_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 = g_args.state; /* Auxliary control loop */ while (data.terminate != true) { PRINTFV("foc_main loop %d\n", time); #ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_MAIN foc_nxscope_work(&nxs); #endif /* Get active control threads */ thrs_active = foc_threads_get(); /* 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 ((g_args.en & (1 << i)) && (thrs_active & (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 ((g_args.en & (1 << i)) && (thrs_active & (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 ((g_args.en & (1 << i)) && (thrs_active & (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) { /* Is start allowed now ? */ pthread_mutex_lock(&g_start_allowed_lock); startallowed = g_start_allowed; pthread_mutex_unlock(&g_start_allowed_lock); if (startallowed) { for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1) { if ((g_args.en & (1 << i)) && (thrs_active & (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 (g_args.time != -1) { if (time >= (g_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: /* De-initialize control interface */ ret = foc_intf_deinit(); if (ret < 0) { PRINTF("ERROR: foc_inf_deinit failed %d\n", ret); } errout_no_intf: /* Stop FOC control threads */ for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1) { /* Only for active threads */ thrs_active = foc_threads_get(); if ((g_args.en & (1 << i)) && (thrs_active & (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 (g_args.en & (1 << i)) { /* Close FOC control thread queue */ if (mqd[i] != (mqd_t)-1) { mq_close(mqd[i]); } } } /* De-initialize control threads */ foc_threads_deinit(); #ifdef CONFIG_EXAMPLES_FOC_NXSCOPE errout_no_threads: /* De-initialize NxScope */ foc_nxscope_deinit(&nxs); #endif errout_no_nxscope: PRINTF("foc_main exit\n"); return 0; }