motor_ctl-4/tests/unit_test/mc_status.c

#include "mc_status.h"
#include "string.h"

void McStatusInit(McStatus *s, McPuBase *base)
{
    s->Base = base;
    memset(&s->Named, 0, sizeof(McStatusNamed));
    memset(&s->Pu, 0, sizeof(McStatusPu));
}

void McStatusSyncNamed(McStatus *s)
{
    McPuBase      *b = s->Base;
    McStatusNamed *n = &s->Named;
    McStatusPu    *p = &s->Pu;

    n->Ia    = (s16)((p->swIa * b->uwIbAp) >> 14);
    n->Ib    = (s16)((p->swIb * b->uwIbAp) >> 14);
    n->Ic    = (s16)((p->swIc * b->uwIbAp) >> 14);
    n->IpeakAbs = (s16)((p->swIpeakAbs * b->uwIbAp) >> 14);

    n->Ialpha = (s16)((p->swIalpha * b->uwIbAp) >> 14);
    n->Ibeta  = (s16)((p->swIbeta * b->uwIbAp) >> 14);
    n->Izero  = (s16)((p->swIzero * b->uwIbAp) >> 14);

    n->Id = (s16)(((s32)p->swId * b->uwIbAp) >> 14);
    n->Iq = (s16)(((s32)p->swIq * b->uwIbAp) >> 14);
    n->I0 = (s16)(((s32)p->swI0 * b->uwIbAp) >> 14);

    n->Ua = (s16)(((s32)p->swUa * b->uwUbVt) >> 14);
    n->Ub = (s16)(((s32)p->swUb * b->uwUbVt) >> 14);
    n->Uc = (s16)(((s32)p->swUc * b->uwUbVt) >> 14);

    n->Ualpha = (s16)(((s32)p->swUalpha * b->uwUbVt) >> 14);
    n->Ubeta  = (s16)(((s32)p->swUbeta * b->uwUbVt) >> 14);
    n->Uzero  = (s16)(((s32)p->swUzero * b->uwUbVt) >> 14);

    n->ElecAngle = (s16)(((s32)p->swElecAngle * 360) >> 15);
    n->ElecOmega = (s16)(((s32)p->swElecOmega * b->uwWeb) >> 15);
    n->MechAngle = (s16)(((s32)p->swMechAngle * 360) >> 15);
    n->MechOmega = (s16)(((s32)p->swMechOmega * b->uwWeb) >> 15);

    n->ElecFreq = (s16)(((s32)p->swElecFreq * b->uwFbHz) >> 15);

    n->Uac = (s16)(((s32)p->swUac * b->uwUbVt) >> 14);
    n->Udc = (s16)(((s32)p->swUdc * b->uwUbVt) >> 14);

    n->TempIgbt  = p->swTempIgbt;
    n->TempMotor = p->swTempMotor;
    n->TempInv   = p->swTempInv;
}

void McStatusSyncPu(McStatus *s)
{
    McPuBase      *b = s->Base;
    McStatusNamed *n = &s->Named;
    McStatusPu    *p = &s->Pu;

    p->swIa    = (s16)(((s32)n->Ia << 14) / b->uwIbAp);
    p->swIb    = (s16)(((s32)n->Ib << 14) / b->uwIbAp);
    p->swIc    = (s16)(((s32)n->Ic << 14) / b->uwIbAp);
    p->swIpeakAbs = (s16)(((s32)n->IpeakAbs << 14) / b->uwIbAp);

    p->swIalpha = (s16)(((s32)n->Ialpha << 14) / b->uwIbAp);
    p->swIalpha = (s16)(((s32)n->Ibeta << 14) / b->uwIbAp);
    p->swIzero  = (s16)(((s32)n->Izero << 14) / b->uwIbAp);

    p->swId = (s16)(((s32)n->Id << 14) / b->uwIbAp);
    p->swIq = (s16)(((s32)n->Iq << 14) / b->uwIbAp);
    p->swI0 = (s16)(((s32)n->I0 << 14) / b->uwIbAp);

    p->swUa = (s16)(((s32)n->Ua << 14) / b->uwUbVt);
    p->swUb = (s16)(((s32)n->Ub << 14) / b->uwUbVt);
    p->swUc = (s16)(((s32)n->Uc << 14) / b->uwUbVt);

    p->swUalpha = (s16)(((s32)n->Ualpha << 14) / b->uwUbVt);
    p->swUbeta  = (s16)(((s32)n->Ubeta << 14) / b->uwUbVt);
    p->swUzero  = (s16)(((s32)n->Uzero << 14) / b->uwUbVt);

    p->swElecAngle = (s16)((s32)(n->ElecAngle << 15) / 360);
    p->swElecOmega = (s16)((s32)(n->ElecOmega << 15) / b->uwWeb / 10);
    p->swMechAngle = (s16)((s32)(n->MechAngle << 15) / 360);
    p->swMechOmega = (s16)((s32)(n->MechOmega << 15) / b->uwWeb / 10);

    p->swElecFreq = (s16)(((s32)n->ElecFreq << 15) / b->uwFbHz / 10);

    p->swUac = (s16)(((s32)n->Uac << 14) / b->uwUbVt);
    p->swUdc = (s16)(((s32)n->Udc << 14) / b->uwUbVt);

    p->swTempIgbt  = n->TempIgbt;
    p->swTempMotor = n->TempMotor;
    p->swTempInv   = n->TempInv;
}