/**************************************************************************** * apps/examples/foc/foc_float_thr.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 "foc_cfg.h" #include "foc_debug.h" #include "foc_motor_f32.h" #include "industry/foc/foc_utils.h" #include "industry/foc/foc_common.h" #ifdef CONFIG_EXAMPLES_FOC_NXSCOPE # include "logging/nxscope/nxscope.h" #endif /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ #ifndef CONFIG_INDUSTRY_FOC_FLOAT # error #endif /**************************************************************************** * Private Type Definition ****************************************************************************/ /**************************************************************************** * Private Functions ****************************************************************************/ /**************************************************************************** * Name: foc_handler_run ****************************************************************************/ static int foc_handler_run(FAR struct foc_motor_f32_s *motor, FAR struct foc_device_s *dev) { struct foc_handler_input_f32_s input; struct foc_handler_output_f32_s output; float current[CONFIG_MOTOR_FOC_PHASES]; int ret = OK; int i = 0; DEBUGASSERT(motor); DEBUGASSERT(dev); /* FOC device fault */ if (motor->fault == true) { /* Stop motor */ motor->dq_ref.q = 0.0f; motor->dq_ref.d = 0.0f; motor->angle_now = 0.0f; motor->vbus = 0.0f; /* Force velocity to zero */ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ motor->torq.des = 0.0f; #endif #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL motor->vel.des = 0.0f; #endif #ifdef CONFIG_EXAMPLES_FOC_HAVE_POS motor->pos.des = 0.0f; #endif } /* Get real currents */ for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) { current[i] = (motor->iphase_adc * dev->state.curr[i]); } /* Get input for FOC handler */ input.current = current; input.dq_ref = &motor->dq_ref; input.vdq_comp = &motor->vdq_comp; input.angle = motor->angle_now; input.vbus = motor->vbus; input.mode = motor->foc_mode; /* Run FOC controller */ ret = foc_handler_run_f32(&motor->handler, &input, &output); /* Get duty from controller */ for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) { dev->params.duty[i] = FOCDUTY_FROM_FLOAT(output.duty[i]); } /* Get FOC handler state */ foc_handler_state_f32(&motor->handler, &motor->foc_state, &motor->mod_state); return ret; } #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM /**************************************************************************** * Name: foc_model_state_get ****************************************************************************/ static int foc_model_state_get(FAR struct foc_motor_f32_s *motor, FAR struct foc_device_s *dev) { int i = 0; DEBUGASSERT(motor); DEBUGASSERT(dev); /* Get model state */ foc_model_state_f32(&motor->model, &motor->model_state); /* Get model currents */ for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) { dev->state.curr[i] = motor->model_state.curr_raw[i]; } return OK; } #endif #ifdef FOC_STATE_PRINT_PRE /**************************************************************************** * Name: foc_state_print ****************************************************************************/ static int foc_state_print(FAR struct foc_motor_f32_s *motor) { DEBUGASSERT(motor); PRINTF("f32 inst %d:\n", motor->envp->inst); foc_handler_state_print_f32(&motor->foc_state); return OK; } #endif #ifdef CONFIG_EXAMPLES_FOC_NXSCOPE /**************************************************************************** * Name: foc_float_nxscope ****************************************************************************/ static void foc_float_nxscope(FAR struct foc_nxscope_s *nxs, FAR struct foc_motor_f32_s *motor, FAR struct foc_device_s *dev) { #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG != 0) FAR float *ptr = NULL; int i = nxs->ch_per_inst * motor->envp->id; #endif #ifndef CONFIG_EXAMPLES_FOC_NXSCOPE_CONTROL nxscope_lock(&nxs->nxs); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IABC) ptr = (FAR float *)&motor->foc_state.curr; nxscope_put_vfloat(&nxs->nxs, i++, ptr, CONFIG_MOTOR_FOC_PHASES); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IDQ) ptr = (FAR float *)&motor->foc_state.idq; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IAB) ptr = (FAR float *)&motor->foc_state.iab; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VABC) ptr = (FAR float *)&motor->foc_state.volt; nxscope_put_vfloat(&nxs->nxs, i++, ptr, CONFIG_MOTOR_FOC_PHASES); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQ) ptr = (FAR float *)&motor->foc_state.vdq; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VAB) ptr = (FAR float *)&motor->foc_state.vab; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AEL) ptr = (FAR float *)&motor->angle_el; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AM) ptr = (FAR float *)&motor->angle_m; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL) ptr = (FAR float *)&motor->vel_el; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM) ptr = (FAR float *)&motor->vel_mech; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS) ptr = (FAR float *)&motor->vbus; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPTORQ) ptr = (FAR float *)&motor->torq; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 3); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPVEL) ptr = (FAR float *)&motor->vel; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 3); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPPOS) ptr = (FAR float *)&motor->pos; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 3); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_DQREF) ptr = (FAR float *)&motor->dq_ref; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQCOMP) ptr = (FAR float *)&motor->vdq_comp; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SVM3) float svm3_tmp[4]; /* Convert sector to float. * Normally, a sector value is an integer in the range 1-6 but we convert * it to float and range to 0.1-0.6. This is to send the entire SVM3 state * as float array and scale the sector value closer to PWM duty values * (range 0.0 to 0.5) which makes it easier to visualize the data later. */ svm3_tmp[0] = (float)motor->mod_state.sector * 0.1f; svm3_tmp[1] = motor->mod_state.d_u; svm3_tmp[2] = motor->mod_state.d_v; svm3_tmp[3] = motor->mod_state.d_w; ptr = svm3_tmp; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 4); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VOBS) ptr = (FAR float *)&motor->vel_obs; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AOBS) ptr = (FAR float *)&motor->angle_obs; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1); #endif #ifndef CONFIG_EXAMPLES_FOC_NXSCOPE_CONTROL nxscope_unlock(&nxs->nxs); #endif } #endif /**************************************************************************** * Public Functions ****************************************************************************/ /**************************************************************************** * Name: foc_float_thr ****************************************************************************/ int foc_float_thr(FAR struct foc_ctrl_env_s *envp) { struct foc_mq_s handle; struct foc_motor_f32_s motor; struct foc_device_s dev; int ret = OK; DEBUGASSERT(envp); PRINTFV("foc_float_thr, id=%d\n", envp->id); /* Reset data */ memset(&handle, 0, sizeof(struct foc_mq_s)); /* Initialize motor controller */ ret = foc_motor_init(&motor, envp); if (ret < 0) { PRINTF("ERROR: foc_motor_init failed %d!\n", ret); goto errout; } /* Initialize FOC device as blocking */ ret = foc_device_init(&dev, envp->id); if (ret < 0) { PRINTF("ERROR: foc_device_init failed %d!\n", ret); goto errout; } /* Store data from device */ motor.pwm_duty_max = FOCDUTY_TO_FLOAT(dev.info.hw_cfg.pwm_max); motor.iphase_adc = dev.info.hw_cfg.iphase_scale / 100000.0f; /* Start with motor free */ handle.app_state = FOC_EXAMPLE_STATE_FREE; /* Wait some time */ usleep(1000); /* Control loop */ while (motor.mq.quit == false) { if (motor.mq.start == true) { /* Get FOC device state */ ret = foc_dev_state_get(&dev); if (ret < 0) { PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret); goto errout; } } PRINTFV("foc_float_thr %d %d\n", envp->id, motor.time); /* Handle mqueue */ ret = foc_mq_handle(envp->mqd, &handle); if (ret < 0) { PRINTF("ERROR: foc_mq_handle failed %d!\n", ret); goto errout; } /* Handle motor data */ ret = foc_motor_handle(&motor, &handle); if (ret < 0) { PRINTF("ERROR: foc_motor_handle failed %d!\n", ret); goto errout; } if (motor.startstop == true) { /* Start or stop device */ PRINTF("Start FOC device %d state=%d!\n", motor.envp->id, motor.mq.start); ret = foc_device_start(&dev, motor.mq.start); if (ret < 0) { PRINTFV("ERROR: foc_device_start failed %d!\n", ret); goto errout; } motor.startstop = false; /* Start from the beginning of the control loop */ continue; } /* Ignore control logic if controller not started yet */ if (motor.mq.start == false) { usleep(1000); continue; } #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM /* Get model state */ ret = foc_model_state_get(&motor, &dev); if (ret < 0) { PRINTF("ERROR: foc_model_state_get failed %d!\n", ret); goto errout; } #endif /* Handle controller state */ ret = foc_dev_state_handle(&dev, &motor.fault); if (ret < 0) { PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret); goto errout; } /* Get motor state */ ret = foc_motor_get(&motor); if (ret < 0) { PRINTF("ERROR: foc_motor_get failed %d!\n", ret); goto errout; } /* Motor control */ ret = foc_motor_control(&motor); if (ret < 0) { PRINTF("ERROR: foc_motor_control failed %d!\n", ret); goto errout; } /* Run FOC */ ret = foc_handler_run(&motor, &dev); if (ret < 0) { PRINTF("ERROR: foc_handler_run failed %d!\n", ret); goto errout; } #ifdef FOC_STATE_PRINT_PRE /* Print state if configured */ if (motor.time % FOC_STATE_PRINT_PRE == 0) { foc_state_print(&motor); } #endif #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM /* Feed FOC model with data */ foc_model_run_f32(&motor.model, FOC_MODEL_LOAD, &motor.foc_state.vab); #endif /* Set FOC device parameters */ ret = foc_dev_params_set(&dev); if (ret < 0) { PRINTF("ERROR: foc_dev_prams_set failed %d!\n", ret); goto errout; } #ifdef CONFIG_EXAMPLES_FOC_NXSCOPE /* Capture nxscope samples */ if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0) { foc_float_nxscope(envp->nxs, &motor, &dev); } #endif #ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_CONTROL /* Handle nxscope work */ if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_WORK_PRESCALER == 0) { foc_nxscope_work(envp->nxs); } #endif /* Terminate control thread */ if (motor.ctrl_state == FOC_CTRL_STATE_TERMINATE) { PRINTF("TERMINATE CTRL THREAD\n"); goto errout; } /* Increase counter */ motor.time += 1; #ifdef CONFIG_EXAMPLES_FOC_PERF if (dev.perf.max_changed) { PRINTF_PERF("max=%" PRId32 "\n", dev.perf.max); } #endif } errout: /* Deinit motor controller */ ret = foc_motor_deinit(&motor); if (ret != OK) { PRINTF("ERROR: foc_motor_deinit failed %d!\n", ret); } PRINTF("Stop FOC device %d!\n", envp->id); /* De-initialize FOC device */ ret = foc_device_deinit(&dev); if (ret < 0) { PRINTF("ERROR: foc_device_deinit %d failed %d\n", envp->id, ret); } PRINTF("foc_float_thr %d exit\n", envp->id); return ret; }