Diff: lib_L6470DC.cpp

Revision:

6:7ca2eef852fd

Child:

7:2ff9788e2827

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib_L6470DC.cpp	Fri Apr 01 11:38:25 2022 +0000
@@ -0,0 +1,731 @@
+#include "mbed.h"
+#include "lib_L6470DC.h"
+
+////////////////////////////////////////////////////////////////////////////////
+//                              CONSTRUCTOR                                   //
+////////////////////////////////////////////////////////////////////////////////
+
+L6470DC::L6470DC(int nMax, PinName mosi, PinName miso, PinName sclk, PinName cs):m_spi(mosi, miso, sclk),m_cs(cs), nMotMax(nMax)
+{
+    m_cs = 1;
+    m_spi.format(8,3);
+    m_spi.frequency(5000000);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//                              BASICS_FUNC                                   //
+////////////////////////////////////////////////////////////////////////////////
+
+unsigned char L6470DC::send(unsigned char nMOT,unsigned char temp)
+{
+    unsigned char temp0;
+    int i;
+    m_cs = 0;
+    for (i = nMotMax - 1; i >= 0; i--) {
+        if(i==nMOT) temp =(unsigned char) m_spi.write(temp);
+        else temp0 =(unsigned char) m_spi.write(0x00);
+    }
+
+    m_cs = 1;
+    wait_us(1);
+    return(temp);
+}
+
+void L6470DC::send_bytes(unsigned char nMOT,unsigned char temp[],int i)
+{
+    while(0 < i--) {
+        temp[i] = send(nMOT,temp[i]);
+    }
+}
+
+void L6470DC::NOP(unsigned char nMOT)
+{
+    send(nMOT,0x00);
+}
+
+void L6470DC::SetParam(unsigned char nMOT,L6470_Register param,int value)
+{
+    int length,address;
+
+    switch(param) {
+        case ABS_POS:
+            length  = LEN_ABS_POS;
+            address = ADD_ABS_POS;
+            break;
+        case EL_POS:
+            length  = LEN_EL_POS;
+            address = ADD_EL_POS;
+            break;
+        case MARK:
+            length  = LEN_MARK;
+            address = ADD_MARK;
+            break;
+        case SPEED:
+            length  = LEN_SPEED;
+            address = ADD_SPEED;
+            break;
+        case ACC:
+            length  = LEN_ACC;
+            address = ADD_ACC;
+            break;
+        case DEC:
+            length  = LEN_DEC;
+            address = ADD_DEC;
+            break;
+        case MAX_SPEED:
+            length  = LEN_MAX_SPEED;
+            address = ADD_MAX_SPEED;
+            break;
+        case MIN_SPEED:
+            length  = LEN_MIN_SPEED;
+            address = ADD_MIN_SPEED;
+            break;
+        case KVAL_HOLD:
+            length  = LEN_KVAL_HOLD;
+            address = ADD_KVAL_HOLD;
+            break;
+        case KVAL_RUN:
+            length  = LEN_KVAL_RUN;
+            address = ADD_KVAL_RUN;
+            break;
+        case KVAL_ACC:
+            length  = LEN_KVAL_ACC;
+            address = ADD_KVAL_ACC;
+            break;
+        case KVAL_DEC:
+            length  = LEN_KVAL_DEC;
+            address = ADD_KVAL_DEC;
+            break;
+        case INT_SPD:
+            length  = LEN_INT_SPD;
+            address = ADD_INT_SPD;
+            break;
+        case ST_SLP:
+            length  = LEN_ST_SLP;
+            address = ADD_ST_SLP;
+            break;
+        case FN_SLP_ACC:
+            length  = LEN_FN_SLP_ACC;
+            address = ADD_FN_SLP_ACC;
+            break;
+        case FN_SLP_DEC:
+            length  = LEN_FN_SLP_DEC;
+            address = ADD_FN_SLP_DEC;
+            break;
+        case K_THERA:
+            length  = LEN_K_THERA;
+            address = ADD_K_THERA;
+            break;
+        case ADC_OUT:
+            length  = LEN_ADC_OUT;
+            address = ADD_ADC_OUT;
+            break;
+        case OCR_TH:
+            length  = LEN_OCR_TH;
+            address = ADD_OCR_TH;
+            break;
+        case STALL_TH:
+            length  = LEN_STALL_TH;
+            address = ADD_STALL_TH;
+            break;
+        case FS_SPD:
+            length  = LEN_FS_SPD;
+            address = ADD_FS_SPD;
+            break;
+        case STEP_MODE:
+            length  = LEN_STEP_MODE;
+            address = ADD_STEP_MODE;
+            break;
+        case ARARM_FN:
+            length  = LEN_ARARM_FN;
+            address = ADD_ARARM_FN;
+            break;
+        case CONFIG:
+            length  = LEN_CONFIG;
+            address = ADD_CONFIG;
+            break;
+        case STATUS:
+            length  = LEN_STATUS;
+            address = ADD_STATUS;
+            break;
+        default:
+            length  = 0;
+            address = 0;
+            break;
+
+    }
+
+    int n = length/8;
+    int m = length%8;
+    if(m==0) {
+        unsigned char temp[n+1];
+        temp[n] = 0x00|(unsigned char)(address);
+        while(0 < n--) {
+            temp[n]=(unsigned char) (value >> 8*n)&0xFF;
+        }
+        send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
+    } else {
+        unsigned char temp[n+2];
+        temp[n+1] = 0x00|(unsigned char)(address);
+        temp[n] =(unsigned char) (value >> 8*n)&~(0xff<<m);
+        while(0 < n--) {
+            temp[n]=(unsigned char) (value >> 8*n)&0xFF;
+        }
+        send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
+    }
+}
+
+int L6470DC::GetParam(unsigned char nMOT,L6470_Register param)
+{
+    int value = 0;
+    int length,address;
+    switch(param) {
+        case ABS_POS:
+            length  = LEN_ABS_POS;
+            address = ADD_ABS_POS;
+            break;
+        case EL_POS:
+            length  = LEN_EL_POS;
+            address = ADD_EL_POS;
+            break;
+        case MARK:
+            length  = LEN_MARK;
+            address = ADD_MARK;
+            break;
+        case SPEED:
+            length  = LEN_SPEED;
+            address = ADD_SPEED;
+            break;
+        case ACC:
+            length  = LEN_ACC;
+            address = ADD_ACC;
+            break;
+        case DEC:
+            length  = LEN_DEC;
+            address = ADD_DEC;
+            break;
+        case MAX_SPEED:
+            length  = LEN_MAX_SPEED;
+            address = ADD_MAX_SPEED;
+            break;
+        case MIN_SPEED:
+            length  = LEN_MIN_SPEED;
+            address = ADD_MIN_SPEED;
+            break;
+        case KVAL_HOLD:
+            length  = LEN_KVAL_HOLD;
+            address = ADD_KVAL_HOLD;
+            break;
+        case KVAL_RUN:
+            length  = LEN_KVAL_RUN;
+            address = ADD_KVAL_RUN;
+            break;
+        case KVAL_ACC:
+            length  = LEN_KVAL_ACC;
+            address = ADD_KVAL_ACC;
+            break;
+        case KVAL_DEC:
+            length  = LEN_KVAL_DEC;
+            address = ADD_KVAL_DEC;
+            break;
+        case INT_SPD:
+            length  = LEN_INT_SPD;
+            address = ADD_INT_SPD;
+            break;
+        case ST_SLP:
+            length  = LEN_ST_SLP;
+            address = ADD_ST_SLP;
+            break;
+        case FN_SLP_ACC:
+            length  = LEN_FN_SLP_ACC;
+            address = ADD_FN_SLP_ACC;
+            break;
+        case FN_SLP_DEC:
+            length  = LEN_FN_SLP_DEC;
+            address = ADD_FN_SLP_DEC;
+            break;
+        case K_THERA:
+            length  = LEN_K_THERA;
+            address = ADD_K_THERA;
+            break;
+        case ADC_OUT:
+            length  = LEN_ADC_OUT;
+            address = ADD_ADC_OUT;
+            break;
+        case OCR_TH:
+            length  = LEN_OCR_TH;
+            address = ADD_OCR_TH;
+            break;
+        case STALL_TH:
+            length  = LEN_STALL_TH;
+            address = ADD_STALL_TH;
+            break;
+        case FS_SPD:
+            length  = LEN_FS_SPD;
+            address = ADD_FS_SPD;
+            break;
+        case STEP_MODE:
+            length  = LEN_STEP_MODE;
+            address = ADD_STEP_MODE;
+            break;
+        case ARARM_FN:
+            length  = LEN_ARARM_FN;
+            address = ADD_ARARM_FN;
+            break;
+        case CONFIG:
+            length  = LEN_CONFIG;
+            address = ADD_CONFIG;
+            break;
+        case STATUS:
+            length  = LEN_STATUS;
+            address = ADD_STATUS;
+            break;
+        default:
+            length  = 0;
+            address = 0;
+            break;
+
+    }
+
+    int n = length/8;
+    int m = length%8;
+    if(m==0) {
+        unsigned char temp[n+1];
+        for(int i = 0; i < n+1; i++) {
+            temp[i]=0;
+        }
+        temp[n] = 0x20|(unsigned char)(address);
+        send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
+        while(0 < n--) {
+            value |= (int)temp[n] << 8*n;
+        }
+    } else {
+        n++;
+        unsigned char temp[n+1];
+        for(int i = 0; i < n+2; i++) {
+            temp[i]=0;
+        }
+        temp[n] = 0x20|(unsigned char)(address);
+        send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
+        while(0 < n--) {
+            value |= (int)temp[n] << 8*n;
+        }
+    }
+
+    return(value);
+}
+
+
+
+void L6470DC::Run(unsigned char nMOT,unsigned char dir,int spd)
+{
+    unsigned char temp[4];
+    //spd = L6470_Step_s_2_Speed((float) spd);
+    temp[3] = 0x50|dir;
+    temp[2] = (unsigned char) (spd >> 16)&0x0F;
+    temp[1] = (unsigned char) (spd >>  8)&0xFF;
+    temp[0] = (unsigned char) (spd >>  0)&0xFF;
+    send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
+}
+
+void L6470DC::StepClock(unsigned char nMOT,unsigned char dir)
+{
+    send(nMOT,0x58|dir);
+}
+
+void L6470DC::Move(unsigned char nMOT,unsigned char dir,int n_step)
+{
+    unsigned char temp[4];
+    temp[3] = 0x40|dir;
+    temp[2] = (unsigned char) (n_step >> 16)&0x3F;
+    temp[1] = (unsigned char) (n_step >>  8)&0xFF;
+    temp[0] = (unsigned char) (n_step >>  0)&0xFF;
+    send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
+}
+
+void L6470DC::GoTo(unsigned char nMOT,int abs_pos)
+{
+    unsigned char temp[4];
+    temp[3] = 0x60;
+    temp[2] = (unsigned char) (abs_pos >> 16)&0x3F;
+    temp[1] = (unsigned char) (abs_pos >>  8)&0xFF;
+    temp[0] = (unsigned char) (abs_pos >>  0)&0xFF;
+    send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
+}
+
+void L6470DC::GoTo_DIR(unsigned char nMOT,unsigned char dir,int abs_pos)
+{
+    unsigned char temp[4];
+    temp[3] = 0x68|dir;
+    temp[2] = (unsigned char) (abs_pos >> 16)&0x3F;
+    temp[1] = (unsigned char) (abs_pos >>  8)&0xFF;
+    temp[0] = (unsigned char) (abs_pos >>  0)&0xFF;
+    send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
+}
+
+void L6470DC::GoUntil(unsigned char nMOT,unsigned char act,unsigned char dir,int spd)
+{
+    unsigned char temp[4];
+    temp[3] = 0x82|(act << 3)|dir;
+    temp[2] = (unsigned char) (spd >> 16)&0x0F;
+    temp[1] = (unsigned char) (spd >>  8)&0xFF;
+    temp[0] = (unsigned char) (spd >>  0)&0xFF;
+    send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
+}
+
+void L6470DC::ReleaseSW(unsigned char nMOT,unsigned char act,unsigned char dir)
+{
+    send(nMOT,0x92|(act << 3)|dir);
+}
+
+void L6470DC::GoHome(unsigned char nMOT)
+{
+    send(nMOT,0x70);
+}
+
+void L6470DC::GoMark(unsigned char nMOT)
+{
+    send(nMOT,0x78);
+}
+
+void L6470DC::ResetPos(unsigned char nMOT)
+{
+    send(nMOT,0xD8);
+}
+
+void L6470DC::ResetDevice(unsigned char nMOT)
+{
+    send(nMOT,0xC0);
+}
+
+void L6470DC::SoftStop(unsigned char nMOT)
+{
+    send(nMOT,0xB0);
+}
+
+void L6470DC::HardStop(unsigned char nMOT)
+{
+    send(nMOT,0xB8);
+}
+
+
+void L6470DC::SoftHiZ(unsigned char nMOT)
+{
+    send(nMOT,0xA0);
+}
+
+void L6470DC::HardHiZ(unsigned char nMOT)
+{
+    send(nMOT,0xA8);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//                             ADVANCED_FUNC                                  //
+////////////////////////////////////////////////////////////////////////////////
+
+void L6470DC::Config(unsigned char nMOT, L6470_MotorConf *conf)
+{
+    L6470_MotorConf *MotorParameterData = (L6470_MotorConf *) conf;
+    StepperMotorRegister MotorRegister;
+    
+    ResetDevice(nMOT);
+
+    MotorRegister.ACC = L6470_Step_s2_2_Acc(MotorParameterData->acc);
+    MotorRegister.DEC = L6470_Step_s2_2_Dec(MotorParameterData->dec);
+    MotorRegister.MAX_SPEED = L6470_Step_s_2_MaxSpeed(MotorParameterData->maxspeed);
+    MotorRegister.MIN_SPEED = L6470_Step_s_2_MinSpeed(MotorParameterData->minspeed);
+    MotorRegister.FS_SPD = L6470_Step_s_2_FsSpd(MotorParameterData->fsspd);
+    MotorRegister.KVAL_HOLD = (uint8_t)((float)((float)(MotorParameterData->kvalhold * 256) / (MotorParameterData->motorvoltage)));
+    MotorRegister.KVAL_RUN = (uint8_t)((float)((float)(MotorParameterData->kvalrun * 256) / (MotorParameterData->motorvoltage)));
+    MotorRegister.KVAL_ACC = (uint8_t)((float)((float)(MotorParameterData->kvalacc * 256) / (MotorParameterData->motorvoltage)));
+    MotorRegister.KVAL_DEC = (uint8_t)((float)((float)(MotorParameterData->kvaldec * 256) / (MotorParameterData->motorvoltage)));
+    MotorRegister.INT_SPEED = L6470_Step_s_2_IntSpeed(MotorParameterData->intspeed);
+    MotorRegister.ST_SLP = L6470_s_Step_2_StSlp(MotorParameterData->stslp);
+    MotorRegister.FN_SLP_ACC = L6470_s_Step_2_FnSlpAcc(MotorParameterData->fnslpacc);
+    MotorRegister.FN_SLP_DEC = L6470_s_Step_2_FnSlpDec(MotorParameterData->fnslpdec);
+    MotorRegister.K_THERM = MotorParameterData->kterm;
+    MotorRegister.OCD_TH = L6470_mA_2_OcdTh(MotorParameterData->ocdth);
+    MotorRegister.STALL_TH = L6470_mA_2_StallTh(MotorParameterData->stallth);
+    MotorRegister.ALARM_EN = MotorParameterData->alarmen;
+    MotorRegister.CONFIG = MotorParameterData->config;
+    MotorRegister.STEP_MODE = MotorParameterData->step_sel;
+
+    /* Write the L6470 registers with the prepared data */
+    SetParam(nMOT, ACC, MotorRegister.ACC);
+    SetParam(nMOT, DEC, MotorRegister.DEC);
+    SetParam(nMOT, MAX_SPEED, MotorRegister.MAX_SPEED);
+    SetParam(nMOT, MIN_SPEED, MotorRegister.MIN_SPEED);
+    SetParam(nMOT, FS_SPD, MotorRegister.FS_SPD);
+    SetParam(nMOT, KVAL_HOLD, MotorRegister.KVAL_HOLD);
+    SetParam(nMOT, KVAL_RUN, MotorRegister.KVAL_RUN);
+    SetParam(nMOT, KVAL_ACC, MotorRegister.KVAL_ACC);
+    SetParam(nMOT, KVAL_DEC, MotorRegister.KVAL_DEC);
+    SetParam(nMOT, INT_SPD, MotorRegister.INT_SPEED);
+    SetParam(nMOT, ST_SLP, MotorRegister.ST_SLP);
+    SetParam(nMOT, FN_SLP_ACC, MotorRegister.FN_SLP_ACC);
+    SetParam(nMOT, FN_SLP_DEC, MotorRegister.FN_SLP_DEC);
+    SetParam(nMOT, K_THERA, MotorRegister.K_THERM);
+    SetParam(nMOT, OCR_TH, MotorRegister.OCD_TH);
+    SetParam(nMOT, STALL_TH, MotorRegister.STALL_TH);
+    SetParam(nMOT, STEP_MODE, MotorRegister.STEP_MODE);
+    SetParam(nMOT, ARARM_FN, MotorRegister.ALARM_EN);
+    SetParam(nMOT, CONFIG, MotorRegister.CONFIG);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
+int L6470DC::GetPosition(int nMOT)
+{
+    return L6470_AbsPos_2_Position(GetParam(nMOT, ABS_POS));
+}
+
+int L6470DC::GetMark(int nMOT)
+{
+    return L6470_AbsPos_2_Position(GetParam(nMOT, MARK));
+}
+
+int L6470DC::GetSpeed(int nMOT)
+{
+    return GetParam(nMOT, SPEED);
+}
+
+void L6470DC::SetHome(int nMOT)
+{
+    ResetPos(nMOT);
+}
+
+void L6470DC::SetMaxSpeed(int nMOT, int speed)
+{
+    SetParam(nMOT, MAX_SPEED, speed);
+}
+
+void L6470DC::SetMark(int nMOT, int mark)
+{
+    SetParam(nMOT, MARK, L6470_Position_2_AbsPos(mark));
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                              CONVERSIONS                                   //
+////////////////////////////////////////////////////////////////////////////////
+
+int32_t L6470DC::L6470_AbsPos_2_Position(uint32_t AbsPos)
+{
+    
+    if (AbsPos > L6470_MAX_POSITION)
+        return (AbsPos - (L6470_POSITION_RANGE + 1));
+    else
+    
+    return AbsPos;
+}
+
+uint32_t L6470DC::L6470_Position_2_AbsPos(int32_t Position)
+{
+  if ((Position >= 0) && (Position <= L6470_MAX_POSITION))
+    return Position;
+  else
+  {
+    if ((Position >= L6470_MIN_POSITION) && (Position < 0))
+      return (Position + (L6470_POSITION_RANGE + 1));
+    else
+      return (L6470_POSITION_RANGE + 1);        // OVF
+  }
+}
+
+float L6470DC::L6470_Speed_2_Step_s(uint32_t Speed)
+{
+    return (Speed * ((float)14.9012e-3));
+}
+
+uint32_t L6470DC::L6470_Step_s_2_Speed(float Step_s)
+{
+    if (Step_s <= (L6470_MAX_SPEED * ((float)14.9012e-3)))
+        return (uint32_t)(Step_s / ((float)14.9012e-3));
+    else
+        return 0;
+}
+
+float L6470DC::L6470_Acc_2_Step_s2(uint16_t Acc)
+{
+    if (Acc <= L6470_MAX_ACC)
+        return (Acc * ((float)1.4552e1));
+    else
+        return 0;
+}
+
+uint16_t L6470DC::L6470_Step_s2_2_Acc(float Step_s2)
+{
+    if (Step_s2 <= (L6470_MAX_ACC * ((float)1.4552e1)))
+        return (uint16_t)(Step_s2 / ((float)1.4552e1));
+    else
+        return 0;
+}
+
+float L6470DC::L6470_Dec_2_Step_s2(uint16_t Dec)
+{
+    if (Dec <= L6470_MAX_DEC)
+        return (Dec * ((float)1.4552e1));
+    else
+        return 0;
+}
+
+uint16_t L6470DC::L6470_Step_s2_2_Dec(float Step_s2)
+{
+    if (Step_s2 <= (L6470_MAX_DEC * ((float)1.4552e1)))
+        return (uint16_t)(Step_s2 / ((float)1.4552e1));
+    else
+        return 0;
+}
+
+float L6470DC::L6470_MaxSpeed_2_Step_s(uint16_t MaxSpeed)
+{
+    if (MaxSpeed <= L6470_MAX_MAX_SPEED)
+        return (MaxSpeed * ((float)15.2588));
+    else
+        return 0;
+}
+
+uint16_t L6470DC::L6470_Step_s_2_MaxSpeed(float Step_s)
+{
+    if (Step_s <= (L6470_MAX_MAX_SPEED * ((float)15.2588)))
+        return (uint16_t)(Step_s / ((float)15.2588));
+    else
+        return 0;
+}
+
+float L6470DC::L6470_MinSpeed_2_Step_s(uint16_t MinSpeed)
+{
+    if (MinSpeed <= L6470_MAX_MIN_SPEED)
+        return (MinSpeed * ((float)238.4186e-3));
+    else
+        return 0;
+}
+
+uint16_t L6470DC::L6470_Step_s_2_MinSpeed(float Step_s)
+{
+    if (Step_s <= (L6470_MAX_MIN_SPEED * ((float)238.4186e-3)))
+        return (uint16_t)(Step_s / ((float)238.4186e-3));
+    else
+        return 0;
+}
+
+float L6470DC::L6470_FsSpd_2_Step_s(uint16_t FsSpd)
+{
+    if (FsSpd <= L6470_MAX_FS_SPD)
+        return ((FsSpd+0.5) * ((float)15.25));
+    else
+        return 0;
+}
+
+uint16_t L6470DC::L6470_Step_s_2_FsSpd(float Step_s)
+{
+    if (Step_s <= ((L6470_MAX_FS_SPD+0.5) * ((float)15.25)))
+        return (uint16_t)((float)(Step_s / ((float)15.25)) - (float)0.5);
+    else
+        return 0;
+}
+
+float L6470DC::L6470_IntSpeed_2_Step_s(uint16_t IntSpeed)
+{
+    if (IntSpeed <= L6470_MAX_INT_SPEED)
+        return (IntSpeed * ((float)59.6046e-3));
+    else
+        return 0;
+}
+
+uint16_t L6470DC::L6470_Step_s_2_IntSpeed(float Step_s)
+{
+    if (Step_s <= (L6470_MAX_INT_SPEED * ((float)59.6046e-3)))
+        return (uint16_t)(Step_s / ((float)59.6046e-3));
+    else
+        return 0;
+}
+
+float L6470DC::L6470_StSlp_2_s_Step(uint8_t StSlp)
+{
+    return (StSlp * ((float)1.5686e-5));
+}
+
+uint8_t L6470DC::L6470_s_Step_2_StSlp(float s_Step)
+{
+    if (s_Step <= (L6470_MAX_ST_SLP * ((float)1.5686e-5)))
+        return (uint8_t)(s_Step / ((float)1.5686e-5));
+    else
+        return 0;
+}
+
+float L6470DC::L6470_FnSlpAcc_2_s_Step(uint8_t FnSlpAcc)
+{
+    return (FnSlpAcc * ((float)1.5686e-5));
+}
+
+uint8_t L6470DC::L6470_s_Step_2_FnSlpAcc(float s_Step)
+{
+    if (s_Step <= (L6470_MAX_FN_SLP_ACC * ((float)1.5686e-5)))
+        return (uint8_t)(s_Step / ((float)1.5686e-5));
+    else
+        return 0;
+}
+
+float L6470DC::L6470_FnSlpDec_2_s_Step(uint8_t FnSlpDec)
+{
+    return (FnSlpDec * ((float)1.5686e-5));
+}
+
+
+uint8_t L6470DC::L6470_s_Step_2_FnSlpDec(float s_Step)
+{
+    if (s_Step <= (L6470_MAX_FN_SLP_DEC * ((float)1.5686e-5)))
+        return (uint8_t)(s_Step / ((float)1.5686e-5));
+    else
+        return 0;
+}
+
+float L6470DC::L6470_OcdTh_2_mA(uint8_t OcdTh)
+{
+    if (OcdTh <= L6470_MAX_OCD_TH)
+        return ((OcdTh+1) * ((float)375));
+    else
+        return 0;
+}
+
+uint8_t L6470DC::L6470_mA_2_OcdTh(float mA)
+{
+    float result, decimal;
+
+    if (mA <= ((L6470_MAX_OCD_TH+1) * ((float)375))) {
+        result = (mA / ((float)375));
+        decimal = result - (uint8_t)result;
+
+        if (decimal < (float)0.5)
+            return ((uint8_t)result - 1);
+        else
+            return ((uint8_t)result);
+    } else
+        return 0;
+}
+
+float L6470DC::L6470_StallTh_2_mA(uint8_t StallTh)
+{
+    if (StallTh <= L6470_MAX_STALL_TH)
+        return ((StallTh+1) * ((float)31.25));
+    else
+        return 0;   // Warning
+}
+
+uint8_t L6470DC::L6470_mA_2_StallTh(float mA)
+{
+    float result, decimal;
+
+    if (mA <= ((L6470_MAX_STALL_TH+1) * ((float)31.25))) {
+        result = (mA / ((float)31.25));
+        decimal = result - (uint8_t)result;
+
+        if (decimal < (float)0.5)
+            return ((uint8_t)result - 1);
+        else
+            return ((uint8_t)result);
+    } else
+        return 0;
+}
+