377 lines
11 KiB
C
377 lines
11 KiB
C
/****************************************************************************
|
|
* libs/libdsp/lib_motor_b16.c
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*
|
|
* 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 <dspb16.h>
|
|
#include <string.h>
|
|
|
|
/****************************************************************************
|
|
* Pre-processor Definitions
|
|
****************************************************************************/
|
|
|
|
#define POLE_CNTR_THR (0)
|
|
|
|
/****************************************************************************
|
|
* 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
|
|
*
|
|
* Description:
|
|
* Update motor angle structure using electrical motor angle.
|
|
*
|
|
* Input Parameters:
|
|
* angle - (in/out) pointer to the motor angle structure
|
|
* angle_new - (in) new motor electrical angle in range <0.0, 2PI>
|
|
*
|
|
* Returned Value:
|
|
* None
|
|
*
|
|
****************************************************************************/
|
|
|
|
void motor_angle_e_update_b16(FAR struct motor_angle_b16_s *angle,
|
|
b16_t angle_new, b16_t dir)
|
|
{
|
|
LIBDSP_DEBUGASSERT(angle != NULL);
|
|
LIBDSP_DEBUGASSERT(angle_new >= 0 && angle_new <= MOTOR_ANGLE_E_MAX_B16);
|
|
LIBDSP_DEBUGASSERT(dir == DIR_CW_B16 || dir == DIR_CCW_B16);
|
|
|
|
b16_t tmp1 = 0;
|
|
b16_t tmp2 = 0;
|
|
|
|
/* Check if we crossed electrical angle boundaries */
|
|
|
|
if (dir == DIR_CW_B16)
|
|
{
|
|
/* For CW direction - previous angle is greater than current angle */
|
|
|
|
if (angle_new - angle->angle_el.angle < -POLE_CNTR_THR)
|
|
{
|
|
angle->i += 1;
|
|
}
|
|
}
|
|
|
|
else if (dir == DIR_CCW_B16)
|
|
{
|
|
/* For CCW direction - previous angle is lower than current angle */
|
|
|
|
if (angle_new - angle->angle_el.angle > POLE_CNTR_THR)
|
|
{
|
|
angle->i -= 1;
|
|
}
|
|
}
|
|
|
|
/* Reset pole counter if needed */
|
|
|
|
if (angle->i >= angle->p)
|
|
{
|
|
angle->i = 0;
|
|
}
|
|
|
|
else if (angle->i < 0)
|
|
{
|
|
angle->i = angle->p - 1;
|
|
}
|
|
|
|
/* Update electrical angle structure */
|
|
|
|
phase_angle_update_b16(&angle->angle_el, angle_new);
|
|
|
|
/* Calculate mechanical angle.
|
|
* One electrical angle rotation is equal to one mechanical rotation
|
|
* divided by number of motor pole pairs.
|
|
*/
|
|
|
|
tmp1 = b16mulb16(MOTOR_ANGLE_E_RANGE_B16, itob16(angle->i));
|
|
tmp2 = tmp1 + angle->angle_el.angle;
|
|
|
|
angle->anglem = b16mulb16(tmp2, angle->one_by_p);
|
|
|
|
/* Normalize mechanical angle to <0, 2PI> and store */
|
|
|
|
angle_norm_2pi_b16(&angle->anglem, MOTOR_ANGLE_M_MIN_B16,
|
|
MOTOR_ANGLE_M_MAX_B16);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: motor_angle_m_update_b16
|
|
*
|
|
* Description:
|
|
* Update motor angle structure using mechanical motor angle
|
|
*
|
|
* Input Parameters:
|
|
* angle - (in/out) pointer to the motor angle structure
|
|
* angle_new - (in) new motor mechanical angle in range <0.0, 2PI>
|
|
*
|
|
* Returned Value:
|
|
* None
|
|
*
|
|
****************************************************************************/
|
|
|
|
void motor_angle_m_update_b16(FAR struct motor_angle_b16_s *angle,
|
|
b16_t angle_new, b16_t dir)
|
|
{
|
|
LIBDSP_DEBUGASSERT(angle != NULL);
|
|
LIBDSP_DEBUGASSERT(angle_new >= 0 && angle_new <= MOTOR_ANGLE_E_MAX_B16);
|
|
LIBDSP_DEBUGASSERT(dir == DIR_CW_B16 || dir == DIR_CCW_B16);
|
|
|
|
b16_t angle_el = 0;
|
|
b16_t tmp1 = 0;
|
|
b16_t tmp2 = 0;
|
|
|
|
/* Store new mechanical angle */
|
|
|
|
angle->anglem = angle_new;
|
|
|
|
/* Update pole counter */
|
|
|
|
tmp1 = b16mulb16(angle->anglem, itob16(angle->p));
|
|
|
|
angle->i = (uint8_t) b16toi(b16divb16(tmp1, MOTOR_ANGLE_M_MAX_B16));
|
|
|
|
/* Get electrical angle */
|
|
|
|
tmp1 = b16mulb16(angle->anglem, itob16(angle->p));
|
|
tmp2 = b16mulb16(MOTOR_ANGLE_E_MAX_B16, itob16(angle->i));
|
|
|
|
angle_el = (tmp1 - tmp2);
|
|
|
|
/* Update electrical angle structure */
|
|
|
|
phase_angle_update_b16(&angle->angle_el, angle_el);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: motor_angle_m_get_b16
|
|
*
|
|
* Description:
|
|
* Get motor mechanical angle
|
|
*
|
|
* Input Parameters:
|
|
* angle - (in/out) pointer to the motor angle structure
|
|
*
|
|
* Returned Value:
|
|
* Return motor mechanical angle
|
|
*
|
|
****************************************************************************/
|
|
|
|
b16_t motor_angle_m_get_b16(FAR struct motor_angle_b16_s *angle)
|
|
{
|
|
LIBDSP_DEBUGASSERT(angle != NULL);
|
|
|
|
return angle->anglem;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: motor_angle_e_get_b16
|
|
*
|
|
* Description:
|
|
* Get motor electrical angle
|
|
*
|
|
* Input Parameters:
|
|
* angle - (in/out) pointer to the motor angle structure
|
|
*
|
|
* Returned Value:
|
|
* Return motor electrical angle
|
|
*
|
|
****************************************************************************/
|
|
|
|
b16_t motor_angle_e_get_b16(FAR struct motor_angle_b16_s *angle)
|
|
{
|
|
LIBDSP_DEBUGASSERT(angle != NULL);
|
|
|
|
return angle->angle_el.angle;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: motor_phy_params_init_b16
|
|
*
|
|
* Description:
|
|
* Initialize motor physical parameters
|
|
*
|
|
* Input Parameters:
|
|
* phy - (in/out) pointer to the motor physical parameters
|
|
* poles - (in) number of the motor pole pairs
|
|
* res - (in) average phase-to-neutral base motor resistance
|
|
* (without temperature compensation)
|
|
* ind - (in) average phase-to-neutral motor inductance
|
|
* flux - (in) flux linkage
|
|
*
|
|
* Returned Value:
|
|
* None
|
|
*
|
|
****************************************************************************/
|
|
|
|
void motor_phy_params_init_b16(FAR struct motor_phy_params_b16_s *phy,
|
|
uint8_t poles, b16_t res, b16_t ind,
|
|
b16_t flux_link)
|
|
{
|
|
LIBDSP_DEBUGASSERT(phy != NULL);
|
|
|
|
memset(phy, 0, sizeof(struct motor_phy_params_b16_s));
|
|
|
|
phy->p = poles;
|
|
phy->flux_link = flux_link;
|
|
phy->res = res;
|
|
phy->ind = ind;
|
|
phy->one_by_ind = b16divb16(b16ONE, ind);
|
|
phy->one_by_p = b16divb16(b16ONE, poles);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: pmsm_phy_params_init_b16
|
|
*
|
|
* Description:
|
|
* Initialize PMSM physical parameters
|
|
*
|
|
* Input Parameters:
|
|
* phy - (in/out) pointer to the PMSM physical parameters
|
|
* poles - (in) number of the motor pole pairs
|
|
* res - (in) average phase-to-neutral base motor resistance
|
|
* (without temperature compensation)
|
|
* ind - (in) average phase-to-neutral motor inductance
|
|
* iner - (in) rotor inertia (J)
|
|
* flux - (in) flux linkage
|
|
* ind_d - (in) d-inductance
|
|
* ind_q - (in) q-inductance
|
|
*
|
|
* Returned Value:
|
|
* None
|
|
*
|
|
****************************************************************************/
|
|
|
|
void pmsm_phy_params_init_b16(FAR struct pmsm_phy_params_b16_s *phy,
|
|
uint8_t poles, b16_t res, b16_t ind,
|
|
b16_t iner, b16_t flux,
|
|
b16_t ind_d, b16_t ind_q)
|
|
{
|
|
LIBDSP_DEBUGASSERT(phy != NULL);
|
|
|
|
/* Initialize motor phy */
|
|
|
|
motor_phy_params_init_b16(&phy->motor, poles, res, ind, flux);
|
|
|
|
/* Iniitalize PMSM specific data */
|
|
|
|
phy->iner = iner;
|
|
phy->ind_d = ind_d;
|
|
phy->ind_q = ind_q;
|
|
phy->one_by_iner = b16divb16(b16ONE, iner);
|
|
phy->one_by_indd = b16divb16(b16ONE, ind_d);
|
|
phy->one_by_indq = b16divb16(b16ONE, ind_q);
|
|
}
|