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

libdsp/fixed16: add openloop handler

Browse Source
This commit is contained in:
raiden00pl 2021-04-01 13:41:37 +02:00 committed by Alan Carvalho de Assis
parent f07b64d1af
commit 55517a51e0
2 changed files with 106 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
include/dspb16.h
View File

@ -242,6 +242,14 @@ struct svm3_state_b16_s
b16_t d_w; /* Duty cycle for phase W */
};
/* Motor open-loop control data */
struct openloop_data_b16_s
{
b16_t angle; /* Open-loop current angle normalized to <0.0, 2PI> */
b16_t per; /* Open-loop control execution period */
};
/* FOC initialize data */
struct foc_initdata_b16_s
@ -441,6 +449,13 @@ void foc_vabmod_get_b16(FAR struct foc_data_b16_s *foc,
FAR ab_frame_b16_t *v_ab_mod);
void foc_vdq_mag_max_get_b16(FAR struct foc_data_b16_s *foc, FAR b16_t *max);
/* Motor openloop control */
void motor_openloop_init_b16(FAR struct openloop_data_b16_s *op, b16_t per);
void motor_openloop_b16(FAR struct openloop_data_b16_s *op, b16_t speed,
b16_t dir);
b16_t motor_openloop_angle_get_b16(FAR struct openloop_data_b16_s *op);
/* Motor angle */
void motor_angle_init_b16(FAR struct motor_angle_b16_s *angle, uint8_t p);

91
libs/libdsp/lib_motor_b16.c
View File

@ -34,6 +34,97 @@
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: motor_openloop_init_b16
*
* Description:
* Initialize open-loop data
*
* Input Parameters:
* op - (in/out) pointer to the openloop data structure
* per - (in) period of the open-loop control
*
* Returned Value:
* None
*
****************************************************************************/
void motor_openloop_init_b16(FAR struct openloop_data_b16_s *op, b16_t per)
{
LIBDSP_DEBUGASSERT(op != NULL);
LIBDSP_DEBUGASSERT(per > 0);
/* Reset openloop structure */
memset(op, 0, sizeof(struct openloop_data_b16_s));
/* Initialize data */
op->per = per;
}
/****************************************************************************
* Name: motor_openloop_b16
*
* Description:
* One step of the open-loop control
*
* Input Parameters:
* op - (in/out) pointer to the open-loop data structure
* speed - (in) open-loop speed
* dir - (in) rotation direction
*
* Returned Value:
* None
*
****************************************************************************/
void motor_openloop_b16(FAR struct openloop_data_b16_s *op, b16_t speed,
b16_t dir)
{
LIBDSP_DEBUGASSERT(op != NULL);
LIBDSP_DEBUGASSERT(speed >= 0);
LIBDSP_DEBUGASSERT(dir == DIR_CW_B16 || dir == DIR_CCW_B16);
b16_t phase_step = 0;
b16_t tmp = 0;
/* Get phase step */
tmp = b16mulb16(dir, speed);
phase_step = b16mulb16(tmp, op->per);
/* Update open-loop angle */
op->angle += phase_step;
/* Normalize the open-loop angle to 0.0 - 2PI range */
angle_norm_2pi_b16(&op->angle, MOTOR_ANGLE_E_MIN_B16,
MOTOR_ANGLE_E_MAX_B16);
}
/****************************************************************************
* Name: motor_openloop_angle_get_b16
*
* Description:
* Get angle from open-loop controller
*
* Input Parameters:
* op - (in/out) pointer to the open-loop data structure
*
* Returned Value:
* Return angle from open-loop controller
*
****************************************************************************/
b16_t motor_openloop_angle_get_b16(FAR struct openloop_data_b16_s *op)
{
LIBDSP_DEBUGASSERT(op != NULL);
return op->angle;
}
/****************************************************************************
* Name: motor_angle_e_update_b16
*