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

Add FOC library

Browse Source 
Supported features:
- float and fixed16 operations
- helper interface to interact with the kernel-side FOC device
- general interface for FOC controller
- PI current/voltage controller
- 3-phase space vector modulation
- general interface for FOC model
- PMSM model
- general interface for phase angle data
- open-loop handler
- general interface for phase velocity data
- general interface for ramp (eg. velocity ramp, torque ramp)
- (WIP) CORDIC interface for hardware acceleration
This commit is contained in:
raiden00pl 2021-03-20 21:06:54 +01:00 committed by Xiang Xiao
parent a333a908a8
commit d75d6b7af0
39 changed files with 6744 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

140
include/industry/foc/fixed16/foc_angle.h Normal file
View File

@ -0,0 +1,140 @@
/****************************************************************************
* apps/include/industry/foc/fixed16/foc_angle.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 __INDUSTRY_FOC_FIXED16_FOC_ANGLE_H
#define __INDUSTRY_FOC_FIXED16_FOC_ANGLE_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <dspb16.h>
#include "industry/foc/fixed16/foc_handler.h"
/****************************************************************************
* Public Type Definition
****************************************************************************/
/* Input to angle handler */
struct foc_angle_in_b16_s
{
FAR struct foc_state_b16_s *state; /* FOC state */
b16_t angle; /* Last angle */
b16_t vel; /* Last velocity */
b16_t dir; /* Movement direction */
};
/* Output from angle handler */
struct foc_angle_out_b16_s
{
b16_t angle;
};
/* Forward declaration */
typedef struct foc_angle_b16_s foc_angle_b16_t;
/* Angle operations */
struct foc_angle_ops_b16_s
{
/* Initialize */
CODE int (*init)(FAR foc_angle_b16_t *h);
/* Deinitialize */
CODE void (*deinit)(FAR foc_angle_b16_t *h);
/* Configure */
CODE int (*cfg)(FAR foc_angle_b16_t *h, FAR void *cfg);
/* Run */
CODE void (*run)(FAR foc_angle_b16_t *h,
FAR struct foc_angle_in_b16_s *in,
FAR struct foc_angle_out_b16_s *out);
};
/* Angle handler - sensor or sensorless */
struct foc_angle_b16_s
{
FAR struct foc_angle_ops_b16_s *ops;
FAR void *data;
};
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
/* Open-loop configuration data */
struct foc_openloop_cfg_b16_s
{
b16_t per; /* Controller period */
};
#endif
/****************************************************************************
* Public Data
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
/* Open-loop angle operations (fixed16) */
extern struct foc_angle_ops_b16_s g_foc_angle_ol_b16;
#endif
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_angle_init_b16
****************************************************************************/
int foc_angle_init_b16(FAR foc_angle_b16_t *h,
FAR struct foc_angle_ops_b16_s *ops);
/****************************************************************************
* Name: foc_angle_deinit_b16
****************************************************************************/
int foc_angle_deinit_b16(FAR foc_angle_b16_t *h);
/****************************************************************************
* Name: foc_angle_cfg_b16
****************************************************************************/
int foc_angle_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg);
/****************************************************************************
* Name: foc_angle_run_b16
****************************************************************************/
void foc_angle_run_b16(FAR foc_angle_b16_t *h,
FAR struct foc_angle_in_b16_s *in,
FAR struct foc_angle_out_b16_s *out);
#endif /* __INDUSTRY_FOC_FIXED16_FOC_ANGLE_H */

48
include/industry/foc/fixed16/foc_cordic.h Normal file
View File

@ -0,0 +1,48 @@
/****************************************************************************
* apps/include/industry/foc/fixed16/foc_cordic.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 __INDUSTRY_FOC_FIXED16_FOC_CORDIC_H
#define __INDUSTRY_FOC_FIXED16_FOC_CORDIC_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <dspb16.h>
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_cordic_dqsat_b16
****************************************************************************/
int foc_cordic_dqsat_b16(int fd, FAR dq_frame_b16_t *dq_ref, b16_t mag_max);
/****************************************************************************
* Name: foc_cordic_angle_b16
****************************************************************************/
int foc_cordic_angle_b16(int fd, FAR phase_angle_b16_t *angle, b16_t a);
#endif /* __INDUSTRY_FOC_FIXED16_FOC_CORDIC_H */

247
include/industry/foc/fixed16/foc_handler.h Normal file
View File

@ -0,0 +1,247 @@
/****************************************************************************
* apps/include/industry/foc/fixed16/foc_handler.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 __INDUSTRY_FOC_FIXED16_FOC_HANDLER_H
#define __INDUSTRY_FOC_FIXED16_FOC_HANDLER_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <dspb16.h>
#ifdef CONFIG_INDUSTRY_FOC_CORDIC
# include "industry/foc/fixed16/foc_cordic.h"
#endif
/****************************************************************************
* Public Type Definition
****************************************************************************/
/* Input to FOC controller */
struct foc_handler_input_b16_s
{
FAR b16_t *current; /* Phase current samples */
FAR dq_frame_b16_t *dq_ref; /* DQ reference frame */
FAR dq_frame_b16_t *vdq_comp; /* DQ voltage compensation */
FAR b16_t angle; /* Phase angle */
FAR b16_t vbus; /* Bus voltage */
FAR int mode; /* Controller mode (enum foc_handler_mode_e) */
};
/* Output from FOC controller */
struct foc_handler_output_b16_s
{
b16_t duty[CONFIG_MOTOR_FOC_PHASES]; /* New duty cycle for PWM */
};
/* Controller state */
struct foc_state_b16_s
{
b16_t curr[CONFIG_MOTOR_FOC_PHASES];
b16_t volt[CONFIG_MOTOR_FOC_PHASES];
ab_frame_b16_t iab;
ab_frame_b16_t vab;
dq_frame_b16_t vdq;
dq_frame_b16_t idq;
};
/* Forward declaration */
typedef struct foc_handler_b16_s foc_handler_b16_t;
/* Modulation operations */
struct foc_modulation_ops_b16_s
{
/* Initialize */
CODE int (*init)(FAR foc_handler_b16_t *h);
/* Deinitialzie */
CODE void (*deinit)(FAR foc_handler_b16_t *h);
/* Configure modulation */
CODE void (*cfg)(FAR foc_handler_b16_t *h, FAR void *cfg);
/* Modulation specific current correction */
CODE void (*current)(FAR foc_handler_b16_t *h, FAR b16_t *curr);
/* Get the base voltage for a given modulation scheme */
CODE void (*vbase_get)(FAR foc_handler_b16_t *h,
b16_t vbus,
FAR b16_t *vbase);
/* Run modulation */
CODE void (*run)(FAR foc_handler_b16_t *h,
FAR ab_frame_b16_t *v_ab_mod,
FAR b16_t *duty);
};
/* Current/voltage controller operations */
struct foc_control_ops_b16_s
{
/* Initialize */
CODE int (*init)(FAR foc_handler_b16_t *h);
/* Deinitialize */
CODE void (*deinit)(FAR foc_handler_b16_t *h);
/* Configure controller */
CODE void (*cfg)(FAR foc_handler_b16_t *h, FAR void *cfg);
/* Feed controller with input data */
CODE void (*input_set)(FAR foc_handler_b16_t *h,
FAR b16_t *current,
b16_t vbase,
b16_t angle);
/* Run voltage controller */
CODE void (*voltage_run)(FAR foc_handler_b16_t *h,
FAR dq_frame_b16_t *dq_ref,
FAR ab_frame_b16_t *v_ab_mod);
/* Run current controller */
CODE void (*current_run)(FAR foc_handler_b16_t *h,
FAR dq_frame_b16_t *dq_ref,
FAR dq_frame_b16_t *vdq_comp,
FAR ab_frame_b16_t *v_ab_mod);
/* Get controller state data */
CODE void (*state_get)(FAR foc_handler_b16_t *h,
FAR struct foc_state_b16_s *state);
};
/* FOC handler operations */
struct foc_handler_ops_b16_s
{
/* Current/voltage controller interface */
struct foc_control_ops_b16_s *ctrl;
/* Modulation interface */
struct foc_modulation_ops_b16_s *mod;
};
/* FOC handler data */
struct foc_handler_b16_s
{
#ifdef CONFIG_INDUSTRY_FOC_CORDIC
int fd; /* CORDIC device */
#endif
struct foc_handler_ops_b16_s ops; /* Handler operations */
FAR void *modulation; /* Modulation data */
FAR void *control; /* Controller data */
};
/* Modulation configuration */
struct foc_mod_cfg_b16_s
{
b16_t pwm_duty_max; /* Maximum allowed PWM duty cycle */
};
/****************************************************************************
* Public Data
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI
/* FOC PI controller (fixed16) */
extern struct foc_control_ops_b16_s g_foc_control_pi_b16;
#endif
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
/* 3-phase space vector modulation Modulation handler (fixed16) */
extern struct foc_modulation_ops_b16_s g_foc_mod_svm3_b16;
#endif
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_handler_init_b16
****************************************************************************/
int foc_handler_init_b16(FAR foc_handler_b16_t *h,
FAR struct foc_control_ops_b16_s *ctrl,
FAR struct foc_modulation_ops_b16_s *mod);
/****************************************************************************
* Name: foc_handler_deinit_b16
****************************************************************************/
int foc_handler_deinit_b16(FAR foc_handler_b16_t *h);
/****************************************************************************
* Name: foc_handler_run_b16
****************************************************************************/
int foc_handler_run_b16(FAR foc_handler_b16_t *h,
FAR struct foc_handler_input_b16_s *in,
FAR struct foc_handler_output_b16_s *out);
/****************************************************************************
* Name: foc_handler_cfg_b16
****************************************************************************/
void foc_handler_cfg_b16(FAR foc_handler_b16_t *h,
FAR void *ctrl_cfg,
FAR void *mod_cfg);
/****************************************************************************
* Name: foc_handler_state_b16
****************************************************************************/
void foc_handler_state_b16(FAR foc_handler_b16_t *h,
FAR struct foc_state_b16_s *state);
#ifdef CONFIG_INDUSTRY_FOC_HANDLER_PRINT
/****************************************************************************
* Name: foc_handler_state_print_b16
****************************************************************************/
void foc_handler_state_print_b16(FAR struct foc_state_b16_s *state);
#endif
#endif /* __INDUSTRY_FOC_FIXED16_FOC_HANDLER_H */

160
include/industry/foc/fixed16/foc_model.h Normal file
View File

@ -0,0 +1,160 @@
/****************************************************************************
* apps/include/industry/foc/fixed16/foc_model.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 __INDUSTRY_FOC_FIXED16_FOC_MODEL_H
#define __INDUSTRY_FOC_FIXED16_FOC_MODEL_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <dspb16.h>
/****************************************************************************
* Public Type Definition
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_MODEL_PMSM
/* PMSM model configuration */
struct foc_model_pmsm_cfg_b16_s
{
uint8_t poles;
b16_t res;
b16_t ind;
b16_t iner;
b16_t flux_link;
b16_t ind_d;
b16_t ind_q;
b16_t per;
b16_t iphase_adc;
};
#endif /* CONFIG_INDUSTRY_FOC_MODEL_PMSM */
/* FOC model state */
struct foc_model_state_b16_s
{
int32_t curr_raw[CONFIG_MOTOR_FOC_PHASES];
b16_t curr[CONFIG_MOTOR_FOC_PHASES];
b16_t volt[CONFIG_MOTOR_FOC_PHASES];
ab_frame_b16_t iab;
ab_frame_b16_t vab;
dq_frame_b16_t vdq;
dq_frame_b16_t idq;
b16_t omega_e;
b16_t omega_m;
};
/* Forward declaration */
typedef struct foc_model_b16_s foc_model_b16_t;
/* FOC model operations */
struct foc_model_ops_b16_s
{
/* Initialize */
CODE int (*init)(FAR foc_model_b16_t *h);
/* Deinitialize */
CODE void (*deinit)(FAR foc_model_b16_t *h);
/* Configure model */
CODE int (*cfg)(FAR foc_model_b16_t *h, FAR void *cfg);
/* Run electrical model */
CODE void (*ele_run)(FAR foc_model_b16_t *h,
FAR ab_frame_b16_t *v_ab);
/* Run mechanical model */
CODE void (*mech_run)(FAR foc_model_b16_t *h,
b16_t load);
/* Get model state */
CODE void (*state)(FAR foc_model_b16_t *h,
FAR struct foc_model_state_b16_s *state);
};
/* FOC model handler */
struct foc_model_b16_s
{
FAR struct foc_model_ops_b16_s *ops;
FAR void *model;
};
/****************************************************************************
* Public Data
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_MODEL_PMSM
/* PMSM model operations (fixed16) */
extern struct foc_model_ops_b16_s g_foc_model_pmsm_ops_b16;
#endif
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_model_init_b16
****************************************************************************/
int foc_model_init_b16(FAR foc_model_b16_t *h,
FAR struct foc_model_ops_b16_s *ops);
/****************************************************************************
* Name: foc_model_deinit_b16
****************************************************************************/
int foc_model_deinit_b16(FAR foc_model_b16_t *h);
/****************************************************************************
* Name: foc_model_cfg_b16
****************************************************************************/
int foc_model_cfg_b16(FAR foc_model_b16_t *h, FAR void *cfg);
/****************************************************************************
* Name: foc_model_run_b16
****************************************************************************/
void foc_model_run_b16(FAR foc_model_b16_t *h,
b16_t load,
FAR ab_frame_b16_t *v_ab);
/****************************************************************************
* Name: foc_model_state_b16
****************************************************************************/
void foc_model_state_b16(FAR foc_model_b16_t *h,
FAR struct foc_model_state_b16_s *state);
#endif /* __INDUSTRY_FOC_FIXED16_FOC_MODEL_H */

68
include/industry/foc/fixed16/foc_ramp.h Normal file
View File

@ -0,0 +1,68 @@
/****************************************************************************
* apps/include/industry/foc/fixed16/foc_ramp.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 __INDUSTRY_FOC_FIXED16_FOC_RAMP_H
#define __INDUSTRY_FOC_FIXED16_FOC_RAMP_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <dspb16.h>
/****************************************************************************
* Public Type Definition
****************************************************************************/
/* Ramp data */
struct foc_ramp_b16_s
{
uint8_t ramp_mode; /* Ramp mode */
b16_t per; /* Controller period */
b16_t ramp_thr; /* Ramp threshold */
b16_t ramp_acc; /* Ramp acceleration */
b16_t ramp_dec; /* Ramp deceleration */
b16_t ramp_dec_per; /* dec * per */
b16_t ramp_acc_per; /* acc * per */
b16_t diff; /* Difference now */
};
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_ramp_init_b16
****************************************************************************/
int foc_ramp_init_b16(FAR struct foc_ramp_b16_s *ramp, b16_t per,
b16_t thr, b16_t acc, b16_t dec);
/****************************************************************************
* Name: foc_ramp_run_b16
****************************************************************************/
int foc_ramp_run_b16(FAR struct foc_ramp_b16_s *ramp, b16_t des,
b16_t now, FAR b16_t *set);
#endif /* __INDUSTRY_FOC_FIXED16_FOC_RAMP_H */

121
include/industry/foc/fixed16/foc_velocity.h Normal file
View File

@ -0,0 +1,121 @@
/****************************************************************************
* apps/include/industry/foc/fixed16/foc_velocity.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 __INDUSTRY_FOC_FIXED16_FOC_VELOCITY_H
#define __INDUSTRY_FOC_FIXED16_FOC_VELOCITY_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <dspb16.h>
#include "industry/foc/fixed16/foc_handler.h"
/****************************************************************************
* Public Type Definition
****************************************************************************/
/* Input to velocity handler */
struct foc_velocity_in_b16_s
{
FAR struct foc_state_b16_s *state; /* FOC state */
b16_t angle; /* Last angle */
b16_t vel; /* Last velocity */
b16_t dir; /* Movement direction */
};
/* Output from velocity handler */
struct foc_velocity_out_b16_s
{
b16_t velocity;
};
/* Forward declaration */
typedef struct foc_velocity_b16_s foc_velocity_b16_t;
/* Velocity operations */
struct foc_velocity_ops_b16_s
{
/* Initialize */
CODE int (*init)(FAR foc_velocity_b16_t *h);
/* Deinitialize */
CODE void (*deinit)(FAR foc_velocity_b16_t *h);
/* Configure */
CODE int (*cfg)(FAR foc_velocity_b16_t *h, FAR void *cfg);
/* Run */
CODE void (*run)(FAR foc_velocity_b16_t *h,
FAR struct foc_velocity_in_b16_s *in,
FAR struct foc_velocity_out_b16_s *out);
};
/* Velocity handler - sensor or sensorless */
struct foc_velocity_b16_s
{
FAR struct foc_velocity_ops_b16_s *ops;
FAR void *data;
};
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_velocity_init_b16
****************************************************************************/
int foc_velocity_init_b16(FAR foc_velocity_b16_t *h,
FAR struct foc_velocity_ops_b16_s *ops);
/****************************************************************************
* Name: foc_velocity_deinit_b16
****************************************************************************/
int foc_velocity_deinit_b16(FAR foc_velocity_b16_t *h);
/****************************************************************************
* Name: foc_velocity_cfg_b16
****************************************************************************/
int foc_velocity_cfg_b16(FAR foc_velocity_b16_t *h, FAR void *cfg);
/****************************************************************************
* Name: foc_velocity_run_b16
****************************************************************************/
void foc_velocity_run_b16(FAR foc_velocity_b16_t *h,
FAR struct foc_velocity_in_b16_s *in,
FAR struct foc_velocity_out_b16_s *out);
#endif /* __INDUSTRY_FOC_FIXED16_FOC_VELOCITY_H */

140
include/industry/foc/float/foc_angle.h Normal file
View File

@ -0,0 +1,140 @@
/****************************************************************************
* apps/include/industry/foc/float/foc_angle.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 __INDUSTRY_FOC_FLOAT_FOC_ANGLE_H
#define __INDUSTRY_FOC_FLOAT_FOC_ANGLE_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <dsp.h>
#include "industry/foc/float/foc_handler.h"
/****************************************************************************
* Public Type Definition
****************************************************************************/
/* Input to angle handler */
struct foc_angle_in_f32_s
{
FAR struct foc_state_f32_s *state; /* FOC state */
float angle; /* Last angle */
float vel; /* Last velocity */
float dir; /* Movement direction */
};
/* Output from angle handler */
struct foc_angle_out_f32_s
{
float angle;
};
/* Forward declaration */
typedef struct foc_angle_f32_s foc_angle_f32_t;
/* Angle operations */
struct foc_angle_ops_f32_s
{
/* Initialize */
CODE int (*init)(FAR foc_angle_f32_t *h);
/* Deinitialize */
CODE void (*deinit)(FAR foc_angle_f32_t *h);
/* Configure */
CODE int (*cfg)(FAR foc_angle_f32_t *h, FAR void *cfg);
/* Run angle handler */
CODE void (*run)(FAR foc_angle_f32_t *h,
FAR struct foc_angle_in_f32_s *in,
FAR struct foc_angle_out_f32_s *out);
};
/* Angle handler - sensor or sensorless */
struct foc_angle_f32_s
{
FAR struct foc_angle_ops_f32_s *ops;
FAR void *data;
};
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
/* Open-loop configuration data */
struct foc_openloop_cfg_f32_s
{
float per; /* Controller period */
};
#endif /* CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP */
/****************************************************************************
* Public Data
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
/* Open-loop angle operations (float) */
extern struct foc_angle_ops_f32_s g_foc_angle_ol_f32;
#endif
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_angle_init_f32
****************************************************************************/
int foc_angle_init_f32(FAR foc_angle_f32_t *h,
FAR struct foc_angle_ops_f32_s *ops);
/****************************************************************************
* Name: foc_angle_deinit_f32
****************************************************************************/
int foc_angle_deinit_f32(FAR foc_angle_f32_t *h);
/****************************************************************************
* Name: foc_angle_cfg_f32
****************************************************************************/
int foc_angle_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg);
/****************************************************************************
* Name: foc_angle_run_f32
****************************************************************************/
void foc_angle_run_f32(FAR foc_angle_f32_t *h,
FAR struct foc_angle_in_f32_s *in,
FAR struct foc_angle_out_f32_s *out);
#endif /* __INDUSTRY_FOC_FLOAT_FOC_ANGLE_H */

48
include/industry/foc/float/foc_cordic.h Normal file
View File

@ -0,0 +1,48 @@
/****************************************************************************
* apps/include/industry/foc/float/foc_cordic.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 __INDUSTRY_FOC_FLOAT_FOC_CORDIC_H
#define __INDUSTRY_FOC_FLOAT_FOC_CORDIC_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <dsp.h>
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_cordic_dqsat_f32
****************************************************************************/
int foc_cordic_dqsat_f32(int fd, FAR dq_frame_f32_t *dq_ref, float mag_max);
/****************************************************************************
* Name: foc_cordic_angle_f32
****************************************************************************/
int foc_cordic_angle_f32(int fd, FAR phase_angle_f32_t *angle, float a);
#endif /* __INDUSTRY_FOC_FLOAT_FOC_CORDIC_H */

248
include/industry/foc/float/foc_handler.h Normal file
View File

@ -0,0 +1,248 @@
/****************************************************************************
* apps/include/industry/foc/float/foc_handler.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 __INDUSTRY_FOC_FLOAT_FOC_HANDLER_H
#define __INDUSTRY_FOC_FLOAT_FOC_HANDLER_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <dsp.h>
#ifdef CONFIG_INDUSTRY_FOC_CORDIC
# include "industry/foc/float/foc_cordic.h"
#endif
/****************************************************************************
* Public Type Definition
****************************************************************************/
/* Input to FOC controller */
struct foc_handler_input_f32_s
{
FAR float *current; /* Phase current samples */
FAR dq_frame_f32_t *dq_ref; /* DQ reference frame */
FAR dq_frame_f32_t *vdq_comp; /* DQ voltage compensation */
FAR float angle; /* Phase angle */
FAR float vbus; /* Bus voltage */
FAR int mode; /* Controller mode (enum foc_handler_mode_e) */
};
/* Output from FOC controller */
struct foc_handler_output_f32_s
{
float duty[CONFIG_MOTOR_FOC_PHASES]; /* New duty cycle for PWM */
};
/* Controller state */
struct foc_state_f32_s
{
float curr[CONFIG_MOTOR_FOC_PHASES];
float volt[CONFIG_MOTOR_FOC_PHASES];
ab_frame_f32_t iab;
ab_frame_f32_t vab;
dq_frame_f32_t vdq;
dq_frame_f32_t idq;
};
/* Forward declaration */
typedef struct foc_handler_f32_s foc_handler_f32_t;
/* Modulation operations */
struct foc_modulation_ops_f32_s
{
/* Initialize */
CODE int (*init)(FAR foc_handler_f32_t *h);
/* Deinitialzie */
CODE void (*deinit)(FAR foc_handler_f32_t *h);
/* Configure modulation */
CODE void (*cfg)(FAR foc_handler_f32_t *h, FAR void *cfg);
/* Modulation specific current correction */
CODE void (*current)(FAR foc_handler_f32_t *h, FAR float *curr);
/* Get the base voltage for a given modulation scheme */
CODE void (*vbase_get)(FAR foc_handler_f32_t *h,
float vbus,
FAR float *vbase);
/* Run modulation */
CODE void (*run)(FAR foc_handler_f32_t *h,
FAR ab_frame_f32_t *v_ab_mod,
FAR float *duty);
};
/* Current/voltage controller operations */
struct foc_control_ops_f32_s
{
/* Initialize */
CODE int (*init)(FAR foc_handler_f32_t *h);
/* Deinitialize */
CODE void (*deinit)(FAR foc_handler_f32_t *h);
/* Configure controller */
CODE void (*cfg)(FAR foc_handler_f32_t *h,
FAR void *cfg);
/* Feed controller with input data */
CODE void (*input_set)(FAR foc_handler_f32_t *h,
FAR float *current,
float vbase,
float angle);
/* Run voltage controller */
CODE void (*voltage_run)(FAR foc_handler_f32_t *h,
FAR dq_frame_f32_t *dq_ref,
FAR ab_frame_f32_t *v_ab_mod);
/* Run current controller */
CODE void (*current_run)(FAR foc_handler_f32_t *h,
FAR dq_frame_f32_t *dq_ref,
FAR dq_frame_f32_t *vdq_comp,
FAR ab_frame_f32_t *v_ab_mod);
/* Get controller state data */
CODE void (*state_get)(FAR foc_handler_f32_t *h,
FAR struct foc_state_f32_s *state);
};
/* FOC handler operations */
struct foc_handler_ops_f32_s
{
/* Current/voltage controller interface */
struct foc_control_ops_f32_s *ctrl;
/* Modulation interface */
struct foc_modulation_ops_f32_s *mod;
};
/* FOC handler data */
struct foc_handler_f32_s
{
#ifdef CONFIG_INDUSTRY_FOC_CORDIC
int fd; /* CORDIC device */
#endif
struct foc_handler_ops_f32_s ops; /* Handler operations */
FAR void *modulation; /* Modulation data */
FAR void *control; /* Controller data */
};
/* Modulation configuration */
struct foc_mod_cfg_f32_s
{
float pwm_duty_max; /* Maximum allowed PWM duty cycle */
};
/****************************************************************************
* Public Data
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI
/* FOC PI controller (float) */
extern struct foc_control_ops_f32_s g_foc_control_pi_f32;
#endif
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
/* 3-phase space vector modulation Modulation handler (float) */
extern struct foc_modulation_ops_f32_s g_foc_mod_svm3_f32;
#endif
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_handler_init_f32
****************************************************************************/
int foc_handler_init_f32(FAR foc_handler_f32_t *h,
FAR struct foc_control_ops_f32_s *ctrl,
FAR struct foc_modulation_ops_f32_s *mod);
/****************************************************************************
* Name: foc_handler_deinit_f32
****************************************************************************/
int foc_handler_deinit_f32(FAR foc_handler_f32_t *h);
/****************************************************************************
* Name: foc_handler_run_f32
****************************************************************************/
int foc_handler_run_f32(FAR foc_handler_f32_t *h,
FAR struct foc_handler_input_f32_s *in,
FAR struct foc_handler_output_f32_s *out);
/****************************************************************************
* Name: foc_handler_cfg_f32
****************************************************************************/
void foc_handler_cfg_f32(FAR foc_handler_f32_t *h,
FAR void *ctrl_cfg,
FAR void *mod_cfg);
/****************************************************************************
* Name: foc_handler_state_f32
****************************************************************************/
void foc_handler_state_f32(FAR foc_handler_f32_t *h,
FAR struct foc_state_f32_s *state);
#ifdef CONFIG_INDUSTRY_FOC_HANDLER_PRINT
/****************************************************************************
* Name: foc_handler_state_print_f32
****************************************************************************/
void foc_handler_state_print_f32(FAR struct foc_state_f32_s *state);
#endif
#endif /* __INDUSTRY_FOC_FLOAT_FOC_HANDLER_H */

160
include/industry/foc/float/foc_model.h Normal file
View File

@ -0,0 +1,160 @@
/****************************************************************************
* apps/include/industry/foc/float/foc_model.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 __INDUSTRY_FOC_FLOAT_FOC_MODEL_H
#define __INDUSTRY_FOC_FLOAT_FOC_MODEL_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <dsp.h>
/****************************************************************************
* Public Type Definition
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_MODEL_PMSM
/* PMSM model configuration */
struct foc_model_pmsm_cfg_f32_s
{
uint8_t poles;
float res;
float ind;
float iner;
float flux_link;
float ind_d;
float ind_q;
float per;
float iphase_adc;
};
#endif /* CONFIG_INDUSTRY_FOC_MODEL_PMSM */
/* FOC model state */
struct foc_model_state_f32_s
{
int32_t curr_raw[CONFIG_MOTOR_FOC_PHASES];
float curr[CONFIG_MOTOR_FOC_PHASES];
float volt[CONFIG_MOTOR_FOC_PHASES];
ab_frame_f32_t iab;
ab_frame_f32_t vab;
dq_frame_f32_t vdq;
dq_frame_f32_t idq;
float omega_e;
float omega_m;
};
/* Forward declaration */
typedef struct foc_model_f32_s foc_model_f32_t;
/* FOC model operations */
struct foc_model_ops_f32_s
{
/* Initialize */
CODE int (*init)(FAR foc_model_f32_t *h);
/* Deinitialize */
CODE void (*deinit)(FAR foc_model_f32_t *h);
/* Configure model */
CODE int (*cfg)(FAR foc_model_f32_t *h, FAR void *cfg);
/* Run electrical model */
CODE void (*ele_run)(FAR foc_model_f32_t *h,
FAR ab_frame_f32_t *v_ab);
/* Run mechanical model */
CODE void (*mech_run)(FAR foc_model_f32_t *h,
float load);
/* Get model state */
CODE void (*state)(FAR foc_model_f32_t *h,
FAR struct foc_model_state_f32_s *state);
};
/* FOC model handler */
struct foc_model_f32_s
{
FAR struct foc_model_ops_f32_s *ops;
FAR void *model;
};
/****************************************************************************
* Public Data
****************************************************************************/
#ifdef CONFIG_INDUSTRY_FOC_MODEL_PMSM
/* PMSM model operations (float) */
extern struct foc_model_ops_f32_s g_foc_model_pmsm_ops_f32;
#endif
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_model_init_f32
****************************************************************************/
int foc_model_init_f32(FAR foc_model_f32_t *h,
FAR struct foc_model_ops_f32_s *ops);
/****************************************************************************
* Name: foc_model_deinit_f32
****************************************************************************/
int foc_model_deinit_f32(FAR foc_model_f32_t *h);
/****************************************************************************
* Name: foc_model_cfg_f32
****************************************************************************/
int foc_model_cfg_f32(FAR foc_model_f32_t *h, FAR void *cfg);
/****************************************************************************
* Name: foc_model_run_f32
****************************************************************************/
void foc_model_run_f32(FAR foc_model_f32_t *h,
float load,
FAR ab_frame_f32_t *v_ab);
/****************************************************************************
* Name: foc_model_state_f32
****************************************************************************/
void foc_model_state_f32(FAR foc_model_f32_t *h,
FAR struct foc_model_state_f32_s *state);
#endif /* __INDUSTRY_FOC_FLOAT_FOC_MODEL_H */

68
include/industry/foc/float/foc_ramp.h Normal file
View File

@ -0,0 +1,68 @@
/****************************************************************************
* apps/include/industry/foc/float/foc_ramp.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 __INDUSTRY_FOC_FLOAT_FOC_RAMP_H
#define __INDUSTRY_FOC_FLOAT_FOC_RAMP_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <dsp.h>
/****************************************************************************
* Public Type Definition
****************************************************************************/
/* Ramp data */
struct foc_ramp_f32_s
{
uint8_t ramp_mode; /* Ramp mode */
float per; /* Controller period */
float ramp_thr; /* Ramp threshold */
float ramp_acc; /* Ramp acceleration */
float ramp_dec; /* Ramp deceleration */
float ramp_dec_per; /* dec * per */
float ramp_acc_per; /* acc * per */
float diff; /* Difference now */
};
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_ramp_init_f32
****************************************************************************/
int foc_ramp_init_f32(FAR struct foc_ramp_f32_s *ramp, float per,
float thr, float acc, float dec);
/****************************************************************************
* Name: foc_ramp_run_f32
****************************************************************************/
int foc_ramp_run_f32(FAR struct foc_ramp_f32_s *ramp, float des,
float now, FAR float *set);
#endif /* __INDUSTRY_FOC_FLOAT_FOC_RAMP_H */

121
include/industry/foc/float/foc_velocity.h Normal file
View File

@ -0,0 +1,121 @@
/****************************************************************************
* apps/include/industry/foc/float/foc_velocity.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 __INDUSTRY_FOC_FLOAT_FOC_VELOCITY_H
#define __INDUSTRY_FOC_FLOAT_FOC_VELOCITY_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <dsp.h>
#include "industry/foc/float/foc_handler.h"
/****************************************************************************
* Public Type Definition
****************************************************************************/
/* Input to velocity handler */
struct foc_velocity_in_f32_s
{
FAR struct foc_state_f32_s *state; /* FOC state */
float angle; /* Last angle */
float vel; /* Last velocity */
float dir; /* Movement direction */
};
/* Output from velocity handler */
struct foc_velocity_out_f32_s
{
float velocity;
};
/* Forward declaration */
typedef struct foc_velocity_f32_s foc_velocity_f32_t;
/* Velocity operations */
struct foc_velocity_ops_f32_s
{
/* Initialize */
CODE int (*init)(FAR foc_velocity_f32_t *h);
/* Deinitialize */
CODE void (*deinit)(FAR foc_velocity_f32_t *h);
/* Configure */
CODE int (*cfg)(FAR foc_velocity_f32_t *h, FAR void *cfg);
/* Run velocity handler */
CODE void (*run)(FAR foc_velocity_f32_t *h,
FAR struct foc_velocity_in_f32_s *in,
FAR struct foc_velocity_out_f32_s *out);
};
/* Velocity handler - sensor or sensorless */
struct foc_velocity_f32_s
{
FAR struct foc_velocity_ops_f32_s *ops;
FAR void *data;
};
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_velocity_init_f32
****************************************************************************/
int foc_velocity_init_f32(FAR foc_velocity_f32_t *h,
FAR struct foc_velocity_ops_f32_s *ops);
/****************************************************************************
* Name: foc_velocity_deinit_f32
****************************************************************************/
int foc_velocity_deinit_f32(FAR foc_velocity_f32_t *h);
/****************************************************************************
* Name: foc_velocity_cfg_f32
****************************************************************************/
int foc_velocity_cfg_f32(FAR foc_velocity_f32_t *h, FAR void *cfg);
/****************************************************************************
* Name: foc_velocity_run_f32
****************************************************************************/
void foc_velocity_run_f32(FAR foc_velocity_f32_t *h,
FAR struct foc_velocity_in_f32_s *in,
FAR struct foc_velocity_out_f32_s *out);
#endif /* __INDUSTRY_FOC_FLOAT_FOC_VELOCITY_H */

78
include/industry/foc/foc_common.h Normal file
View File

@ -0,0 +1,78 @@
/****************************************************************************
* apps/include/industry/foc/foc_common.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 __APPS_INCLUDE_INDUSTRY_FOC_FOC_COMMON_H
#define __APPS_INCLUDE_INDUSTRY_FOC_FOC_COMMON_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <stdint.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Public Types
****************************************************************************/
/* FOC controller mode */
enum foc_handler_mode_e
{
FOC_HANDLER_MODE_INIT = 0, /* Initial state */
FOC_HANDLER_MODE_IDLE, /* Idle */
FOC_HANDLER_MODE_VOLTAGE, /* Voltage mode - control DQ voltage */
FOC_HANDLER_MODE_CURRENT /* Current mode - control DQ current */
};
/* FOC number type identifiers */
enum foc_number_type_e
{
FOC_NUMBER_TYPE_INVALID = 0,
#ifdef CONFIG_INDUSTRY_FOC_FLOAT
FOC_NUMBER_TYPE_FLOAT = 1, /* float */
#endif
#ifdef CONFIG_INDUSTRY_FOC_FIXED16
FOC_NUMBER_TYPE_FIXED16 = 2, /* b16_t */
#endif
};
/* Speed ramp mode */
enum foc_ramp_mode_e
{
RAMP_MODE_INVALID = 0, /* Reserved */
RAMP_MODE_SOFTSTART = 1, /* Soft start */
RAMP_MODE_SOFTSTOP = 2, /* Soft stop */
RAMP_MODE_NORMAL = 3, /* Normal operation */
};
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
#endif /* __APPS_INCLUDE_INDUSTRY_FOC_FOC_COMMON_H */

42
include/industry/foc/foc_log.h Normal file
View File

@ -0,0 +1,42 @@
/****************************************************************************
* apps/include/industry/foc/foc_log.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 __APPS_INCLUDE_INDUSTRY_FOC_FOC_LOG_H
#define __APPS_INCLUDE_INDUSTRY_FOC_FOC_LOG_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <stdio.h>
#include <inttypes.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define FOCLIBLOG(format, ...) printf(format, ##__VA_ARGS__)
#define FOCLIBERR(format, ...) printf(format, ##__VA_ARGS__)
#define FOCLIBWARN(format, ...) printf(format, ##__VA_ARGS__)
#endif /* __APPS_INCLUDE_INDUSTRY_FOC_FOC_LOG_H */

92
include/industry/foc/foc_utils.h Normal file
View File

@ -0,0 +1,92 @@
/****************************************************************************
* apps/include/industry/foc/foc_utils.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 __APPS_INCLUDE_INDUSTRY_FOC_FOC_UTILS_H
#define __APPS_INCLUDE_INDUSTRY_FOC_FOC_UTILS_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/motor/foc/foc.h>
#include <nuttx/motor/motor_ioctl.h>
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: foc_dev_stop
****************************************************************************/
int foc_dev_stop(int fd);
/****************************************************************************
* Name: foc_dev_start
****************************************************************************/
int foc_dev_start(int fd);
/****************************************************************************
* Name: foc_dev_clearfault
****************************************************************************/
int foc_dev_clearfault(int fd);
/****************************************************************************
* Name: foc_dev_getstate
****************************************************************************/
int foc_dev_getstate(int fd, FAR struct foc_state_s *state);
/****************************************************************************
* Name: foc_dev_setparams
****************************************************************************/
int foc_dev_setparams(int fd, FAR struct foc_params_s *params);
/****************************************************************************
* Name: foc_dev_setcfg
****************************************************************************/
int foc_dev_setcfg(int fd, FAR struct foc_cfg_s *cfg);
/****************************************************************************
* Name: foc_dev_getcfg
****************************************************************************/
int foc_dev_getcfg(int fd, FAR struct foc_cfg_s *cfg);
/****************************************************************************
* Name: foc_dev_getinfo
****************************************************************************/
int foc_dev_getinfo(int fd, FAR struct foc_info_s *info);
/****************************************************************************
* Name: foc_cfg_print
****************************************************************************/
void foc_cfg_print(FAR struct foc_cfg_s *cfg);
#endif /* __APPS_INCLUDE_INDUSTRY_FOC_FOC_UTILS_H */

69
industry/foc/Kconfig Normal file
View File

@ -0,0 +1,69 @@
#
# For a description of the syntax of this configuration file,
# see the file kconfig-language.txt in the NuttX tools repository.
#
config INDUSTRY_FOC
bool "FOC Library"
default n
select LIBDSP
---help---
Enable or disable the FOC Library
if INDUSTRY_FOC
config INDUSTRY_FOC_CORDIC
bool "Enable CORDIC support"
depends on EXPERIMENTAL
default n
---help---
Enable support for CORDIC hardware acceleration
config INDUSTRY_FOC_FIXED16
bool "Enable support for fixed16"
default n
---help---
Enable support for FOC fixed16 calculations
config INDUSTRY_FOC_FLOAT
bool "Enable support for float"
default n
---help---
Enable support for FOC float calculations
config INDUSTRY_FOC_HANDLER_PRINT
bool "FOC handler state printer"
default n
---help---
Enable support for FOC handler state printer
config INDUSTRY_FOC_ANGLE_OPENLOOP
bool "FOC angle open-loop handler"
default y
---help---
Enable support for open-loop angle handler
config INDUSTRY_FOC_CONTROL_PI
bool "FOC PI controller"
default y
---help---
Enable support for classic FOC PI controller
config INDUSTRY_FOC_MODULATION_SVM3
bool "FOC SVM3 modulation"
default y
---help---
Enable support for FOC 3-phase space vector modulation
config INDUSTRY_FOC_MODEL_PMSM
bool "FOC PMSM model support"
select INDUSTRY_FOC_HAVE_MODEL
default n
---help---
Enable support for PMSM model
config INDUSTRY_FOC_HAVE_MODEL
bool
default n
endif

24
industry/foc/Make.defs Normal file
View File

@ -0,0 +1,24 @@
############################################################################
# apps/industry/foc/Make.defs
# Adds selected applications to apps/ build
#
# 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.
#
############################################################################
ifeq ($(CONFIG_INDUSTRY_FOC),y)
CONFIGURED_APPS += $(APPDIR)/industry/foc
endif

85
industry/foc/Makefile Normal file
View File

@ -0,0 +1,85 @@
############################################################################
# apps/industry/foc/Makefile
#
# 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.
#
############################################################################
include $(APPDIR)/Make.defs
# FOC Library
CSRCS = foc_utils.c
# float support
ifeq ($(CONFIG_INDUSTRY_FOC_FLOAT),y)
CSRCS += float/foc_ramp.c
CSRCS += float/foc_handler.c
CSRCS += float/foc_angle.c
CSRCS += float/foc_velocity.c
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP),y)
CSRCS += float/foc_openloop.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_CORDIC),y)
CSRCS += float/foc_cordic.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_CONTROL_PI),y)
CSRCS += float/foc_picontrol.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_MODULATION_SVM3),y)
CSRCS += float/foc_svm3.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_HAVE_MODEL),y)
CSRCS += float/foc_model.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_MODEL_PMSM),y)
CSRCS += float/foc_model_pmsm.c
endif
endif
# fixed16 support
ifeq ($(CONFIG_INDUSTRY_FOC_FIXED16),y)
CSRCS += fixed16/foc_ramp.c
CSRCS += fixed16/foc_handler.c
CSRCS += fixed16/foc_angle.c
CSRCS += fixed16/foc_velocity.c
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP),y)
CSRCS += fixed16/foc_openloop.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_CORDIC),y)
CSRCS += fixed16/foc_cordic.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_CONTROL_PI),y)
CSRCS += fixed16/foc_picontrol.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_MODULATION_SVM3),y)
CSRCS += fixed16/foc_svm3.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_HAVE_MODEL),y)
CSRCS += fixed16/foc_model.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_MODEL_PMSM),y)
CSRCS += fixed16/foc_model_pmsm.c
endif
endif
include $(APPDIR)/Application.mk

157
industry/foc/fixed16/foc_angle.c Normal file
View File

@ -0,0 +1,157 @@
/****************************************************************************
* apps/industry/foc/fixed16/foc_angle.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 <assert.h>
#include <errno.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/fixed16/foc_angle.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_angle_init_b16
*
* Description:
* Initialize the FOC angle handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
* ops - pointer to FOC angle handler operations
*
****************************************************************************/
int foc_angle_init_b16(FAR foc_angle_b16_t *h,
FAR struct foc_angle_ops_b16_s *ops)
{
int ret = OK;
DEBUGASSERT(h);
DEBUGASSERT(ops);
/* Angle ops */
DEBUGASSERT(ops->init);
DEBUGASSERT(ops->deinit);
DEBUGASSERT(ops->cfg);
DEBUGASSERT(ops->run);
/* Reset handler */
memset(h, 0, sizeof(foc_angle_b16_t));
/* Connect ops */
h->ops = ops;
/* Initialize angle */
ret = h->ops->init(h);
if (ret < 0)
{
FOCLIBERR("ERROR: ops->init failed %d\n", ret);
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_angle_deinit_b16
*
* Description:
* De-initialize the FOC angle handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
*
****************************************************************************/
int foc_angle_deinit_b16(FAR foc_angle_b16_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Deinitialize angle */
h->ops->deinit(h);
/* Reset data */
memset(h, 0, sizeof(foc_angle_b16_t));
return ret;
}
/****************************************************************************
* Name: foc_angle_cfg_b16
*
* Description:
* Configure the FOC angle handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
* cfg - pointer to angle handler configuration data
*
****************************************************************************/
int foc_angle_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg)
{
DEBUGASSERT(h);
DEBUGASSERT(cfg);
return h->ops->cfg(h, cfg);
}
/****************************************************************************
* Name: foc_angle_run_b16
*
* Description:
* Process the FOC angle handler data (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
* in - pointer to FOC angle handler input data
* out - pointer to FOC angle handler output data
*
****************************************************************************/
void foc_angle_run_b16(FAR foc_angle_b16_t *h,
FAR struct foc_angle_in_b16_s *in,
FAR struct foc_angle_out_b16_s *out)
{
DEBUGASSERT(h);
DEBUGASSERT(in);
DEBUGASSERT(out);
/* Run angle handler */
h->ops->run(h, in, out);
}

81
industry/foc/fixed16/foc_cordic.c Normal file
View File

@ -0,0 +1,81 @@
/****************************************************************************
* apps/industry/foc/fixed16/foc_cordic.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 <assert.h>
#include "industry/foc/fixed16/foc_cordic.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_cordic_dqsat_b16
*
* Description:
* CORDIC DQ-frame saturation (fixed16)
*
* Input Parameter:
* fd - the file descriptor for CORDIC device
* dq_ref - DQ vector
* mag_max - vector magnitude max
*
****************************************************************************/
int foc_cordic_dqsat_b16(int fd, FAR dq_frame_b16_t *dq, b16_t mag_max)
{
DEBUGASSERT(dq);
/* TODO: */
ASSERT(0);
return OK;
}
/****************************************************************************
* Name: foc_cordic_angle_b16
*
* Description:
* CORDIC angle update (fixed16)
*
* Input Parameter:
* fd - the file descriptor for CORDIC device
* angle - phase angle data
* a - phase angle in rad
*
****************************************************************************/
int foc_cordic_angle_b16(int fd, FAR phase_angle_b16_t *angle, b16_t a)
{
DEBUGASSERT(angle);
/* TODO: */
ASSERT(0);
return OK;
}

345
industry/foc/fixed16/foc_handler.c Normal file
View File

@ -0,0 +1,345 @@
/****************************************************************************
* apps/industry/foc/fixed16/foc_handler.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 <assert.h>
#include <errno.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/foc_common.h"
#include "industry/foc/fixed16/foc_handler.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_handler_init_b16
*
* Description:
* Initialize the FOC handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
* ctrl - pointer to controller operations
* mod - pointer to modulation operations
*
****************************************************************************/
int foc_handler_init_b16(FAR foc_handler_b16_t *h,
FAR struct foc_control_ops_b16_s *ctrl,
FAR struct foc_modulation_ops_b16_s *mod)
{
int ret = OK;
DEBUGASSERT(h);
DEBUGASSERT(ctrl);
DEBUGASSERT(mod);
/* Controller ops */
DEBUGASSERT(ctrl->init);
DEBUGASSERT(ctrl->deinit);
DEBUGASSERT(ctrl->cfg);
DEBUGASSERT(ctrl->input_set);
DEBUGASSERT(ctrl->voltage_run);
DEBUGASSERT(ctrl->current_run);
DEBUGASSERT(ctrl->state_get);
/* Modulation ops */
DEBUGASSERT(mod->init);
DEBUGASSERT(mod->deinit);
DEBUGASSERT(mod->cfg);
DEBUGASSERT(mod->current);
DEBUGASSERT(mod->vbase_get);
DEBUGASSERT(mod->run);
/* Reset handler */
memset(h, 0, sizeof(foc_handler_b16_t));
/* Connect ops */
h->ops.ctrl = ctrl;
h->ops.mod = mod;
/* Initialize control handler */
ret = h->ops.mod->init(h);
if (ret < 0)
{
FOCLIBERR("ERROR: mod->init failed %d\n", ret);
goto errout;
}
/* Initialize modulation handler */
ret = h->ops.ctrl->init(h);
if (ret < 0)
{
FOCLIBERR("ERROR: ctrl->init failed %d\n", ret);
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_handler_deinit_b16
*
* Description:
* De-initialize the FOC handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
*
****************************************************************************/
int foc_handler_deinit_b16(FAR foc_handler_b16_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Deinitialize modulation handler */
h->ops.mod->deinit(h);
/* Deinitialize control handler */
h->ops.ctrl->deinit(h);
/* Reset data */
memset(h, 0, sizeof(foc_handler_b16_t));
return ret;
}
/****************************************************************************
* Name: foc_handler_cfg_b16
*
* Description:
* Configure the FOC handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
* ctrl_cfg - pointer to controller configuration data
* mod_cfg - pointer to modulation configuration data
*
****************************************************************************/
void foc_handler_cfg_b16(FAR foc_handler_b16_t *h,
FAR void *ctrl_cfg,
FAR void *mod_cfg)
{
DEBUGASSERT(h);
DEBUGASSERT(ctrl_cfg);
DEBUGASSERT(mod_cfg);
/* Configure controller */
h->ops.ctrl->cfg(h, ctrl_cfg);
/* Configure modulation */
h->ops.mod->cfg(h, mod_cfg);
}
/****************************************************************************
* Name: foc_handler_run_b16
*
* Description:
* Run the FOC handler and process the given input data (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
* in - pointer to FOC handler input data
* out - pointer to FOC handler output data
*
****************************************************************************/
int foc_handler_run_b16(FAR foc_handler_b16_t *h,
FAR struct foc_handler_input_b16_s *in,
FAR struct foc_handler_output_b16_s *out)
{
ab_frame_b16_t v_ab_mod;
b16_t vbase = 0;
int ret = OK;
DEBUGASSERT(h);
DEBUGASSERT(in);
DEBUGASSERT(out);
/* Do nothing if control mode not specified yet.
* This also protects against initial state when the controller is
* started but input data has not yet been provided.
*/
if (in->mode <= FOC_HANDLER_MODE_INIT)
{
ret = -EINVAL;
goto errout;
}
/* Correct current samples according to modulation state */
h->ops.mod->current(h, in->current);
/* Get VBASE */
h->ops.mod->vbase_get(h, in->vbus, &vbase);
/* Feed controller with phase currents */
h->ops.ctrl->input_set(h, in->current, vbase, in->angle);
/* Call controller */
switch (in->mode)
{
/* IDLE */
case FOC_HANDLER_MODE_IDLE:
{
/* Do nothing and set duty to zeros */
ret = OK;
goto errout;
}
/* FOC current mode - control DQ-current */
case FOC_HANDLER_MODE_CURRENT:
{
/* Current controller */
h->ops.ctrl->current_run(h,
in->dq_ref,
in->vdq_comp,
&v_ab_mod);
break;
}
/* FOC voltage mode - control DQ-voltage */
case FOC_HANDLER_MODE_VOLTAGE:
{
/* Voltage controller */
h->ops.ctrl->voltage_run(h,
in->dq_ref,
&v_ab_mod);
break;
}
/* Otherwise - we should not be here */
default:
{
ret = -EINVAL;
goto errout;
}
}
/* Duty cycle modulation */
h->ops.mod->run(h, &v_ab_mod, out->duty);
return ret;
errout:
/* Set duty to zeros */
memset(out->duty, 0, sizeof(b16_t) * CONFIG_MOTOR_FOC_PHASES);
return ret;
}
/****************************************************************************
* Name: foc_handler_state_b16
*
* Description:
* Get FOC handler state (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
* state - pointer to FOC state data
*
****************************************************************************/
void foc_handler_state_b16(FAR foc_handler_b16_t *h,
FAR struct foc_state_b16_s *state)
{
DEBUGASSERT(h);
DEBUGASSERT(state);
h->ops.ctrl->state_get(h, state);
}
#ifdef CONFIG_INDUSTRY_FOC_HANDLER_PRINT
/****************************************************************************
* Name: foc_handler_state_print_b16
*
* Description:
* Print FOC handler state (fixed16)
*
* Input Parameter:
* state - pointer to FOC state data
*
****************************************************************************/
void foc_handler_state_print_b16(FAR struct foc_state_b16_s *state)
{
DEBUGASSERT(state);
#if CONFIG_MOTOR_FOC_PHASES == 3
FOCLIBLOG("curr = [%.2f %.2f %.2f]\n",
b16tof(state->curr[0]),
b16tof(state->curr[1]),
b16tof(state->curr[2]));
FOCLIBLOG("volt = [%.2f %.2f %.2f]\n",
b16tof(state->volt[0]),
b16tof(state->volt[1]),
b16tof(state->volt[2]));
#else
# error TODO
#endif
FOCLIBLOG("iab = [%.2f %.2f]\n",
b16tof(state->iab.a), b16tof(state->iab.b));
FOCLIBLOG("vab = [%.2f %.2f]\n",
b16tof(state->vab.a), b16tof(state->vab.b));
FOCLIBLOG("idq = [%.2f %.2f]\n",
b16tof(state->idq.d), b16tof(state->idq.q));
FOCLIBLOG("vdq = [%.2f %.2f]\n",
b16tof(state->vdq.d), b16tof(state->vdq.q));
}
#endif

188
industry/foc/fixed16/foc_model.c Normal file
View File

@ -0,0 +1,188 @@
/****************************************************************************
* apps/industry/foc/fixed16/foc_model.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 <assert.h>
#include <errno.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/foc_common.h"
#include "industry/foc/fixed16/foc_model.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_model_init_b16
*
* Description:
* Initialize the FOC model (fixed16)
*
* Input Parameter:
* h - pointer to FOC model handler
* ops - pointer to FOC model operations
*
****************************************************************************/
int foc_model_init_b16(FAR foc_model_b16_t *h,
FAR struct foc_model_ops_b16_s *ops)
{
int ret = OK;
DEBUGASSERT(h);
DEBUGASSERT(ops);
/* Model ops */
DEBUGASSERT(ops->init);
DEBUGASSERT(ops->deinit);
DEBUGASSERT(ops->cfg);
DEBUGASSERT(ops->ele_run);
DEBUGASSERT(ops->mech_run);
DEBUGASSERT(ops->state);
/* Reset handler */
memset(h, 0, sizeof(foc_model_b16_t));
/* Connect ops */
h->ops = ops;
/* Initialize model */
ret = h->ops->init(h);
if (ret < 0)
{
FOCLIBERR("ERROR: ops->init failed %d\n", ret);
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_model_deinit_b16
*
* Description:
* De-initialize the FOC model (fixed16)
*
* Input Parameter:
* h - pointer to FOC model handler
*
****************************************************************************/
int foc_model_deinit_b16(FAR foc_model_b16_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Deinitialize model */
h->ops->deinit(h);
/* Reset data */
memset(h, 0, sizeof(foc_model_b16_t));
return ret;
}
/****************************************************************************
* Name: foc_model_cfg_b16
*
* Description:
* Configure the FOC model (fixed16)
*
* Input Parameter:
* h - pointer to FOC model handler
* cfg - pointer to FOC model configuration data
*
****************************************************************************/
int foc_model_cfg_b16(FAR foc_model_b16_t *h, FAR void *cfg)
{
DEBUGASSERT(h);
DEBUGASSERT(cfg);
return h->ops->cfg(h, cfg);
}
/****************************************************************************
* Name: foc_model_run_b16
*
* Description:
* Run the FOC model (fixed16)
*
* Input Parameter:
* h - pointer to FOC model handler
* load - applied load
* v_ab - applied voltage in alpha-beta frame
*
* REVISIT:
* It would be better if we feed model with duty cycle and VBUS, but it
* complicates the calculations, so we leave it like that for now
*
****************************************************************************/
void foc_model_run_b16(FAR foc_model_b16_t *h,
b16_t load,
FAR ab_frame_b16_t *v_ab)
{
DEBUGASSERT(h);
DEBUGASSERT(v_ab);
/* Run electrical model */
h->ops->ele_run(h, v_ab);
/* Run mechanical model */
h->ops->mech_run(h, load);
}
/****************************************************************************
* Name: foc_model_state_b16
*
* Description:
* Get model state (fixed16)
*
* Input Parameter:
* h - pointer to FOC model handler
* state - pointer to FOC model state
*
****************************************************************************/
void foc_model_state_b16(FAR foc_model_b16_t *h,
FAR struct foc_model_state_b16_s *state)
{
DEBUGASSERT(h);
DEBUGASSERT(state);
h->ops->state(h, state);
}

300
industry/foc/fixed16/foc_model_pmsm.c Normal file
View File

@ -0,0 +1,300 @@
/****************************************************************************
* apps/industry/foc/fixed16/foc_model_pmsm.c
* This file implements PMSM model for fixed16
*
* 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 <assert.h>
#include <errno.h>
#include <stdlib.h>
#include "industry/foc/fixed16/foc_model.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#if CONFIG_MOTOR_FOC_PHASES != 3
# error
#endif
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/* PMSM model data */
struct foc_model_pmsm_b16_s
{
b16_t one_by_iphadc;
struct foc_model_pmsm_cfg_b16_s cfg;
struct pmsm_model_b16_s model;
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int foc_model_pmsm_init_b16(FAR foc_model_b16_t *h);
static void foc_model_pmsm_deinit_b16(FAR foc_model_b16_t *h);
static int foc_model_pmsm_cfg_b16(FAR foc_model_b16_t *h, FAR void *cfg);
static void foc_model_pmsm_elerun_b16(FAR foc_model_b16_t *h,
FAR ab_frame_b16_t *v_ab);
static void foc_model_pmsm_mechrun_b16(FAR foc_model_b16_t *h, b16_t load);
static void foc_model_pmsm_state_b16(FAR foc_model_b16_t *h,
FAR struct foc_model_state_b16_s *state);
/****************************************************************************
* Public Data
****************************************************************************/
/* FOC model fixed16 interface */
struct foc_model_ops_b16_s g_foc_model_pmsm_ops_b16 =
{
.init = foc_model_pmsm_init_b16,
.deinit = foc_model_pmsm_deinit_b16,
.cfg = foc_model_pmsm_cfg_b16,
.ele_run = foc_model_pmsm_elerun_b16,
.mech_run = foc_model_pmsm_mechrun_b16,
.state = foc_model_pmsm_state_b16,
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: foc_model_pmsm_init_b16
*
* Description:
* Initialize PMSM model (fixed16)
*
* Input Parameter:
* h - pointer to FOC model handler
*
****************************************************************************/
static int foc_model_pmsm_init_b16(FAR foc_model_b16_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Connect model data */
h->model = zalloc(sizeof(struct foc_model_pmsm_b16_s));
if (h->model == NULL)
{
ret = -ENOMEM;
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_model_pmsm_deinit_b16
*
* Description:
* Deinitialize PMSM model (fixed16)
*
* Input Parameter:
* h - pointer to FOC model handler
*
****************************************************************************/
static void foc_model_pmsm_deinit_b16(FAR foc_model_b16_t *h)
{
DEBUGASSERT(h);
/* Free model data */
if (h->model)
{
free (h->model);
}
}
/****************************************************************************
* Name: foc_model_pmsm_cfg_b16
*
* Description:
* Configure PMSM model (fixed16)
*
* Input Parameter:
* h - pointer to FOC model handler
* cfg - pointer to FOC model configuration (struct foc_model_pmsm_b16_s)
*
****************************************************************************/
static int foc_model_pmsm_cfg_b16(FAR foc_model_b16_t *h, FAR void *cfg)
{
FAR struct foc_model_pmsm_b16_s *model = NULL;
struct pmsm_phy_params_b16_s phy;
DEBUGASSERT(h);
DEBUGASSERT(cfg);
/* Get model data */
DEBUGASSERT(h->model);
model = h->model;
/* Copy data */
memcpy(&model->cfg, cfg, sizeof(struct foc_model_pmsm_cfg_b16_s));
/* Initialize model */
pmsm_phy_params_init_b16(&phy,
model->cfg.poles,
model->cfg.res,
model->cfg.ind,
model->cfg.iner,
model->cfg.flux_link,
model->cfg.ind_q,
model->cfg.ind_d);
pmsm_model_initialize_b16(&model->model, &phy, model->cfg.per);
/* Get one by iphase_adc */
model->one_by_iphadc = b16divb16(b16ONE, model->cfg.iphase_adc);
return OK;
}
/****************************************************************************
* Name: foc_model_pmsm_elerun_b16
*
* Description:
* Run model electrical simulation (fixed16)
*
* Input Parameter:
* h - pointer to FOC model handler
* v_ab - applied voltage in alpha-beta frame
*
****************************************************************************/
static void foc_model_pmsm_elerun_b16(FAR foc_model_b16_t *h,
FAR ab_frame_b16_t *v_ab)
{
FAR struct foc_model_pmsm_b16_s *model = NULL;
DEBUGASSERT(h);
DEBUGASSERT(v_ab);
/* Get model data */
DEBUGASSERT(h->model);
model = h->model;
/* Run electrical model */
pmsm_model_elec_b16(&model->model, v_ab);
}
/****************************************************************************
* Name: foc_model_pmsm_mechrun_b16
*
* Description:
* Run model mechanical simulation (fixed16)
*
* Input Parameter:
* h - pointer to FOC model handler
* load - applied load
*
****************************************************************************/
static void foc_model_pmsm_mechrun_b16(FAR foc_model_b16_t *h, b16_t load)
{
FAR struct foc_model_pmsm_b16_s *model = NULL;
DEBUGASSERT(h);
/* Get model data */
DEBUGASSERT(h->model);
model = h->model;
/* Run mechanical model */
pmsm_model_mech_b16(&model->model, load);
}
/****************************************************************************
* Name: foc_model_pmsm_state_b16
*
* Description:
* Get the model state (fixed16)
*
* Input Parameter:
* h - pointer to FOC model handler
* state - pointer to FOC model state
*
****************************************************************************/
static void foc_model_pmsm_state_b16(FAR foc_model_b16_t *h,
FAR struct foc_model_state_b16_s *state)
{
FAR struct foc_model_pmsm_b16_s *model = NULL;
DEBUGASSERT(h);
DEBUGASSERT(state);
/* Get model data */
DEBUGASSERT(h->model);
model = h->model;
/* Get model state */
state->volt[0] = model->model.state.v_abc.a;
state->volt[1] = model->model.state.v_abc.b;
state->volt[2] = model->model.state.v_abc.c;
state->vab.a = model->model.state.v_ab.a;
state->vab.b = model->model.state.v_ab.b;
state->vdq.d = model->model.state.v_dq.d;
state->vdq.q = model->model.state.v_dq.q;
state->curr[0] = model->model.state.i_abc.a;
state->curr[1] = model->model.state.i_abc.b;
state->curr[2] = model->model.state.i_abc.c;
state->iab.a = model->model.state.i_ab.a;
state->iab.b = model->model.state.i_ab.b;
state->idq.d = model->model.state.i_dq.d;
state->idq.q = model->model.state.i_dq.q;
state->omega_e = model->model.state.omega_e;
state->omega_m = model->model.state.omega_m;
/* Get RAW currents */
state->curr_raw[0] = b16toi(b16mulb16(state->curr[0],
model->one_by_iphadc));
state->curr_raw[1] = b16toi(b16mulb16(state->curr[1],
model->one_by_iphadc));
state->curr_raw[2] = b16toi(b16mulb16(state->curr[2],
model->one_by_iphadc));
}

195
industry/foc/fixed16/foc_openloop.c Normal file
View File

@ -0,0 +1,195 @@
/****************************************************************************
* apps/industry/foc/fixed16/foc_openloop.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 <assert.h>
#include <errno.h>
#include <stdlib.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/fixed16/foc_angle.h"
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/* Open-loop data */
struct foc_openloop_b16_s
{
struct foc_openloop_cfg_b16_s cfg;
struct openloop_data_b16_s data;
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int foc_angle_ol_init_b16(FAR foc_angle_b16_t *h);
static void foc_angle_ol_deinit_b16(FAR foc_angle_b16_t *h);
static int foc_angle_ol_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg);
static void foc_angle_ol_run_b16(FAR foc_angle_b16_t *h,
FAR struct foc_angle_in_b16_s *in,
FAR struct foc_angle_out_b16_s *out);
/****************************************************************************
* Public Data
****************************************************************************/
struct foc_angle_ops_b16_s g_foc_angle_ol_b16 =
{
.init = foc_angle_ol_init_b16,
.deinit = foc_angle_ol_deinit_b16,
.cfg = foc_angle_ol_cfg_b16,
.run = foc_angle_ol_run_b16,
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: foc_angle_ol_init_b16
*
* Description:
* Initialize the open-loop FOC angle handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
*
****************************************************************************/
static int foc_angle_ol_init_b16(FAR foc_angle_b16_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Connect angle data */
h->data = zalloc(sizeof(struct foc_openloop_b16_s));
if (h->data == NULL)
{
ret = -ENOMEM;
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_angle_ol_deinit_b16
*
* Description:
* De-initialize the open-loop FOC angle handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
*
****************************************************************************/
static void foc_angle_ol_deinit_b16(FAR foc_angle_b16_t *h)
{
DEBUGASSERT(h);
/* Free angle data */
if (h->data)
{
free(h->data);
}
}
/****************************************************************************
* Name: foc_angle_ol_cfg_b16
*
* Description:
* Configure the open-loop FOC angle handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
* cfg - pointer to angle handler configuration data
* (struct foc_openloop_b16_s)
*
****************************************************************************/
static int foc_angle_ol_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg)
{
FAR struct foc_openloop_b16_s *ol = NULL;
DEBUGASSERT(h);
/* Get modulation data */
DEBUGASSERT(h->data);
ol = h->data;
/* Copy configuration */
memcpy(&ol->cfg, cfg, sizeof(struct foc_openloop_cfg_b16_s));
/* Initialize open-loop controller data */
DEBUGASSERT(ol->cfg.per > 0);
motor_openloop_init_b16(&ol->data, ol->cfg.per);
return OK;
}
/****************************************************************************
* Name: foc_angle_ol_run_b16
*
* Description:
* Process the FOC open-loop angle data (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
* in - pointer to FOC angle handler input data
* out - pointer to FOC angle handler output data
*
****************************************************************************/
static void foc_angle_ol_run_b16(FAR foc_angle_b16_t *h,
FAR struct foc_angle_in_b16_s *in,
FAR struct foc_angle_out_b16_s *out)
{
FAR struct foc_openloop_b16_s *ol = NULL;
DEBUGASSERT(h);
/* Get modulation data */
DEBUGASSERT(h->data);
ol = h->data;
/* Update open-loop */
motor_openloop_b16(&ol->data, in->vel, in->dir);
/* Get open-loop angle */
out->angle = motor_openloop_angle_get_b16(&ol->data);
}

417
industry/foc/fixed16/foc_picontrol.c Normal file
View File

@ -0,0 +1,417 @@
/****************************************************************************
* apps/industry/foc/fixed16/foc_picontrol.c
* This file implements classical FOC PI current controller for fixed16
*
* 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 <assert.h>
#include <errno.h>
#include <stdlib.h>
#include "industry/foc/fixed16/foc_handler.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#if CONFIG_MOTOR_FOC_PHASES != 3
# error
#endif
/****************************************************************************
* Private Data Types
****************************************************************************/
/* FOC PI fixed16 controller data */
struct foc_picontrol_b16_s
{
b16_t vbase_last; /* Last VBASE sample */
phase_angle_b16_t angle; /* Phase angle */
struct foc_initdata_b16_s cfg; /* Controller configuration */
struct foc_data_b16_s data; /* Controller private data */
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int foc_control_init_b16(FAR foc_handler_b16_t *h);
static void foc_control_deinit_b16(FAR foc_handler_b16_t *h);
static void foc_control_cfg_b16(FAR foc_handler_b16_t *h, FAR void *cfg);
static void foc_control_input_set_b16(FAR foc_handler_b16_t *h,
FAR b16_t *current,
b16_t vbase,
b16_t angle);
static void foc_control_voltage_run_b16(FAR foc_handler_b16_t *h,
FAR dq_frame_b16_t *dq_ref,
FAR ab_frame_b16_t *v_ab_mod);
static void foc_control_current_run_b16(FAR foc_handler_b16_t *h,
FAR dq_frame_b16_t *dq_ref,
FAR dq_frame_b16_t *vdq_comp,
FAR ab_frame_b16_t *v_ab_mod);
static void foc_control_state_get_b16(FAR foc_handler_b16_t *h,
FAR struct foc_state_b16_s *state);
/****************************************************************************
* Public Data
****************************************************************************/
/* FOC control fixed16 interface */
struct foc_control_ops_b16_s g_foc_control_pi_b16 =
{
.init = foc_control_init_b16,
.deinit = foc_control_deinit_b16,
.cfg = foc_control_cfg_b16,
.input_set = foc_control_input_set_b16,
.voltage_run = foc_control_voltage_run_b16,
.current_run = foc_control_current_run_b16,
.state_get = foc_control_state_get_b16,
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: foc_control_init_b16
*
* Description:
* Initialize the FOC PI controller (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
*
****************************************************************************/
static int foc_control_init_b16(FAR foc_handler_b16_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Connect controller data */
h->control = zalloc(sizeof(struct foc_picontrol_b16_s));
if (h->control == NULL)
{
ret = -ENOMEM;
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_control_deinit_b16
*
* Description:
* Deinitialize the FOC PI controller (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
*
****************************************************************************/
static void foc_control_deinit_b16(FAR foc_handler_b16_t *h)
{
DEBUGASSERT(h);
/* Free controller data */
if (h->control)
{
free(h->control);
}
}
/****************************************************************************
* Name: foc_control_cfg_set_b16
*
* Description:
* Configure the FOC controller (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
* cfg - pointer to controller configuration data
* (struct foc_picontrol_b16_s)
*
****************************************************************************/
static void foc_control_cfg_b16(FAR foc_handler_b16_t *h, FAR void *cfg)
{
FAR struct foc_picontrol_b16_s *foc = NULL;
DEBUGASSERT(h);
DEBUGASSERT(cfg);
/* Get controller data */
DEBUGASSERT(h->control);
foc = h->control;
/* Copy data */
memcpy(&foc->cfg, cfg, sizeof(struct foc_initdata_b16_s));
/* Initialize FOC controller data */
foc_init_b16(&foc->data, &foc->cfg);
}
/****************************************************************************
* Name: foc_control_input_set_b16
*
* Description:
* Update input for controller (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
* current - phase currents in amps
* vbase - base voltage for controller
* angle - phase angle in rad
*
****************************************************************************/
static void foc_control_input_set_b16(FAR foc_handler_b16_t *h,
FAR b16_t *current,
b16_t vbase,
b16_t angle)
{
FAR struct foc_picontrol_b16_s *foc = NULL;
abc_frame_b16_t i_abc;
DEBUGASSERT(h);
DEBUGASSERT(current);
/* Get controller data */
DEBUGASSERT(h->control);
foc = h->control;
/* Get current in abc frame */
i_abc.a = current[0];
i_abc.b = current[1];
i_abc.c = current[2];
foc_iabc_update_b16(&foc->data, &i_abc);
/* Update base voltage only if changed */
if (foc->vbase_last != vbase)
{
/* Update FOC base voltage */
foc_vbase_update_b16(&foc->data, vbase);
/* Update last FOC base voltage */
foc->vbase_last = vbase;
}
/* Update phase angle */
#ifndef CONFIG_INDUSTRY_FOC_CORDIC
phase_angle_update_b16(&foc->angle, angle);
#else
foc_cordic_angle_b16(h->fd, &foc->angle, angle);
#endif
/* Feed the controller with phase angle */
foc_angle_update_b16(&foc->data, &foc->angle);
}
/****************************************************************************
* Name: foc_control_voltage_b16
*
* Description:
* Handle the FOC voltage control (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
* dq_ref - DQ voltage reference frame
* v_ab_mod - (out) modulation alpha-veta voltage
*
****************************************************************************/
static void foc_control_voltage_run_b16(FAR foc_handler_b16_t *h,
FAR dq_frame_b16_t *dq_ref,
FAR ab_frame_b16_t *v_ab_mod)
{
FAR struct foc_picontrol_b16_s *foc = NULL;
b16_t mag_max = 0;
DEBUGASSERT(h);
DEBUGASSERT(dq_ref);
DEBUGASSERT(v_ab_mod);
/* Get controller data */
DEBUGASSERT(h->control);
foc = h->control;
/* Get maximum possible voltage DQ vetor magnitude */
foc_vdq_mag_max_get_b16(&foc->data, &mag_max);
/* Saturate voltage DQ vector */
#ifndef CONFIG_INDUSTRY_FOC_CORDIC
dq_saturate_b16(dq_ref, mag_max);
#else
foc_cordic_dqsat_b16(h->fd, dq_ref, mag_max);
#endif
/* Call FOC voltage controller */
foc_voltage_control_b16(&foc->data, dq_ref);
/* Get output v_ab_mod frame */
foc_vabmod_get_b16(&foc->data, v_ab_mod);
}
/****************************************************************************
* Name: foc_control_current_b16
*
* Description:
* Handle the FOC current control (fixed16)
*
* h - pointer to FOC handler
* dq_ref - DQ current reference frame
* vdq_comp - DQ voltage compensation
* v_ab_mod - (out) modulation alpha-veta voltage
*
****************************************************************************/
static void foc_control_current_run_b16(FAR foc_handler_b16_t *h,
FAR dq_frame_b16_t *dq_ref,
FAR dq_frame_b16_t *vdq_comp,
FAR ab_frame_b16_t *v_ab_mod)
{
FAR struct foc_picontrol_b16_s *foc = NULL;
dq_frame_b16_t v_dq_ref;
b16_t mag_max = 0;
DEBUGASSERT(h);
DEBUGASSERT(dq_ref);
DEBUGASSERT(vdq_comp);
DEBUGASSERT(v_ab_mod);
/* Get controller data */
DEBUGASSERT(h->control);
foc = h->control;
/* Reset voltage reference */
v_dq_ref.d = 0;
v_dq_ref.q = 0;
/* Call FOC current controller */
foc_current_control_b16(&foc->data, dq_ref, vdq_comp, &v_dq_ref);
/* Get maximum possible voltage DQ vetor magnitude */
foc_vdq_mag_max_get_b16(&foc->data, &mag_max);
/* Saturate voltage DQ vector */
#ifndef CONFIG_INDUSTRY_FOC_CORDIC
dq_saturate_b16(dq_ref, mag_max);
#else
foc_cordic_dqsat_b16(h->fd, dq_ref, mag_max);
#endif
/* Call FOC voltage control */
foc_voltage_control_b16(&foc->data, &v_dq_ref);
/* Get output v_ab_mod frame */
foc_vabmod_get_b16(&foc->data, v_ab_mod);
}
/****************************************************************************
* Name: foc_control_state_get_b16
*
* Description:
* Get the FOC controller state (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
* state - (out) pointer to FOC state data
*
****************************************************************************/
static void foc_control_state_get_b16(FAR foc_handler_b16_t *h,
FAR struct foc_state_b16_s *state)
{
FAR struct foc_picontrol_b16_s *foc = NULL;
DEBUGASSERT(h);
DEBUGASSERT(state);
/* Get controller data */
DEBUGASSERT(h->control);
foc = h->control;
/* Copy DQ voltage */
state->vdq.q = foc->data.v_dq.q;
state->vdq.d = foc->data.v_dq.d;
/* Copy DQ current */
state->idq.q = foc->data.i_dq.q;
state->idq.d = foc->data.i_dq.d;
/* Copy alpha-beta current */
state->iab.a = foc->data.i_ab.a;
state->iab.b = foc->data.i_ab.b;
/* Copy alpha-beta voltage */
state->vab.a = foc->data.v_ab.a;
state->vab.b = foc->data.v_ab.b;
/* Copy phase current */
state->curr[0] = foc->data.i_abc.a;
state->curr[1] = foc->data.i_abc.b;
state->curr[2] = foc->data.i_abc.c;
/* Copy phase voltage */
state->volt[0] = foc->data.v_abc.a;
state->volt[1] = foc->data.v_abc.b;
state->volt[2] = foc->data.v_abc.c;
}

178
industry/foc/fixed16/foc_ramp.c Normal file
View File

@ -0,0 +1,178 @@
/****************************************************************************
* apps/industry/foc/fixed16/foc_ramp.c
* This file implements ramp for fixed16
*
* 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 <assert.h>
#include "industry/foc/foc_common.h"
#include "industry/foc/fixed16/foc_ramp.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_ramp_init_b16
*
* Description:
* Initialize ramp (fixed16)
*
* Input Parameter:
* ramp - pointer to ramp handler
* per - ramp period
* thr - ramp threshold
* acc - ramp acceleration
* dec - ramp decceleration
*
****************************************************************************/
int foc_ramp_init_b16(FAR struct foc_ramp_b16_s *ramp, b16_t per,
b16_t thr, b16_t acc, b16_t dec)
{
DEBUGASSERT(ramp);
DEBUGASSERT(per > 0);
DEBUGASSERT(thr > 0);
DEBUGASSERT(acc > 0);
DEBUGASSERT(dec > 0);
ramp->per = per;
ramp->ramp_thr = thr;
ramp->ramp_acc = acc;
ramp->ramp_dec = dec;
ramp->ramp_acc_per = b16mulb16(ramp->ramp_acc, ramp->per);
ramp->ramp_dec_per = b16mulb16(ramp->ramp_dec, ramp->per);
return OK;
}
/****************************************************************************
* Name: foc_ramp_run_b16
*
* Description:
* Handle ramp (fixed16)
*
* Input Parameter:
* ramp - pointer to ramp handler
* des - ramp parameter destination
* now - ramp parameter now
* set - (out) ramp parameter set
*
****************************************************************************/
int foc_ramp_run_b16(FAR struct foc_ramp_b16_s *ramp, b16_t des,
b16_t now, FAR b16_t *set)
{
DEBUGASSERT(ramp);
/* Check if we require soft start/stop operation.
* Only if the user set point differs from the driver set point
*/
if (des != *set)
{
ramp->diff = des - *set;
if (ramp->diff >= ramp->ramp_thr)
{
ramp->ramp_mode = RAMP_MODE_SOFTSTART;
}
else if (ramp->diff <= -ramp->ramp_thr)
{
ramp->ramp_mode = RAMP_MODE_SOFTSTOP;
}
else
{
/* Just set new setpoint */
*set = des;
ramp->ramp_mode = RAMP_MODE_NORMAL;
}
}
else
{
ramp->ramp_mode = RAMP_MODE_NORMAL;
}
/* Handle according to current motor state */
switch (ramp->ramp_mode)
{
case RAMP_MODE_NORMAL:
{
/* Nothing to do here ? */
break;
}
case RAMP_MODE_SOFTSTART:
{
if (des - *set >= ramp->ramp_thr)
{
/* Increase setpoint with ramp */
*set = now + ramp->ramp_acc_per;
}
else
{
/* Set final setpoint and exit soft start */
*set = des;
ramp->ramp_mode = RAMP_MODE_NORMAL;
}
break;
}
case RAMP_MODE_SOFTSTOP:
{
if (des - *set <= -ramp->ramp_thr)
{
/* Stop motor with ramp */
*set = now - ramp->ramp_dec_per;
}
else
{
/* Set final setpoint and exit soft stop */
*set = des;
ramp->ramp_mode = RAMP_MODE_NORMAL;
}
break;
}
default:
{
/* We should not be here */
DEBUGASSERT(0);
break;
}
}
return OK;
}

287
industry/foc/fixed16/foc_svm3.c Normal file
View File

@ -0,0 +1,287 @@
/****************************************************************************
* apps/industry/foc/fixed16/foc_svm3.c
* This file implements 3-phase space vector modulation for fixed16
*
* 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 <assert.h>
#include <errno.h>
#include <stdlib.h>
#include "industry/foc/fixed16/foc_handler.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#if CONFIG_MOTOR_FOC_PHASES != 3
# error
#endif
/* Enable current samples correction if 3-shunts */
#if CONFIG_MOTOR_FOC_SHUNTS == 3
# define FOC_CORRECT_CURRENT_SAMPLES 1
#endif
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/* SVM3 data */
struct foc_svm3mod_b16_s
{
struct foc_mod_cfg_b16_s cfg;
struct svm3_state_b16_s state;
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int foc_modulation_init_b16(FAR foc_handler_b16_t *h);
static void foc_modulation_deinit_b16(FAR foc_handler_b16_t *h);
static void foc_modulation_cfg_b16(FAR foc_handler_b16_t *h, FAR void *cfg);
static void foc_modulation_current_b16(FAR foc_handler_b16_t *h,
FAR b16_t *curr);
static void foc_modulation_vbase_get_b16(FAR foc_handler_b16_t *h,
b16_t vbus,
FAR b16_t *vbase);
static void foc_modulation_run_b16(FAR foc_handler_b16_t *h,
FAR ab_frame_b16_t *v_ab_mod,
FAR b16_t *duty);
/****************************************************************************
* Public Data
****************************************************************************/
/* FOC modulation fixed16 interface */
struct foc_modulation_ops_b16_s g_foc_mod_svm3_b16 =
{
.init = foc_modulation_init_b16,
.deinit = foc_modulation_deinit_b16,
.cfg = foc_modulation_cfg_b16,
.current = foc_modulation_current_b16,
.vbase_get = foc_modulation_vbase_get_b16,
.run = foc_modulation_run_b16,
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: foc_modulation_init_b16
*
* Description:
* Initialize the SVM3 modulation (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
*
****************************************************************************/
static int foc_modulation_init_b16(FAR foc_handler_b16_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Connect modulation data */
h->modulation = zalloc(sizeof(struct foc_svm3mod_b16_s));
if (h->modulation == NULL)
{
ret = -ENOMEM;
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_modulation_deinit_b16
*
* Description:
* Deinitialize the SVM3 modulation (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
*
****************************************************************************/
static void foc_modulation_deinit_b16(FAR foc_handler_b16_t *h)
{
DEBUGASSERT(h);
/* Free modulation data */
if (h->modulation)
{
free(h->modulation);
}
}
/****************************************************************************
* Name: foc_modulation_cfg_b16
*
* Description:
* Configure the SVM3 modulation (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
* cfg - pointer to modulation configuration data
* (struct foc_svm3mod_b16_s)
*
****************************************************************************/
static void foc_modulation_cfg_b16(FAR foc_handler_b16_t *h, FAR void *cfg)
{
FAR struct foc_svm3mod_b16_s *svm = NULL;
DEBUGASSERT(h);
DEBUGASSERT(cfg);
/* Get modulation data */
DEBUGASSERT(h->modulation);
svm = h->modulation;
/* Copy data */
memcpy(&svm->cfg, cfg, sizeof(struct foc_mod_cfg_b16_s));
/* Initialize SVM3 data */
svm3_init_b16(&svm->state);
}
/****************************************************************************
* Name: foc_modulation_vbase_get_b16
*
* Description:
* Get the modulation base voltage (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
* vbus - bus voltage
* vbase - (out) pointer to base voltage from modulation
*
****************************************************************************/
static void foc_modulation_vbase_get_b16(FAR foc_handler_b16_t *h,
b16_t vbus,
FAR b16_t *vbase)
{
DEBUGASSERT(h);
DEBUGASSERT(vbus);
DEBUGASSERT(vbase);
/* Get maximum possible base voltage */
*vbase = SVM3_BASE_VOLTAGE_GET_B16(vbus);
}
/****************************************************************************
* Name: foc_modulation_current_b16
*
* Description:
* Correct current samples accrding to the SVM3 modulation state (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
* curr - (in/out) phase currents
*
****************************************************************************/
static void foc_modulation_current_b16(FAR foc_handler_b16_t *h,
FAR b16_t *curr)
{
#ifdef FOC_CORRECT_CURRENT_SAMPLES
FAR struct foc_svm3mod_b16_s *svm = NULL;
DEBUGASSERT(h);
DEBUGASSERT(curr);
/* Get modulation data */
DEBUGASSERT(h->modulation);
svm = h->modulation;
/* Correct ADC current samples */
svm3_current_correct_b16(&svm->state,
&curr[0],
&curr[1],
&curr[2]);
#endif
}
/****************************************************************************
* Name: foc_modulation_b16
*
* Description:
* Handle the SVM3 modulation (fixed16)
*
* Input Parameter:
* h - pointer to FOC handler
* v_ab_mod - requested modulation voltage scaled to 0.0-1.0 range
* duty - (out) duty cycle for switches
*
****************************************************************************/
static void foc_modulation_run_b16(FAR foc_handler_b16_t *h,
FAR ab_frame_b16_t *v_ab_mod,
FAR b16_t *duty)
{
FAR struct foc_svm3mod_b16_s *svm = NULL;
DEBUGASSERT(h);
DEBUGASSERT(v_ab_mod);
DEBUGASSERT(duty);
/* Get modulation data */
DEBUGASSERT(h->modulation);
svm = h->modulation;
/* Call 3-phase space vector modulation */
svm3_b16(&svm->state, v_ab_mod);
/* Copy duty cycle */
duty[0] = svm->state.d_u;
duty[1] = svm->state.d_v;
duty[2] = svm->state.d_w;
/* Saturate duty cycle */
f_saturate_b16(&duty[0], 0, svm->cfg.pwm_duty_max);
f_saturate_b16(&duty[1], 0, svm->cfg.pwm_duty_max);
f_saturate_b16(&duty[2], 0, svm->cfg.pwm_duty_max);
}

157
industry/foc/fixed16/foc_velocity.c Normal file
View File

@ -0,0 +1,157 @@
/****************************************************************************
* apps/industry/foc/fixed16/foc_velocity.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 <errno.h>
#include <errno.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/fixed16/foc_velocity.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_velocity_init_b16
*
* Description:
* Initialize the FOC velocity handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC velocity handler
* ops - pointer to FOC velocity handler operations
*
****************************************************************************/
int foc_velocity_init_b16(FAR foc_velocity_b16_t *h,
FAR struct foc_velocity_ops_b16_s *ops)
{
int ret = OK;
DEBUGASSERT(h);
DEBUGASSERT(ops);
/* Velocity ops */
DEBUGASSERT(ops->init);
DEBUGASSERT(ops->deinit);
DEBUGASSERT(ops->cfg);
DEBUGASSERT(ops->run);
/* Reset handler */
memset(h, 0, sizeof(foc_velocity_b16_t));
/* Connect ops */
h->ops = ops;
/* Initialize velocity */
ret = h->ops->init(h);
if (ret < 0)
{
FOCLIBERR("ERROR: ops->init failed %d\n", ret);
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_velocity_deinit_b16
*
* Description:
* De-initialize the FOC velocity handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC velocity handler
*
****************************************************************************/
int foc_velocity_deinit_b16(FAR foc_velocity_b16_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Deinitialize velocity */
h->ops->deinit(h);
/* Reset data */
memset(h, 0, sizeof(foc_velocity_b16_t));
return ret;
}
/****************************************************************************
* Name: foc_velocity_cfg_b16
*
* Description:
* Configure the FOC velocity handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC velocity handler
* cfg - pointer to velocity handler configuration data
*
****************************************************************************/
int foc_velocity_cfg_b16(FAR foc_velocity_b16_t *h, FAR void *cfg)
{
DEBUGASSERT(h);
DEBUGASSERT(cfg);
return h->ops->cfg(h, cfg);
}
/****************************************************************************
* Name: foc_velocity_run_b16
*
* Description:
* Process the FOC velocity handler data (fixed16)
*
* Input Parameter:
* h - pointer to FOC velocity handler
* in - pointer to FOC velocity handler input data
* out - pointer to FOC velocity handler output data
*
****************************************************************************/
void foc_velocity_run_b16(FAR foc_velocity_b16_t *h,
FAR struct foc_velocity_in_b16_s *in,
FAR struct foc_velocity_out_b16_s *out)
{
DEBUGASSERT(h);
DEBUGASSERT(in);
DEBUGASSERT(out);
/* Run velocity handler */
h->ops->run(h, in, out);
}

157
industry/foc/float/foc_angle.c Normal file
View File

@ -0,0 +1,157 @@
/****************************************************************************
* apps/industry/foc/float/foc_angle.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 <assert.h>
#include <errno.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/float/foc_angle.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_angle_init_f32
*
* Description:
* Initialize the FOC angle handler (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
* ops - pointer to FOC angle handler operations
*
****************************************************************************/
int foc_angle_init_f32(FAR foc_angle_f32_t *h,
FAR struct foc_angle_ops_f32_s *ops)
{
int ret = OK;
DEBUGASSERT(h);
DEBUGASSERT(ops);
/* Angle ops */
DEBUGASSERT(ops->init);
DEBUGASSERT(ops->deinit);
DEBUGASSERT(ops->cfg);
DEBUGASSERT(ops->run);
/* Reset handler */
memset(h, 0, sizeof(foc_angle_f32_t));
/* Connect ops */
h->ops = ops;
/* Initialize angle */
ret = h->ops->init(h);
if (ret < 0)
{
FOCLIBERR("ERROR: ops->init failed %d\n", ret);
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_angle_deinit_f32
*
* Description:
* De-initialize the FOC angle handler (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
*
****************************************************************************/
int foc_angle_deinit_f32(FAR foc_angle_f32_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Deinitialize angle */
h->ops->deinit(h);
/* Reset data */
memset(h, 0, sizeof(foc_angle_f32_t));
return ret;
}
/****************************************************************************
* Name: foc_angle_cfg_f32
*
* Description:
* Configure the FOC angle handler (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
* cfg - pointer to angle handler configuration data
*
****************************************************************************/
int foc_angle_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg)
{
DEBUGASSERT(h);
DEBUGASSERT(cfg);
return h->ops->cfg(h, cfg);
}
/****************************************************************************
* Name: foc_angle_run_f32
*
* Description:
* Process the FOC angle handler data (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
* in - pointer to FOC angle handler input data
* out - pointer to FOC angle handler output data
*
****************************************************************************/
void foc_angle_run_f32(FAR foc_angle_f32_t *h,
FAR struct foc_angle_in_f32_s *in,
FAR struct foc_angle_out_f32_s *out)
{
DEBUGASSERT(h);
DEBUGASSERT(in);
DEBUGASSERT(out);
/* Run angle handler */
h->ops->run(h, in, out);
}

81
industry/foc/float/foc_cordic.c Normal file
View File

@ -0,0 +1,81 @@
/****************************************************************************
* apps/industry/foc/float/foc_cordic.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 <assert.h>
#include "industry/foc/float/foc_cordic.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_cordic_dqsat_f32
*
* Description:
* CORDIC DQ-frame saturation (float32)
*
* Input Parameter:
* fd - the file descriptor for CORDIC device
* dq_ref - DQ vector
* mag_max - vector magnitude max
*
****************************************************************************/
int foc_cordic_dqsat_f32(int fd, FAR dq_frame_f32_t *dq, float mag_max)
{
DEBUGASSERT(dq);
/* TODO: */
ASSERT(0);
return OK;
}
/****************************************************************************
* Name: foc_cordic_angle_f32
*
* Description:
* CORDIC angle update (float32)
*
* Input Parameter:
* fd - the file descriptor for CORDIC device
* angle - phase angle data
* a - phase angle in rad
*
****************************************************************************/
int foc_cordic_angle_f32(int fd, FAR phase_angle_f32_t *angle, float a)
{
DEBUGASSERT(angle);
/* TODO: */
ASSERT(0);
return OK;
}

345
industry/foc/float/foc_handler.c Normal file
View File

@ -0,0 +1,345 @@
/****************************************************************************
* apps/industry/foc/float/foc_handler.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 <assert.h>
#include <errno.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/foc_common.h"
#include "industry/foc/float/foc_handler.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_handler_init_f32
*
* Description:
* Initialize the FOC handler (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* ctrl - pointer to controller operations
* mod - pointer to modulation operations
*
****************************************************************************/
int foc_handler_init_f32(FAR foc_handler_f32_t *h,
FAR struct foc_control_ops_f32_s *ctrl,
FAR struct foc_modulation_ops_f32_s *mod)
{
int ret = OK;
DEBUGASSERT(h);
DEBUGASSERT(ctrl);
DEBUGASSERT(mod);
/* Controller ops */
DEBUGASSERT(ctrl->init);
DEBUGASSERT(ctrl->deinit);
DEBUGASSERT(ctrl->cfg);
DEBUGASSERT(ctrl->input_set);
DEBUGASSERT(ctrl->voltage_run);
DEBUGASSERT(ctrl->current_run);
DEBUGASSERT(ctrl->state_get);
/* Modulation ops */
DEBUGASSERT(mod->init);
DEBUGASSERT(mod->deinit);
DEBUGASSERT(mod->cfg);
DEBUGASSERT(mod->current);
DEBUGASSERT(mod->vbase_get);
DEBUGASSERT(mod->run);
/* Reset handler */
memset(h, 0, sizeof(foc_handler_f32_t));
/* Connect ops */
h->ops.ctrl = ctrl;
h->ops.mod = mod;
/* Initialize control handler */
ret = h->ops.mod->init(h);
if (ret < 0)
{
FOCLIBERR("ERROR: mod->init failed %d\n", ret);
goto errout;
}
/* Initialize modulation handler */
ret = h->ops.ctrl->init(h);
if (ret < 0)
{
FOCLIBERR("ERROR: ctrl->init failed %d\n", ret);
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_handler_deinit_f32
*
* Description:
* De-initialize the FOC handler (float32)
*
* Input Parameter:
* h - pointer to FOC handler
*
****************************************************************************/
int foc_handler_deinit_f32(FAR foc_handler_f32_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Deinitialize modulation handler */
h->ops.mod->deinit(h);
/* Deinitialize control handler */
h->ops.ctrl->deinit(h);
/* Reset data */
memset(h, 0, sizeof(foc_handler_f32_t));
return ret;
}
/****************************************************************************
* Name: foc_handler_cfg_f32
*
* Description:
* Configure the FOC handler (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* ctrl_cfg - pointer to controller configuration data
* mod_cfg - pointer to modulation configuration data
*
****************************************************************************/
void foc_handler_cfg_f32(FAR foc_handler_f32_t *h,
FAR void *ctrl_cfg,
FAR void *mod_cfg)
{
DEBUGASSERT(h);
DEBUGASSERT(ctrl_cfg);
DEBUGASSERT(mod_cfg);
/* Configure controller */
h->ops.ctrl->cfg(h, ctrl_cfg);
/* Configure modulation */
h->ops.mod->cfg(h, mod_cfg);
}
/****************************************************************************
* Name: foc_handler_run_f32
*
* Description:
* Run the FOC handler and process the given input data (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* in - pointer to FOC handler input data
* out - pointer to FOC handler output data
*
****************************************************************************/
int foc_handler_run_f32(FAR foc_handler_f32_t *h,
FAR struct foc_handler_input_f32_s *in,
FAR struct foc_handler_output_f32_s *out)
{
ab_frame_f32_t v_ab_mod;
float vbase = 0;
int ret = OK;
DEBUGASSERT(h);
DEBUGASSERT(in);
DEBUGASSERT(out);
/* Do nothing if control mode not specified yet.
* This also protects against initial state when the controller is
* started but input data has not yet been provided.
*/
if (in->mode <= FOC_HANDLER_MODE_INIT)
{
ret = -EINVAL;
goto errout;
}
/* Correct current samples according to modulation state */
h->ops.mod->current(h, in->current);
/* Get VBASE */
h->ops.mod->vbase_get(h, in->vbus, &vbase);
/* Feed controller with phase currents */
h->ops.ctrl->input_set(h, in->current, vbase, in->angle);
/* Call controller */
switch (in->mode)
{
/* IDLE */
case FOC_HANDLER_MODE_IDLE:
{
/* Do nothing and set duty to zeros */
ret = OK;
goto errout;
}
/* FOC current mode - control DQ-current */
case FOC_HANDLER_MODE_CURRENT:
{
/* Current controller */
h->ops.ctrl->current_run(h,
in->dq_ref,
in->vdq_comp,
&v_ab_mod);
break;
}
/* FOC voltage mode - control DQ-voltage */
case FOC_HANDLER_MODE_VOLTAGE:
{
/* Voltage controller */
h->ops.ctrl->voltage_run(h,
in->dq_ref,
&v_ab_mod);
break;
}
/* Otherwise - we should not be here */
default:
{
ret = -EINVAL;
goto errout;
}
}
/* Duty cycle modulation */
h->ops.mod->run(h, &v_ab_mod, out->duty);
return ret;
errout:
/* Set duty to zeros */
memset(out->duty, 0, sizeof(float) * CONFIG_MOTOR_FOC_PHASES);
return ret;
}
/****************************************************************************
* Name: foc_handler_state_f32
*
* Description:
* Get FOC handler state (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* state - pointer to FOC state data
*
****************************************************************************/
void foc_handler_state_f32(FAR foc_handler_f32_t *h,
FAR struct foc_state_f32_s *state)
{
DEBUGASSERT(h);
DEBUGASSERT(state);
h->ops.ctrl->state_get(h, state);
}
#ifdef CONFIG_INDUSTRY_FOC_HANDLER_PRINT
/****************************************************************************
* Name: foc_handler_state_print_f32
*
* Description:
* Print FOC handler state (float32)
*
* Input Parameter:
* state - pointer to FOC state data
*
****************************************************************************/
void foc_handler_state_print_f32(FAR struct foc_state_f32_s *state)
{
DEBUGASSERT(state);
#if CONFIG_MOTOR_FOC_PHASES == 3
FOCLIBLOG("curr = [%.2f %.2f %.2f]\n",
state->curr[0],
state->curr[1],
state->curr[2]);
FOCLIBLOG("volt = [%.2f %.2f %.2f]\n",
state->volt[0],
state->volt[1],
state->volt[2]);
#else
# error TODO
#endif
FOCLIBLOG("iab = [%.2f %.2f]\n", state->iab.a,
state->iab.b);
FOCLIBLOG("vab = [%.2f %.2f]\n", state->vab.a,
state->vab.b);
FOCLIBLOG("idq = [%.2f %.2f]\n", state->idq.d,
state->idq.q);
FOCLIBLOG("vdq = [%.2f %.2f]\n", state->vdq.d,
state->vdq.q);
}
#endif

188
industry/foc/float/foc_model.c Normal file
View File

@ -0,0 +1,188 @@
/****************************************************************************
* apps/industry/foc/float/foc_model.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 <assert.h>
#include <errno.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/foc_common.h"
#include "industry/foc/float/foc_model.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_model_init_f32
*
* Description:
* Initialize the FOC model (float32)
*
* Input Parameter:
* h - pointer to FOC model handler
* ops - pointer to FOC model operations
*
****************************************************************************/
int foc_model_init_f32(FAR foc_model_f32_t *h,
FAR struct foc_model_ops_f32_s *ops)
{
int ret = OK;
DEBUGASSERT(h);
DEBUGASSERT(ops);
/* Model ops */
DEBUGASSERT(ops->init);
DEBUGASSERT(ops->deinit);
DEBUGASSERT(ops->cfg);
DEBUGASSERT(ops->ele_run);
DEBUGASSERT(ops->mech_run);
DEBUGASSERT(ops->state);
/* Reset handler */
memset(h, 0, sizeof(foc_model_f32_t));
/* Connect ops */
h->ops = ops;
/* Initialize model */
ret = h->ops->init(h);
if (ret < 0)
{
FOCLIBERR("ERROR: ops->init failed %d\n", ret);
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_model_deinit_f32
*
* Description:
* De-initialize the FOC model (float32)
*
* Input Parameter:
* h - pointer to FOC model handler
*
****************************************************************************/
int foc_model_deinit_f32(FAR foc_model_f32_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Deinitialize model */
h->ops->deinit(h);
/* Reset data */
memset(h, 0, sizeof(foc_model_f32_t));
return ret;
}
/****************************************************************************
* Name: foc_model_cfg_f32
*
* Description:
* Configure the FOC model (float32)
*
* Input Parameter:
* h - pointer to FOC model handler
* cfg - pointer to FOC model configuration data
*
****************************************************************************/
int foc_model_cfg_f32(FAR foc_model_f32_t *h, FAR void *cfg)
{
DEBUGASSERT(h);
DEBUGASSERT(cfg);
return h->ops->cfg(h, cfg);
}
/****************************************************************************
* Name: foc_model_run_f32
*
* Description:
* Run the FOC model (float32)
*
* Input Parameter:
* h - pointer to FOC model handler
* load - applied load
* v_ab - applied voltage in alpha-beta frame
*
* REVISIT:
* It would be better if we feed model with duty cycle and VBUS, but it
* complicates the calculations, so we leave it like that for now
*
****************************************************************************/
void foc_model_run_f32(FAR foc_model_f32_t *h,
float load,
FAR ab_frame_f32_t *v_ab)
{
DEBUGASSERT(h);
DEBUGASSERT(v_ab);
/* Run electrical model */
h->ops->ele_run(h, v_ab);
/* Run mechanical model */
h->ops->mech_run(h, load);
}
/****************************************************************************
* Name: foc_model_state_f32
*
* Description:
* Get model state (float32)
*
* Input Parameter:
* h - pointer to FOC model handler
* state - pointer to FOC model state
*
****************************************************************************/
void foc_model_state_f32(FAR foc_model_f32_t *h,
FAR struct foc_model_state_f32_s *state)
{
DEBUGASSERT(h);
DEBUGASSERT(state);
h->ops->state(h, state);
}

297
industry/foc/float/foc_model_pmsm.c Normal file
View File

@ -0,0 +1,297 @@
/****************************************************************************
* apps/industry/foc/float/foc_model_pmsm.c
* This file implements PMSM model for fixed16
*
* 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 <assert.h>
#include <errno.h>
#include <stdlib.h>
#include "industry/foc/float/foc_model.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#if CONFIG_MOTOR_FOC_PHASES != 3
# error
#endif
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/* PMSM model data */
struct foc_model_pmsm_f32_s
{
float one_by_iphadc;
struct foc_model_pmsm_cfg_f32_s cfg;
struct pmsm_model_f32_s model;
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int foc_model_pmsm_init_f32(FAR foc_model_f32_t *h);
static void foc_model_pmsm_deinit_f32(FAR foc_model_f32_t *h);
static int foc_model_pmsm_cfg_f32(FAR foc_model_f32_t *h, FAR void *cfg);
static void foc_model_pmsm_elerun_f32(FAR foc_model_f32_t *h,
FAR ab_frame_f32_t *v_ab);
static void foc_model_pmsm_mechrun_f32(FAR foc_model_f32_t *h, float load);
static void foc_model_pmsm_state_f32(FAR foc_model_f32_t *h,
FAR struct foc_model_state_f32_s *state);
/****************************************************************************
* Public Data
****************************************************************************/
/* FOC model float interface */
struct foc_model_ops_f32_s g_foc_model_pmsm_ops_f32 =
{
.init = foc_model_pmsm_init_f32,
.deinit = foc_model_pmsm_deinit_f32,
.cfg = foc_model_pmsm_cfg_f32,
.ele_run = foc_model_pmsm_elerun_f32,
.mech_run = foc_model_pmsm_mechrun_f32,
.state = foc_model_pmsm_state_f32,
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: foc_model_pmsm_init_f32
*
* Description:
* Initialize PMSM model (float32)
*
* Input Parameter:
* h - pointer to FOC model handler
*
****************************************************************************/
static int foc_model_pmsm_init_f32(FAR foc_model_f32_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Connect model data */
h->model = zalloc(sizeof(struct foc_model_pmsm_f32_s));
if (h->model == NULL)
{
ret = -ENOMEM;
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_model_pmsm_deinit_f32
*
* Description:
* Deinitialize PMSM model (float32)
*
* Input Parameter:
* h - pointer to FOC model handler
*
****************************************************************************/
static void foc_model_pmsm_deinit_f32(FAR foc_model_f32_t *h)
{
DEBUGASSERT(h);
/* Free model data */
if (h->model)
{
free (h->model);
}
}
/****************************************************************************
* Name: foc_model_pmsm_cfg_f32
*
* Description:
* Configure PMSM model (float32)
*
* Input Parameter:
* h - pointer to FOC model handler
* cfg - pointer to FOC model configuration (struct foc_model_pmsm_f32_s)
*
****************************************************************************/
static int foc_model_pmsm_cfg_f32(FAR foc_model_f32_t *h, FAR void *cfg)
{
FAR struct foc_model_pmsm_f32_s *model = NULL;
struct pmsm_phy_params_f32_s phy;
DEBUGASSERT(h);
DEBUGASSERT(cfg);
/* Get model data */
DEBUGASSERT(h->model);
model = h->model;
/* Copy data */
memcpy(&model->cfg, cfg, sizeof(struct foc_model_pmsm_cfg_f32_s));
/* Initialize model */
pmsm_phy_params_init(&phy,
model->cfg.poles,
model->cfg.res,
model->cfg.ind,
model->cfg.iner,
model->cfg.flux_link,
model->cfg.ind_q,
model->cfg.ind_d);
pmsm_model_initialize(&model->model, &phy, model->cfg.per);
/* Get one by iphase_adc */
model->one_by_iphadc = (1.0f / model->cfg.iphase_adc);
return OK;
}
/****************************************************************************
* Name: foc_model_pmsm_elerun_f32
*
* Description:
* Run model electrical simulation (float32)
*
* Input Parameter:
* h - pointer to FOC model handler
* v_ab - applied voltage in alpha-beta frame
*
****************************************************************************/
static void foc_model_pmsm_elerun_f32(FAR foc_model_f32_t *h,
FAR ab_frame_f32_t *v_ab)
{
FAR struct foc_model_pmsm_f32_s *model = NULL;
DEBUGASSERT(h);
DEBUGASSERT(v_ab);
/* Get model data */
DEBUGASSERT(h->model);
model = h->model;
/* Run electrical model */
pmsm_model_elec(&model->model, v_ab);
}
/****************************************************************************
* Name: foc_model_pmsm_mechrun_f32
*
* Description:
* Run model mechanical simulation (float32)
*
* Input Parameter:
* h - pointer to FOC model handler
* load - applied load
*
****************************************************************************/
static void foc_model_pmsm_mechrun_f32(FAR foc_model_f32_t *h, float load)
{
FAR struct foc_model_pmsm_f32_s *model = NULL;
DEBUGASSERT(h);
/* Get model data */
DEBUGASSERT(h->model);
model = h->model;
/* Run mechanical model */
pmsm_model_mech(&model->model, load);
}
/****************************************************************************
* Name: foc_model_pmsm_state_f32
*
* Description:
* Get the model state (float32)
*
* Input Parameter:
* h - pointer to FOC model handler
* state - pointer to FOC model state
*
****************************************************************************/
static void foc_model_pmsm_state_f32(FAR foc_model_f32_t *h,
FAR struct foc_model_state_f32_s *state)
{
FAR struct foc_model_pmsm_f32_s *model = NULL;
DEBUGASSERT(h);
DEBUGASSERT(state);
/* Get model data */
DEBUGASSERT(h->model);
model = h->model;
/* Get model state */
state->volt[0] = model->model.state.v_abc.a;
state->volt[1] = model->model.state.v_abc.b;
state->volt[2] = model->model.state.v_abc.c;
state->vab.a = model->model.state.v_ab.a;
state->vab.b = model->model.state.v_ab.b;
state->vdq.d = model->model.state.v_dq.d;
state->vdq.q = model->model.state.v_dq.q;
state->curr[0] = model->model.state.i_abc.a;
state->curr[1] = model->model.state.i_abc.b;
state->curr[2] = model->model.state.i_abc.c;
state->iab.a = model->model.state.i_ab.a;
state->iab.b = model->model.state.i_ab.b;
state->idq.d = model->model.state.i_dq.d;
state->idq.q = model->model.state.i_dq.q;
state->omega_e = model->model.state.omega_e;
state->omega_m = model->model.state.omega_m;
/* Get RAW currents */
state->curr_raw[0] = (int32_t)(state->curr[0] * model->one_by_iphadc);
state->curr_raw[1] = (int32_t)(state->curr[1] * model->one_by_iphadc);
state->curr_raw[2] = (int32_t)(state->curr[2] * model->one_by_iphadc);
}

197
industry/foc/float/foc_openloop.c Normal file
View File

@ -0,0 +1,197 @@
/****************************************************************************
* apps/industry/foc/float/foc_openloop.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 <assert.h>
#include <errno.h>
#include <stdlib.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/float/foc_angle.h"
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/* Open-loop data */
struct foc_openloop_f32_s
{
struct foc_openloop_cfg_f32_s cfg;
struct openloop_data_f32_s data;
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int foc_angle_ol_init_f32(FAR foc_angle_f32_t *h);
static void foc_angle_ol_deinit_f32(FAR foc_angle_f32_t *h);
static int foc_angle_ol_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg);
static void foc_angle_ol_run_f32(FAR foc_angle_f32_t *h,
FAR struct foc_angle_in_f32_s *in,
FAR struct foc_angle_out_f32_s *out);
/****************************************************************************
* Public Data
****************************************************************************/
/* FOC angle float interface */
struct foc_angle_ops_f32_s g_foc_angle_ol_f32 =
{
.init = foc_angle_ol_init_f32,
.deinit = foc_angle_ol_deinit_f32,
.cfg = foc_angle_ol_cfg_f32,
.run = foc_angle_ol_run_f32,
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: foc_angle_ol_init_f32
*
* Description:
* Initialize the open-loop FOC angle handler (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
*
****************************************************************************/
static int foc_angle_ol_init_f32(FAR foc_angle_f32_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Connect angle data */
h->data = zalloc(sizeof(struct foc_openloop_f32_s));
if (h->data == NULL)
{
ret = -ENOMEM;
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_angle_ol_deinit_f32
*
* Description:
* De-initialize the open-loop FOC angle handler (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
*
****************************************************************************/
static void foc_angle_ol_deinit_f32(FAR foc_angle_f32_t *h)
{
DEBUGASSERT(h);
/* Free angle data */
if (h->data)
{
free(h->data);
}
}
/****************************************************************************
* Name: foc_angle_ol_cfg_f32
*
* Description:
* Configure the open-loop FOC angle handler (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
* cfg - pointer to angle handler configuration data
* (struct foc_openloop_f32_s)
*
****************************************************************************/
static int foc_angle_ol_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg)
{
FAR struct foc_openloop_f32_s *ol = NULL;
DEBUGASSERT(h);
/* Get modulation data */
DEBUGASSERT(h->data);
ol = h->data;
/* Copy configuration */
memcpy(&ol->cfg, cfg, sizeof(struct foc_openloop_cfg_f32_s));
/* Initialize open-loop controller data */
DEBUGASSERT(ol->cfg.per > 0.0f);
motor_openloop_init(&ol->data, ol->cfg.per);
return OK;
}
/****************************************************************************
* Name: foc_angle_ol_run_f32
*
* Description:
* Process the FOC open-loop angle data (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
* in - pointer to FOC angle handler input data
* out - pointer to FOC angle handler output data
*
****************************************************************************/
static void foc_angle_ol_run_f32(FAR foc_angle_f32_t *h,
FAR struct foc_angle_in_f32_s *in,
FAR struct foc_angle_out_f32_s *out)
{
FAR struct foc_openloop_f32_s *ol = NULL;
DEBUGASSERT(h);
/* Get modulation data */
DEBUGASSERT(h->data);
ol = h->data;
/* Update open-loop */
motor_openloop(&ol->data, in->vel, in->dir);
/* Get open-loop angle */
out->angle = motor_openloop_angle_get(&ol->data);
}

418
industry/foc/float/foc_picontrol.c Normal file
View File

@ -0,0 +1,418 @@
/****************************************************************************
* apps/industry/foc/float/foc_picontrol.c
* This file implements classical FOC PI current controller for float32
*
* 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 <assert.h>
#include <errno.h>
#include <stdlib.h>
#include "industry/foc/float/foc_handler.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#if CONFIG_MOTOR_FOC_PHASES != 3
# error
#endif
/****************************************************************************
* Private Data Types
****************************************************************************/
/* FOC PI float controller data */
struct foc_picontrol_f32_s
{
float vbase_last; /* Last VBASE sample */
phase_angle_f32_t angle; /* Phase angle */
struct foc_initdata_f32_s cfg; /* Controller configuration */
struct foc_data_f32_s data; /* Controller private data */
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int foc_control_init_f32(FAR foc_handler_f32_t *h);
static void foc_control_deinit_f32(FAR foc_handler_f32_t *h);
static void foc_control_cfg_f32(FAR foc_handler_f32_t *h, FAR void *cfg);
static void foc_control_input_set_f32(FAR foc_handler_f32_t *h,
FAR float *current,
float vbase,
float angle);
static void foc_control_voltage_run_f32(FAR foc_handler_f32_t *h,
FAR dq_frame_f32_t *dq_ref,
FAR ab_frame_f32_t *v_ab_mod);
static void foc_control_current_run_f32(FAR foc_handler_f32_t *h,
FAR dq_frame_f32_t *dq_ref,
FAR dq_frame_f32_t *vdq_comp,
FAR ab_frame_f32_t *v_ab_mod);
static void foc_control_state_get_f32(FAR foc_handler_f32_t *h,
FAR struct foc_state_f32_s *state);
/****************************************************************************
* Public Data
****************************************************************************/
/* FOC control float interface */
struct foc_control_ops_f32_s g_foc_control_pi_f32 =
{
.init = foc_control_init_f32,
.deinit = foc_control_deinit_f32,
.cfg = foc_control_cfg_f32,
.input_set = foc_control_input_set_f32,
.voltage_run = foc_control_voltage_run_f32,
.current_run = foc_control_current_run_f32,
.state_get = foc_control_state_get_f32,
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: foc_control_init_f32
*
* Description:
* Initialize the FOC PI controller (float32)
*
* Input Parameter:
* h - pointer to FOC handler
*
****************************************************************************/
static int foc_control_init_f32(FAR foc_handler_f32_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Connect controller data */
h->control = zalloc(sizeof(struct foc_picontrol_f32_s));
if (h->control == NULL)
{
ret = -ENOMEM;
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_control_deinit_f32
*
* Description:
* Deinitialize the FOC PI controller (float32)
*
* Input Parameter:
* h - pointer to FOC handler
*
****************************************************************************/
static void foc_control_deinit_f32(FAR foc_handler_f32_t *h)
{
DEBUGASSERT(h);
/* Free controller data */
if (h->control)
{
free(h->control);
}
}
/****************************************************************************
* Name: foc_control_cfg_set_f32
*
* Description:
* Configure the FOC controller (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* cfg - pointer to controller configuration data
* (struct foc_picontrol_f32_s)
*
****************************************************************************/
static void foc_control_cfg_f32(FAR foc_handler_f32_t *h, FAR void *cfg)
{
FAR struct foc_picontrol_f32_s *foc = NULL;
DEBUGASSERT(h);
DEBUGASSERT(cfg);
/* Get controller data */
DEBUGASSERT(h->control);
foc = h->control;
/* Copy data */
memcpy(&foc->cfg, cfg, sizeof(struct foc_initdata_f32_s));
/* Initialize FOC controller data */
foc_init(&foc->data, &foc->cfg);
}
/****************************************************************************
* Name: foc_control_input_set_f32
*
* Description:
* Update input for controller (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* current - phase currents in amps
* vbase - base voltage for controller
* angle - phase angle in rad
*
****************************************************************************/
static void foc_control_input_set_f32(FAR foc_handler_f32_t *h,
FAR float *current,
float vbase,
float angle)
{
FAR struct foc_picontrol_f32_s *foc = NULL;
abc_frame_f32_t i_abc;
DEBUGASSERT(h);
DEBUGASSERT(current);
/* Get controller data */
DEBUGASSERT(h->control);
foc = h->control;
/* Get current in abc frame */
i_abc.a = current[0];
i_abc.b = current[1];
i_abc.c = current[2];
foc_iabc_update(&foc->data, &i_abc);
/* Update base voltage only if changed */
if (foc->vbase_last != vbase)
{
/* Update FOC base voltage */
foc_vbase_update(&foc->data, vbase);
/* Update last FOC base voltage */
foc->vbase_last = vbase;
}
/* Update phase angle */
#ifndef CONFIG_INDUSTRY_FOC_CORDIC
phase_angle_update(&foc->angle, angle);
#else
foc_cordic_angle_f32(h->fd, &foc->angle, angle);
#endif
/* Feed the controller with phase angle */
foc_angle_update(&foc->data, &foc->angle);
}
/****************************************************************************
* Name: foc_control_voltage_f32
*
* Description:
* Handle the FOC voltage control (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* dq_ref - DQ voltage reference frame
* v_ab_mod - (out) modulation alpha-veta voltage
*
****************************************************************************/
static void foc_control_voltage_run_f32(FAR foc_handler_f32_t *h,
FAR dq_frame_f32_t *dq_ref,
FAR ab_frame_f32_t *v_ab_mod)
{
FAR struct foc_picontrol_f32_s *foc = NULL;
float mag_max = 0;
DEBUGASSERT(h);
DEBUGASSERT(dq_ref);
DEBUGASSERT(v_ab_mod);
/* Get controller data */
DEBUGASSERT(h->control);
foc = h->control;
/* Get maximum possible voltage DQ vetor magnitude */
foc_vdq_mag_max_get(&foc->data, &mag_max);
/* Saturate voltage DQ vector */
#ifndef CONFIG_INDUSTRY_FOC_CORDIC
dq_saturate(dq_ref, mag_max);
#else
foc_cordic_dqsat_f32(h->fd, dq_ref, mag_max);
#endif
/* Call FOC voltage controller */
foc_voltage_control(&foc->data, dq_ref);
/* Get output v_ab_mod frame */
foc_vabmod_get(&foc->data, v_ab_mod);
}
/****************************************************************************
* Name: foc_control_current_f32
*
* Description:
* Handle the FOC current control (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* dq_ref - DQ current reference frame
* vdq_comp - DQ voltage compensation
* v_ab_mod - (out) modulation alpha-veta voltage
*
****************************************************************************/
static void foc_control_current_run_f32(FAR foc_handler_f32_t *h,
FAR dq_frame_f32_t *dq_ref,
FAR dq_frame_f32_t *vdq_comp,
FAR ab_frame_f32_t *v_ab_mod)
{
FAR struct foc_picontrol_f32_s *foc = NULL;
float mag_max = 0;
dq_frame_f32_t v_dq_ref;
DEBUGASSERT(h);
DEBUGASSERT(dq_ref);
DEBUGASSERT(vdq_comp);
DEBUGASSERT(v_ab_mod);
/* Get controller data */
DEBUGASSERT(h->control);
foc = h->control;
/* Reset voltage reference */
v_dq_ref.d = 0.0f;
v_dq_ref.q = 0.0f;
/* Call FOC current controller */
foc_current_control(&foc->data, dq_ref, vdq_comp, &v_dq_ref);
/* Get maximum possible voltage DQ vetor magnitude */
foc_vdq_mag_max_get(&foc->data, &mag_max);
/* Saturate voltage DQ vector */
#ifndef CONFIG_INDUSTRY_FOC_CORDIC
dq_saturate(dq_ref, mag_max);
#else
foc_cordic_dqsat_f32(h->fd, dq_ref, mag_max);
#endif
/* Call FOC voltage control */
foc_voltage_control(&foc->data, &v_dq_ref);
/* Get output v_ab_mod frame */
foc_vabmod_get(&foc->data, v_ab_mod);
}
/****************************************************************************
* Name: foc_control_state_get_f32
*
* Description:
* Get the FOC controller state (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* state - (out) pointer to FOC state data
*
****************************************************************************/
static void foc_control_state_get_f32(FAR foc_handler_f32_t *h,
FAR struct foc_state_f32_s *state)
{
FAR struct foc_picontrol_f32_s *foc = NULL;
DEBUGASSERT(h);
DEBUGASSERT(state);
/* Get controller data */
DEBUGASSERT(h->control);
foc = h->control;
/* Copy DQ voltage */
state->vdq.q = foc->data.v_dq.q;
state->vdq.d = foc->data.v_dq.d;
/* Copy DQ current */
state->idq.q = foc->data.i_dq.q;
state->idq.d = foc->data.i_dq.d;
/* Copy alpha-beta current */
state->iab.a = foc->data.i_ab.a;
state->iab.b = foc->data.i_ab.b;
/* Copy alpha-beta voltage */
state->vab.a = foc->data.v_ab.a;
state->vab.b = foc->data.v_ab.b;
/* Copy phase current */
state->curr[0] = foc->data.i_abc.a;
state->curr[1] = foc->data.i_abc.b;
state->curr[2] = foc->data.i_abc.c;
/* Copy phase voltage */
state->volt[0] = foc->data.v_abc.a;
state->volt[1] = foc->data.v_abc.b;
state->volt[2] = foc->data.v_abc.c;
}

178
industry/foc/float/foc_ramp.c Normal file
View File

@ -0,0 +1,178 @@
/****************************************************************************
* apps/industry/foc/float/foc_ramp.c
* This file implements ramp for float32
*
* 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 <assert.h>
#include "industry/foc/foc_common.h"
#include "industry/foc/float/foc_ramp.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_ramp_init_f32
*
* Description:
* Initialize ramp (float32)
*
* Input Parameter:
* ramp - pointer to ramp handler
* per - ramp period
* thr - ramp threshold
* acc - ramp acceleration
* dec - ramp decceleration
*
****************************************************************************/
int foc_ramp_init_f32(FAR struct foc_ramp_f32_s *ramp, float per,
float thr, float acc, float dec)
{
DEBUGASSERT(ramp);
DEBUGASSERT(per > 0);
DEBUGASSERT(thr > 0);
DEBUGASSERT(acc > 0);
DEBUGASSERT(dec > 0);
ramp->per = per;
ramp->ramp_thr = thr;
ramp->ramp_acc = acc;
ramp->ramp_dec = dec;
ramp->ramp_acc_per = (ramp->ramp_acc * ramp->per);
ramp->ramp_dec_per = (ramp->ramp_dec * ramp->per);
return OK;
}
/****************************************************************************
* Name: foc_ramp_run_f32
*
* Description:
* Handle ramp (float32)
*
* Input Parameter:
* ramp - pointer to ramp handler
* des - ramp parameter destination
* now - ramp parameter now
* set - (out) ramp parameter set
*
****************************************************************************/
int foc_ramp_run_f32(FAR struct foc_ramp_f32_s *ramp, float des,
float now, FAR float *set)
{
DEBUGASSERT(ramp);
/* Check if we require soft start/stop operation.
* Only if the user set point differs from the driver set point
*/
if (des != *set)
{
ramp->diff = des - *set;
if (ramp->diff >= ramp->ramp_thr)
{
ramp->ramp_mode = RAMP_MODE_SOFTSTART;
}
else if (ramp->diff <= -ramp->ramp_thr)
{
ramp->ramp_mode = RAMP_MODE_SOFTSTOP;
}
else
{
/* Just set new setpoint */
*set = des;
ramp->ramp_mode = RAMP_MODE_NORMAL;
}
}
else
{
ramp->ramp_mode = RAMP_MODE_NORMAL;
}
/* Handle according to current motor state */
switch (ramp->ramp_mode)
{
case RAMP_MODE_NORMAL:
{
/* Nothing to do here ? */
break;
}
case RAMP_MODE_SOFTSTART:
{
if (des - *set >= ramp->ramp_thr)
{
/* Increase setpoin with ramp */
*set = now + ramp->ramp_acc_per;
}
else
{
/* Set final setpoint and exit soft start */
*set = des;
ramp->ramp_mode = RAMP_MODE_NORMAL;
}
break;
}
case RAMP_MODE_SOFTSTOP:
{
if (des - *set <= -ramp->ramp_thr)
{
/* Stop motor with ramp */
*set = now - ramp->ramp_dec_per;
}
else
{
/* Set final setpint and exit soft stop */
*set = des;
ramp->ramp_mode = RAMP_MODE_NORMAL;
}
break;
}
default:
{
/* We should not be here */
DEBUGASSERT(0);
break;
}
}
return OK;
}

287
industry/foc/float/foc_svm3.c Normal file
View File

@ -0,0 +1,287 @@
/****************************************************************************
* apps/industry/foc/float/foc_svm3.c
* This file implements 3-phase space vector modulation for float32
*
* 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 <assert.h>
#include <errno.h>
#include <stdlib.h>
#include "industry/foc/float/foc_handler.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#if CONFIG_MOTOR_FOC_PHASES != 3
# error
#endif
/* Enable current samples correction if 3-shunts */
#if CONFIG_MOTOR_FOC_SHUNTS == 3
# define FOC_CORRECT_CURRENT_SAMPLES 1
#endif
/****************************************************************************
* Private Data Types
****************************************************************************/
/* SVM3 data */
struct foc_svm3mod_f32_s
{
struct foc_mod_cfg_f32_s cfg;
struct svm3_state_f32_s state;
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int foc_modulation_init_f32(FAR foc_handler_f32_t *h);
static void foc_modulation_deinit_f32(FAR foc_handler_f32_t *h);
static void foc_modulation_cfg_f32(FAR foc_handler_f32_t *h, FAR void *cfg);
static void foc_modulation_current_f32(FAR foc_handler_f32_t *h,
FAR float *curr);
static void foc_modulation_vbase_get_f32(FAR foc_handler_f32_t *h,
float vbus,
FAR float *vbase);
static void foc_modulation_run_f32(FAR foc_handler_f32_t *h,
FAR ab_frame_f32_t *v_ab_mod,
FAR float *duty);
/****************************************************************************
* Public Data
****************************************************************************/
/* FOC modulation float32 interface */
struct foc_modulation_ops_f32_s g_foc_mod_svm3_f32 =
{
.init = foc_modulation_init_f32,
.deinit = foc_modulation_deinit_f32,
.cfg = foc_modulation_cfg_f32,
.current = foc_modulation_current_f32,
.vbase_get = foc_modulation_vbase_get_f32,
.run = foc_modulation_run_f32,
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: foc_modulation_init_f32
*
* Description:
* Initialize the SVM3 modulation (float32)
*
* Input Parameter:
* h - pointer to FOC handler
*
****************************************************************************/
static int foc_modulation_init_f32(FAR foc_handler_f32_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Connect modulation data */
h->modulation = zalloc(sizeof(struct foc_svm3mod_f32_s));
if (h->modulation == NULL)
{
ret = -ENOMEM;
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_modulation_deinit_f32
*
* Description:
* Deinitialize the SVM3 modulation (float32)
*
* Input Parameter:
* h - pointer to FOC handler
*
****************************************************************************/
static void foc_modulation_deinit_f32(FAR foc_handler_f32_t *h)
{
DEBUGASSERT(h);
/* Free modulation data */
if (h->modulation)
{
free(h->modulation);
}
}
/****************************************************************************
* Name: foc_modulation_cfg_f32
*
* Description:
* Configure the SVM3 modulation (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* cfg - pointer to modulation configuration data
* (struct foc_svm3mod_f32_s)
*
****************************************************************************/
static void foc_modulation_cfg_f32(FAR foc_handler_f32_t *h, FAR void *cfg)
{
FAR struct foc_svm3mod_f32_s *svm = NULL;
DEBUGASSERT(h);
DEBUGASSERT(cfg);
/* Get modulation data */
DEBUGASSERT(h->modulation);
svm = h->modulation;
/* Copy data */
memcpy(&svm->cfg, cfg, sizeof(struct foc_mod_cfg_f32_s));
/* Initialize SVM3 data */
svm3_init(&svm->state);
}
/****************************************************************************
* Name: foc_modulation_vbase_get_f32
*
* Description:
* Get the modulation base voltage (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* vbus - bus voltage
* vbase - (out) pointer to base voltage from modulation
*
****************************************************************************/
static void foc_modulation_vbase_get_f32(FAR foc_handler_f32_t *h,
float vbus,
FAR float *vbase)
{
DEBUGASSERT(h);
DEBUGASSERT(vbus);
DEBUGASSERT(vbase);
/* Get maximum possible base voltage */
*vbase = SVM3_BASE_VOLTAGE_GET(vbus);
}
/****************************************************************************
* Name: foc_modulation_current_f32
*
* Description:
* Correct current samples accrding to the SVM3 modulation state (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* curr - (in/out) phase currents
*
****************************************************************************/
static void foc_modulation_current_f32(FAR foc_handler_f32_t *h,
FAR float *curr)
{
#ifdef FOC_CORRECT_CURRENT_SAMPLES
FAR struct foc_svm3mod_f32_s *svm = NULL;
DEBUGASSERT(h);
DEBUGASSERT(curr);
/* Get modulation data */
DEBUGASSERT(h->modulation);
svm = h->modulation;
/* Correct ADC current samples */
svm3_current_correct(&svm->state,
&curr[0],
&curr[1],
&curr[2]);
#endif
}
/****************************************************************************
* Name: foc_modulation_f32
*
* Description:
* Handle the SVM3 modulation (float32)
*
* Input Parameter:
* h - pointer to FOC handler
* v_ab_mod - requested modulation voltage scaled to 0.0-1.0 range
* duty - (out) duty cycle for switches
*
****************************************************************************/
static void foc_modulation_run_f32(FAR foc_handler_f32_t *h,
FAR ab_frame_f32_t *v_ab_mod,
FAR float *duty)
{
FAR struct foc_svm3mod_f32_s *svm = NULL;
DEBUGASSERT(h);
DEBUGASSERT(v_ab_mod);
DEBUGASSERT(duty);
/* Get modulation data */
DEBUGASSERT(h->modulation);
svm = h->modulation;
/* Call 3-phase space vector modulation */
svm3(&svm->state, v_ab_mod);
/* Copy duty cycle */
duty[0] = svm->state.d_u;
duty[1] = svm->state.d_v;
duty[2] = svm->state.d_w;
/* Saturate duty cycle */
f_saturate(&duty[0], 0.0f, svm->cfg.pwm_duty_max);
f_saturate(&duty[1], 0.0f, svm->cfg.pwm_duty_max);
f_saturate(&duty[2], 0.0f, svm->cfg.pwm_duty_max);
}

157
industry/foc/float/foc_velocity.c Normal file
View File

@ -0,0 +1,157 @@
/****************************************************************************
* apps/industry/foc/float/foc_velocity.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 <debug.h>
#include <errno.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/float/foc_velocity.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_velocity_init_f32
*
* Description:
* Initialize the FOC velocity handler (float32)
*
* Input Parameter:
* h - pointer to FOC velocity handler
* ops - pointer to FOC velocity handler operations
*
****************************************************************************/
int foc_velocity_init_f32(FAR foc_velocity_f32_t *h,
FAR struct foc_velocity_ops_f32_s *ops)
{
int ret = OK;
DEBUGASSERT(h);
DEBUGASSERT(ops);
/* Velocity ops */
DEBUGASSERT(ops->init);
DEBUGASSERT(ops->deinit);
DEBUGASSERT(ops->cfg);
DEBUGASSERT(ops->run);
/* Reset handler */
memset(h, 0, sizeof(foc_velocity_f32_t));
/* Connect ops */
h->ops = ops;
/* Initialize velocity */
ret = h->ops->init(h);
if (ret < 0)
{
FOCLIBERR("ERROR: ops->init failed %d\n", ret);
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_velocity_deinit_f32
*
* Description:
* De-initialize the FOC velocity handler (float32)
*
* Input Parameter:
* h - pointer to FOC velocity handler
*
****************************************************************************/
int foc_velocity_deinit_f32(FAR foc_velocity_f32_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Deinitialize velocity */
h->ops->deinit(h);
/* Reset data */
memset(h, 0, sizeof(foc_velocity_f32_t));
return ret;
}
/****************************************************************************
* Name: foc_velocity_cfg_f32
*
* Description:
* Configure the FOC velocity handler (float32)
*
* Input Parameter:
* h - pointer to FOC velocity handler
* cfg - pointer to velocity handler configuration data
*
****************************************************************************/
int foc_velocity_cfg_f32(FAR foc_velocity_f32_t *h, FAR void *cfg)
{
DEBUGASSERT(h);
DEBUGASSERT(cfg);
return h->ops->cfg(h, cfg);
}
/****************************************************************************
* Name: foc_velocity_run_f32
*
* Description:
* Process the FOC velocity handler data (float32)
*
* Input Parameter:
* h - pointer to FOC velocity handler
* in - pointer to FOC velocity handler input data
* out - pointer to FOC velocity handler output data
*
****************************************************************************/
void foc_velocity_run_f32(FAR foc_velocity_f32_t *h,
FAR struct foc_velocity_in_f32_s *in,
FAR struct foc_velocity_out_f32_s *out)
{
DEBUGASSERT(h);
DEBUGASSERT(in);
DEBUGASSERT(out);
/* Run velocity handler */
h->ops->run(h, in, out);
}

175
industry/foc/foc_utils.c Normal file
View File

@ -0,0 +1,175 @@
/****************************************************************************
* apps/industry/foc/foc_utils.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 <sys/ioctl.h>
#include <errno.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/foc_utils.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_dev_stop
****************************************************************************/
int foc_dev_stop(int fd)
{
int ret = OK;
ret = ioctl(fd, MTRIOC_STOP, 0);
if (ret != OK)
{
FOCLIBERR("ERROR: MTRIOC_STOP failed %d!\n", errno);
ret = -errno;
}
return ret;
}
/****************************************************************************
* Name: foc_dev_start
****************************************************************************/
int foc_dev_start(int fd)
{
int ret = OK;
ret = ioctl(fd, MTRIOC_START, 0);
if (ret != OK)
{
FOCLIBERR("ERROR: MTRIOC_START failed %d!\n", errno);
ret = -errno;
}
return ret;
}
/****************************************************************************
* Name: foc_dev_clearfault
****************************************************************************/
int foc_dev_clearfault(int fd)
{
int ret = OK;
/* Clear fault state */
ret = ioctl(fd, MTRIOC_CLEAR_FAULT, (unsigned long)0);
if (ret != OK)
{
FOCLIBERR("ERROR: MTRIOC_CLEAR_FAULT failed %d!\n", errno);
ret = -errno;
}
return ret;
}
/****************************************************************************
* Name: foc_dev_getstate
****************************************************************************/
int foc_dev_getstate(int fd, FAR struct foc_state_s *state)
{
int ret = OK;
/* Blocking operation */
ret = ioctl(fd, MTRIOC_GET_STATE, (unsigned long)state);
if (ret != OK)
{
FOCLIBERR("ERROR: MTRIOC_GET_STATE failed %d!\n", errno);
ret = -errno;
}
return ret;
}
/****************************************************************************
* Name: foc_dev_setparams
****************************************************************************/
int foc_dev_setparams(int fd, FAR struct foc_params_s *params)
{
int ret = OK;
ret = ioctl(fd, MTRIOC_SET_PARAMS, (unsigned long)params);
if (ret != OK)
{
FOCLIBERR("ERROR: MTRIOC_SET_PARAMS failed %d!\n", errno);
ret = -errno;
}
return ret;
}
/****************************************************************************
* Name: foc_dev_setcfg
****************************************************************************/
int foc_dev_setcfg(int fd, FAR struct foc_cfg_s *cfg)
{
int ret = OK;
ret = ioctl(fd, MTRIOC_SET_CONFIG, (unsigned long)cfg);
if (ret != OK)
{
FOCLIBERR("ERROR: MTRIOC_SET_CONFIG failed %d!\n", errno);
ret = -errno;
}
return ret;
}
/****************************************************************************
* Name: foc_dev_getinfo
****************************************************************************/
int foc_dev_getinfo(int fd, FAR struct foc_info_s *info)
{
int ret = OK;
ret = ioctl(fd, MTRIOC_GET_INFO, (unsigned long)info);
if (ret != OK)
{
FOCLIBERR("ERROR: MTRIOC_GET_INFO failed %d!\n", errno);
ret = -errno;
}
return ret;
}
/****************************************************************************
* Name: foc_cfg_print
****************************************************************************/
void foc_cfg_print(FAR struct foc_cfg_s *cfg)
{
FOCLIBLOG("PWM freq=%" PRIu32 "\n", cfg->pwm_freq);
FOCLIBLOG("Notifier freq=%" PRIu32 "\n", cfg->notifier_freq);
}