sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
5ae5ab7f44
nuttx-apps/industry/foc/float/foc_ident.c

614 lines
15 KiB
C
Raw Blame History

/****************************************************************************
* 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)
#define IDENT_PI_KI (0.05f)
/****************************************************************************
* 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,
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 */
};
/****************************************************************************
* 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;
static float curr_sum = 0.0f;
static float volt_sum = 0.0f;
/* Initialize PI controller */
if (ident->cntr == 0)
{
pi_controller_init(&ident->pi, IDENT_PI_KP, IDENT_PI_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))
{
volt_sum += vector2d_mag(in->foc_state->vdq.q, in->foc_state->vdq.d);
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) * volt_sum / 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 data */
curr_sum = 0.0f;
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;
static float sign = 1.0f;
static float curr1_sum = 0.0f;
static float curr2_sum = 0.0f;
/* If previous sign was -1 then we have top current,
* if previous sing was +1 then we have bottom current.
*/
if (sign > 0)
{
/* Average bottm current */
curr1_sum += in->foc_state->idq.d;
}
else
{
/* Average top current */
curr2_sum += in->foc_state->idq.d;
}
/* Invert voltage to generate square wave D voltage */
sign = -sign;
vref = 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 * curr1_sum / ident->cntr;
curr2_avg = 2 * 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 static data */
sign = 1.0f;
curr1_sum = 0.0f;
curr2_sum = 0.0f;
}
return ret;
}
/****************************************************************************
* 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)
{
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;
}
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_current <= 0.0f || i->cfg.ind_volt <= 0.0f)
{
ret = -EINVAL;
goto errout;
}
if (i->cfg.res_steps <= 0 || i->cfg.ind_steps <= 0 ||
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;
/* 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_IDLE1:
case FOC_IDENT_RUN_IDLE2:
case FOC_IDENT_RUN_IDLE3:
{
/* 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;
}
case FOC_IDENT_RUN_DONE:
{
ret = FOC_ROUTINE_RUN_DONE;
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;
}
Reference in New Issue View Git Blame Copy Permalink