812 lines
20 KiB
C
812 lines
20 KiB
C
/****************************************************************************
|
|
* apps/industry/foc/float/foc_ident.c
|
|
* This file implements motor ident routine 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/foc_common.h"
|
|
#include "industry/foc/foc_log.h"
|
|
|
|
#include "industry/foc/float/foc_ident.h"
|
|
|
|
/****************************************************************************
|
|
* Pre-processor Definitions
|
|
****************************************************************************/
|
|
|
|
#define IDENT_PI_KP (0.0f)
|
|
|
|
/****************************************************************************
|
|
* Private Data Types
|
|
****************************************************************************/
|
|
|
|
/* Identification stages */
|
|
|
|
enum foc_ident_run_stage_e
|
|
{
|
|
FOC_IDENT_RUN_INIT,
|
|
FOC_IDENT_RUN_IDLE1,
|
|
FOC_IDENT_RUN_RES,
|
|
FOC_IDENT_RUN_IDLE2,
|
|
FOC_IDENT_RUN_IND,
|
|
FOC_IDENT_RUN_IDLE3,
|
|
#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX
|
|
FOC_IDENT_RUN_FLU,
|
|
FOC_IDENT_RUN_IDLE4,
|
|
#endif
|
|
FOC_IDENT_RUN_DONE
|
|
};
|
|
|
|
/* Ident routine private data */
|
|
|
|
struct foc_ident_f32_s
|
|
{
|
|
struct foc_routine_ident_cfg_f32_s cfg; /* Ident configuration */
|
|
struct foc_routine_ident_final_f32_s final; /* Ident final result */
|
|
pid_controller_f32_t pi; /* PI controller for res */
|
|
int cntr; /* Helper counter */
|
|
int stage; /* Ident stage */
|
|
|
|
/* global data in resistance identification */
|
|
|
|
float curr_sum;
|
|
float volt_sum;
|
|
|
|
/* global data in inductance identification */
|
|
|
|
float sign;
|
|
float curr1_sum;
|
|
float curr2_sum;
|
|
#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX
|
|
/* global data in flux linkage identification */
|
|
|
|
float vq_sum;
|
|
float vd_sum;
|
|
float iq_sum;
|
|
float id_sum;
|
|
float vel;
|
|
int aveg_sum;
|
|
#endif
|
|
};
|
|
|
|
/****************************************************************************
|
|
* Private Function Prototypes
|
|
****************************************************************************/
|
|
|
|
int foc_routine_ident_init_f32(FAR foc_routine_f32_t *r);
|
|
void foc_routine_ident_deinit_f32(FAR foc_routine_f32_t *r);
|
|
int foc_routine_ident_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg);
|
|
int foc_routine_ident_run_f32(FAR foc_routine_f32_t *r,
|
|
FAR struct foc_routine_in_f32_s *in,
|
|
FAR struct foc_routine_out_f32_s *out);
|
|
int foc_routine_ident_final_f32(FAR foc_routine_f32_t *r, FAR void *data);
|
|
|
|
/****************************************************************************
|
|
* Public Data
|
|
****************************************************************************/
|
|
|
|
struct foc_routine_ops_f32_s g_foc_routine_ident_f32 =
|
|
{
|
|
.init = foc_routine_ident_init_f32,
|
|
.deinit = foc_routine_ident_deinit_f32,
|
|
.cfg = foc_routine_ident_cfg_f32,
|
|
.run = foc_routine_ident_run_f32,
|
|
.final = foc_routine_ident_final_f32,
|
|
};
|
|
|
|
/****************************************************************************
|
|
* Private Functions
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Name: foc_ident_idle_run_f32
|
|
*
|
|
* Description:
|
|
* Force IDLE state
|
|
*
|
|
* Input Parameter:
|
|
* ident - pointer to FOC ident routine
|
|
* in - pointer to FOC routine input data
|
|
* out - pointer to FOC routine output data
|
|
*
|
|
****************************************************************************/
|
|
|
|
int foc_ident_idle_run_f32(FAR struct foc_ident_f32_s *ident,
|
|
FAR struct foc_routine_in_f32_s *in,
|
|
FAR struct foc_routine_out_f32_s *out)
|
|
{
|
|
int ret = FOC_ROUTINE_RUN_NOTDONE;
|
|
|
|
/* Get output */
|
|
|
|
out->dq_ref.q = 0.0f;
|
|
out->dq_ref.d = 0.0f;
|
|
out->vdq_comp.q = 0.0f;
|
|
out->vdq_comp.d = 0.0f;
|
|
out->angle = 0.0f;
|
|
out->foc_mode = FOC_HANDLER_MODE_IDLE;
|
|
|
|
/* Increase counter */
|
|
|
|
ident->cntr += 1;
|
|
|
|
if (ident->cntr > ident->cfg.idle_steps)
|
|
{
|
|
/* Done */
|
|
|
|
ret = FOC_ROUTINE_RUN_DONE;
|
|
|
|
/* Reset counter */
|
|
|
|
ident->cntr = 0;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: foc_ident_res_run_f32
|
|
*
|
|
* Description:
|
|
* Run resistance identification routine
|
|
*
|
|
* Input Parameter:
|
|
* ident - pointer to FOC ident routine
|
|
* in - pointer to FOC routine input data
|
|
* out - pointer to FOC routine output data
|
|
*
|
|
****************************************************************************/
|
|
|
|
int foc_ident_res_run_f32(FAR struct foc_ident_f32_s *ident,
|
|
FAR struct foc_routine_in_f32_s *in,
|
|
FAR struct foc_routine_out_f32_s *out)
|
|
{
|
|
int ret = FOC_ROUTINE_RUN_NOTDONE;
|
|
float err = 0.0f;
|
|
float vref = 0.0f;
|
|
|
|
/* Initialize PI controller */
|
|
|
|
if (ident->cntr == 0)
|
|
{
|
|
DEBUGASSERT(ident->cfg.res_ki > 0.0f);
|
|
pi_controller_init(&ident->pi, IDENT_PI_KP, ident->cfg.res_ki);
|
|
}
|
|
|
|
/* PI saturation */
|
|
|
|
pi_saturation_set(&ident->pi, 0.0f, in->vbus);
|
|
|
|
/* Lock motor with given current */
|
|
|
|
err = ident->cfg.res_current - in->foc_state->idq.d;
|
|
vref = pi_controller(&ident->pi, err);
|
|
|
|
/* Force alpha voltage = vref */
|
|
|
|
out->dq_ref.q = 0.0f;
|
|
out->dq_ref.d = vref;
|
|
out->vdq_comp.q = 0.0f;
|
|
out->vdq_comp.d = 0.0f;
|
|
out->angle = 0.0f;
|
|
out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
|
|
|
|
/* Increase counter */
|
|
|
|
ident->cntr += 1;
|
|
if (ident->cntr > (ident->cfg.res_steps / 3))
|
|
{
|
|
ident->volt_sum += vector2d_mag(in->foc_state->vdq.q,
|
|
in->foc_state->vdq.d);
|
|
ident->curr_sum += vector2d_mag(in->foc_state->idq.q,
|
|
in->foc_state->idq.d);
|
|
}
|
|
|
|
if (ident->cntr > ident->cfg.res_steps)
|
|
{
|
|
/* Get resistance */
|
|
|
|
ident->final.res = (2.0f / 3.0f) * ident->volt_sum / ident->curr_sum;
|
|
|
|
/* Force IDLE state */
|
|
|
|
out->dq_ref.q = 0.0f;
|
|
out->dq_ref.d = 0.0f;
|
|
out->vdq_comp.q = 0.0f;
|
|
out->vdq_comp.d = 0.0f;
|
|
out->angle = 0.0f;
|
|
out->foc_mode = FOC_HANDLER_MODE_IDLE;
|
|
|
|
/* Resistance identification done */
|
|
|
|
ret = FOC_ROUTINE_RUN_DONE;
|
|
|
|
/* Reset counter */
|
|
|
|
ident->cntr = 0;
|
|
|
|
/* Reset static curr_sum and volt_sum */
|
|
|
|
ident->curr_sum = 0.0f;
|
|
ident->volt_sum = 0.0f;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: foc_ident_ind_run_f32
|
|
*
|
|
* Description:
|
|
* Run inductance identification routine
|
|
*
|
|
* Input Parameter:
|
|
* ident - pointer to FOC ident routine
|
|
* in - pointer to FOC routine input data
|
|
* out - pointer to FOC routine output data
|
|
*
|
|
****************************************************************************/
|
|
|
|
int foc_ident_ind_run_f32(FAR struct foc_ident_f32_s *ident,
|
|
FAR struct foc_routine_in_f32_s *in,
|
|
FAR struct foc_routine_out_f32_s *out)
|
|
{
|
|
int ret = FOC_ROUTINE_RUN_NOTDONE;
|
|
float vref = 0.0f;
|
|
float curr1_avg = 0.0f;
|
|
float curr2_avg = 0.0f;
|
|
float delta_curr = 0.0f;
|
|
|
|
/* If previous sign was -1 then we have top current,
|
|
* if previous sing was +1 then we have bottom current.
|
|
*/
|
|
|
|
if (ident->sign > 0)
|
|
{
|
|
/* Average bottm current */
|
|
|
|
ident->curr1_sum += in->foc_state->idq.d;
|
|
}
|
|
else
|
|
{
|
|
/* Average top current */
|
|
|
|
ident->curr2_sum += in->foc_state->idq.d;
|
|
}
|
|
|
|
/* Invert voltage to generate square wave D voltage */
|
|
|
|
ident->sign = -ident->sign;
|
|
vref = ident->sign * ident->cfg.ind_volt;
|
|
|
|
/* Force alpha voltage = vref */
|
|
|
|
out->dq_ref.q = 0.0f;
|
|
out->dq_ref.d = vref;
|
|
out->vdq_comp.q = 0.0f;
|
|
out->vdq_comp.d = 0.0f;
|
|
out->angle = 0.0f;
|
|
out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
|
|
|
|
/* Increase counter */
|
|
|
|
ident->cntr += 1;
|
|
|
|
if (ident->cntr > ident->cfg.ind_steps)
|
|
{
|
|
/* Half samples from curr1, other half from curr2 */
|
|
|
|
curr1_avg = 2 * ident->curr1_sum / ident->cntr;
|
|
curr2_avg = 2 * ident->curr2_sum / ident->cntr;
|
|
|
|
/* Average delta current */
|
|
|
|
delta_curr = curr1_avg - curr2_avg;
|
|
|
|
/* Get inductance
|
|
* L = V * t / dI
|
|
*
|
|
* where:
|
|
* t = per
|
|
* V = ind_volt
|
|
* dI = avg(curr_bottom) - avg(curr_top)
|
|
*/
|
|
|
|
ident->final.ind = ident->cfg.ind_volt * ident->cfg.per / delta_curr;
|
|
|
|
/* Force IDLE state */
|
|
|
|
out->dq_ref.q = 0.0f;
|
|
out->dq_ref.d = 0.0f;
|
|
out->vdq_comp.q = 0.0f;
|
|
out->vdq_comp.d = 0.0f;
|
|
out->angle = 0.0f;
|
|
out->foc_mode = FOC_HANDLER_MODE_IDLE;
|
|
|
|
/* Inductance identification done */
|
|
|
|
ret = FOC_ROUTINE_RUN_DONE;
|
|
|
|
/* Reset counter */
|
|
|
|
ident->cntr = 0;
|
|
|
|
/* Reset curr1_sum curr2_sum and sign */
|
|
|
|
ident->sign = 1.0f;
|
|
ident->curr1_sum = 0.0f;
|
|
ident->curr2_sum = 0.0f;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX
|
|
/****************************************************************************
|
|
* Name: foc_ident_fluxlink_run_f32
|
|
*
|
|
* Description:
|
|
* Run flux linkage identification routine
|
|
*
|
|
* Input Parameter:
|
|
* ident - pointer to FOC ident routine
|
|
* in - pointer to FOC routine input data
|
|
* out - pointer to FOC routine output data
|
|
*
|
|
****************************************************************************/
|
|
|
|
int foc_ident_fluxlink_run_f32(FAR struct foc_ident_f32_s *ident,
|
|
FAR struct foc_routine_in_f32_s *in,
|
|
FAR struct foc_routine_out_f32_s *out)
|
|
{
|
|
int ret = FOC_ROUTINE_RUN_NOTDONE;
|
|
float curr_avg = 0.0f;
|
|
float volt_avg = 0.0f;
|
|
|
|
DEBUGASSERT(ident->cfg.cb.zero != NULL);
|
|
DEBUGASSERT(ident->cfg.cb.angle != NULL);
|
|
DEBUGASSERT(ident->cfg.cb.speed != NULL);
|
|
|
|
if (ident->cfg.cb.angle != NULL)
|
|
{
|
|
/* Update openloop angle by angle callbacks */
|
|
|
|
out->angle = ident->cfg.cb.angle(ident->cfg.cb.priv_angle,
|
|
ident->vel, DIR_CW);
|
|
}
|
|
|
|
if (ident->cfg.cb.speed != NULL)
|
|
{
|
|
/* Update openloop speed by speed callbacks */
|
|
|
|
ident->vel = ident->cfg.cb.speed(ident->cfg.cb.priv_speed,
|
|
ident->cfg.flux_vel, ident->vel);
|
|
}
|
|
|
|
/* Force q axis current = ident->cfg.flux_curr */
|
|
|
|
out->dq_ref.q = ident->cfg.flux_curr;
|
|
out->dq_ref.d = 0.0f;
|
|
out->vdq_comp.q = 0.0f;
|
|
out->vdq_comp.d = 0.0f;
|
|
out->foc_mode = FOC_HANDLER_MODE_CURRENT;
|
|
|
|
/* Increase counter */
|
|
|
|
ident->cntr += 1;
|
|
if (ident->vel >= ident->cfg.flux_vel)
|
|
{
|
|
ident->vq_sum += in->foc_state->vdq.q;
|
|
ident->vd_sum += in->foc_state->vdq.d;
|
|
ident->iq_sum += in->foc_state->idq.q;
|
|
ident->id_sum += in->foc_state->idq.d;
|
|
ident->aveg_sum++;
|
|
}
|
|
|
|
if (ident->cntr > ident->cfg.flux_steps)
|
|
{
|
|
volt_avg = vector2d_mag(ident->vq_sum / ident->aveg_sum,
|
|
ident->vd_sum / ident->aveg_sum);
|
|
curr_avg = vector2d_mag(ident->iq_sum / ident->aveg_sum,
|
|
ident->id_sum / ident->aveg_sum);
|
|
|
|
/* Get flux linkage */
|
|
|
|
ident->final.flux = (volt_avg - ident->final.res * curr_avg) /
|
|
ident->vel - ident->final.ind * curr_avg;
|
|
|
|
/* Force IDLE state */
|
|
|
|
out->dq_ref.q = 0.0f;
|
|
out->dq_ref.d = 0.0f;
|
|
out->vdq_comp.q = 0.0f;
|
|
out->vdq_comp.d = 0.0f;
|
|
out->angle = 0.0f;
|
|
out->foc_mode = FOC_HANDLER_MODE_IDLE;
|
|
|
|
/* Reset global value of open loop angle */
|
|
|
|
if (ident->cfg.cb.zero != NULL)
|
|
{
|
|
ret = ident->cfg.cb.zero(ident->cfg.cb.priv_angle);
|
|
if (ret < 0)
|
|
{
|
|
FOCLIBERR("ERROR: ident zero callback failed %d!\n", ret);
|
|
goto errout;
|
|
}
|
|
}
|
|
|
|
/* Flux linkage identification done */
|
|
|
|
ret = FOC_ROUTINE_RUN_DONE;
|
|
|
|
/* Reset counter */
|
|
|
|
ident->cntr = 0;
|
|
|
|
/* Reset global data */
|
|
|
|
ident->vq_sum = 0.0f;
|
|
ident->vd_sum = 0.0f;
|
|
ident->iq_sum = 0.0f;
|
|
ident->id_sum = 0.0f;
|
|
ident->vel = 0.0f;
|
|
ident->aveg_sum = 0;
|
|
}
|
|
|
|
errout:
|
|
return ret;
|
|
}
|
|
#endif
|
|
|
|
/****************************************************************************
|
|
* Name: foc_routine_ident_init_f32
|
|
*
|
|
* Description:
|
|
* Initialize the FOC ident routine (float32)
|
|
*
|
|
* Input Parameter:
|
|
* r - pointer to FOC routine
|
|
*
|
|
****************************************************************************/
|
|
|
|
int foc_routine_ident_init_f32(FAR foc_routine_f32_t *r)
|
|
{
|
|
FAR struct foc_ident_f32_s *i = NULL;
|
|
int ret = OK;
|
|
|
|
DEBUGASSERT(r);
|
|
|
|
/* Connect angle data */
|
|
|
|
r->data = zalloc(sizeof(struct foc_ident_f32_s));
|
|
if (r->data == NULL)
|
|
{
|
|
ret = -ENOMEM;
|
|
goto errout;
|
|
}
|
|
|
|
i = r->data;
|
|
i->sign = 1.0f;
|
|
|
|
errout:
|
|
return ret;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: foc_routine_ident_deinit_f32
|
|
*
|
|
* Description:
|
|
* Deinitialize the FOC ident routine (float32)
|
|
*
|
|
* Input Parameter:
|
|
* r - pointer to FOC routine
|
|
*
|
|
****************************************************************************/
|
|
|
|
void foc_routine_ident_deinit_f32(FAR foc_routine_f32_t *r)
|
|
{
|
|
DEBUGASSERT(r);
|
|
|
|
if (r->data)
|
|
{
|
|
/* Free routine data */
|
|
|
|
free(r->data);
|
|
}
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: foc_routine_ident_cfg_f32
|
|
*
|
|
* Description:
|
|
* Configure the FOC ident routine (float32)
|
|
*
|
|
* Input Parameter:
|
|
* r - pointer to FOC routine
|
|
* cfg - pointer to ident routine configuration data
|
|
*
|
|
****************************************************************************/
|
|
|
|
int foc_routine_ident_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg)
|
|
{
|
|
FAR struct foc_ident_f32_s *i = NULL;
|
|
int ret = OK;
|
|
|
|
DEBUGASSERT(r);
|
|
|
|
/* Get ident data */
|
|
|
|
DEBUGASSERT(r->data);
|
|
i = r->data;
|
|
|
|
/* Copy configuration */
|
|
|
|
memcpy(&i->cfg, cfg, sizeof(struct foc_routine_ident_cfg_f32_s));
|
|
|
|
/* Verify configuration */
|
|
|
|
if (i->cfg.per <= 0.0f)
|
|
{
|
|
ret = -EINVAL;
|
|
goto errout;
|
|
}
|
|
|
|
if (i->cfg.res_ki <= 0.0f)
|
|
{
|
|
ret = -EINVAL;
|
|
goto errout;
|
|
}
|
|
|
|
if (i->cfg.res_current <= 0.0f || i->cfg.ind_volt <= 0.0f
|
|
#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX
|
|
|| i->cfg.flux_curr <= 0.0f
|
|
#endif
|
|
)
|
|
{
|
|
ret = -EINVAL;
|
|
goto errout;
|
|
}
|
|
|
|
if (i->cfg.res_steps <= 0 || i->cfg.ind_steps <= 0 ||
|
|
#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX
|
|
i->cfg.flux_steps <= 0 ||
|
|
#endif
|
|
i->cfg.idle_steps <= 0)
|
|
{
|
|
ret = -EINVAL;
|
|
goto errout;
|
|
}
|
|
|
|
errout:
|
|
return ret;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: foc_routine_ident_run_f32
|
|
*
|
|
* Description:
|
|
* Run the FOC ident routine step (float32)
|
|
*
|
|
* Input Parameter:
|
|
* r - pointer to FOC routine
|
|
* in - pointer to FOC routine input data
|
|
* out - pointer to FOC routine output data
|
|
*
|
|
****************************************************************************/
|
|
|
|
int foc_routine_ident_run_f32(FAR foc_routine_f32_t *r,
|
|
FAR struct foc_routine_in_f32_s *in,
|
|
FAR struct foc_routine_out_f32_s *out)
|
|
{
|
|
FAR struct foc_ident_f32_s *i = NULL;
|
|
int ret = FOC_ROUTINE_RUN_NOTDONE;
|
|
|
|
DEBUGASSERT(r);
|
|
DEBUGASSERT(in);
|
|
DEBUGASSERT(out);
|
|
|
|
/* Get ident data */
|
|
|
|
DEBUGASSERT(r->data);
|
|
i = r->data;
|
|
#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX
|
|
DEBUGASSERT(i->cfg.cb.kpki != NULL);
|
|
#endif
|
|
|
|
/* Force IDLE state at default */
|
|
|
|
out->dq_ref.q = 0.0f;
|
|
out->dq_ref.d = 0.0f;
|
|
out->vdq_comp.q = 0.0f;
|
|
out->vdq_comp.d = 0.0f;
|
|
out->angle = 0.0f;
|
|
out->foc_mode = FOC_HANDLER_MODE_IDLE;
|
|
|
|
/* Handle ident stage */
|
|
|
|
switch (i->stage)
|
|
{
|
|
case FOC_IDENT_RUN_INIT:
|
|
{
|
|
i->stage += 1;
|
|
ret = FOC_ROUTINE_RUN_NOTDONE;
|
|
|
|
break;
|
|
}
|
|
|
|
case FOC_IDENT_RUN_IDLE3:
|
|
#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX
|
|
if (i->cntr == 0)
|
|
{
|
|
ret = i->cfg.cb.kpki(i->cfg.cb.priv_kpki, i->final.res,
|
|
i->final.ind, i->cfg.per);
|
|
if (ret < 0)
|
|
{
|
|
FOCLIBERR("ERROR: ident kpki callback failed %d!\n", ret);
|
|
goto errout;
|
|
}
|
|
}
|
|
|
|
case FOC_IDENT_RUN_IDLE4:
|
|
#endif
|
|
case FOC_IDENT_RUN_IDLE1:
|
|
case FOC_IDENT_RUN_IDLE2:
|
|
{
|
|
/* De-energetize motor */
|
|
|
|
ret = foc_ident_idle_run_f32(i, in, out);
|
|
if (ret < 0)
|
|
{
|
|
goto errout;
|
|
}
|
|
|
|
if (ret == FOC_ROUTINE_RUN_DONE)
|
|
{
|
|
i->stage += 1;
|
|
ret = FOC_ROUTINE_RUN_NOTDONE;
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case FOC_IDENT_RUN_RES:
|
|
{
|
|
/* Resistance */
|
|
|
|
ret = foc_ident_res_run_f32(i, in, out);
|
|
if (ret < 0)
|
|
{
|
|
goto errout;
|
|
}
|
|
|
|
if (ret == FOC_ROUTINE_RUN_DONE)
|
|
{
|
|
FOCLIBLOG("IDENT RES done!\n");
|
|
|
|
i->stage += 1;
|
|
ret = FOC_ROUTINE_RUN_NOTDONE;
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case FOC_IDENT_RUN_IND:
|
|
{
|
|
/* Inductance */
|
|
|
|
ret = foc_ident_ind_run_f32(i, in, out);
|
|
if (ret < 0)
|
|
{
|
|
goto errout;
|
|
}
|
|
|
|
if (ret == FOC_ROUTINE_RUN_DONE)
|
|
{
|
|
FOCLIBLOG("IDENT IND done!\n");
|
|
|
|
i->stage += 1;
|
|
ret = FOC_ROUTINE_RUN_NOTDONE;
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX
|
|
case FOC_IDENT_RUN_FLU:
|
|
{
|
|
/* Flux linkage */
|
|
|
|
ret = foc_ident_fluxlink_run_f32(i, in, out);
|
|
if (ret < 0)
|
|
{
|
|
goto errout;
|
|
}
|
|
|
|
if (ret == FOC_ROUTINE_RUN_DONE)
|
|
{
|
|
FOCLIBLOG("IDENT FLU done!\n");
|
|
|
|
i->stage += 1;
|
|
ret = FOC_ROUTINE_RUN_NOTDONE;
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
#endif
|
|
case FOC_IDENT_RUN_DONE:
|
|
{
|
|
ret = FOC_ROUTINE_RUN_DONE;
|
|
i->stage = FOC_IDENT_RUN_INIT;
|
|
|
|
break;
|
|
}
|
|
|
|
default:
|
|
{
|
|
FOCLIBERR("ERROR: invalid ident stage %d\n", i->stage);
|
|
ret = -EINVAL;
|
|
goto errout;
|
|
}
|
|
}
|
|
|
|
errout:
|
|
return ret;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: foc_routine_ident_final_f32
|
|
*
|
|
* Description:
|
|
* Finalize the FOC routine data (float32)
|
|
*
|
|
* Input Parameter:
|
|
* r - pointer to FOC routine
|
|
* data - pointer to FOC ident routine final data
|
|
*
|
|
****************************************************************************/
|
|
|
|
int foc_routine_ident_final_f32(FAR foc_routine_f32_t *r, FAR void *data)
|
|
{
|
|
FAR struct foc_ident_f32_s *i = NULL;
|
|
|
|
DEBUGASSERT(r);
|
|
DEBUGASSERT(data);
|
|
|
|
/* Get ident data */
|
|
|
|
DEBUGASSERT(r->data);
|
|
i = r->data;
|
|
|
|
/* Get final data */
|
|
|
|
memcpy(data, &i->final, sizeof(struct foc_routine_ident_final_f32_s));
|
|
|
|
return OK;
|
|
}
|