MtEncoderSPI.cpp

Go to the documentation of this file.

/*
 * MtEncoderSPI.cpp
 *
 *  Created on: 02.11.2021
 *      Author: Yannick
 */
 
#include "MtEncoderSPI.h"
#include "constants.h"
#include "CRC.h"
#ifdef MTENCODERSPI
bool MtEncoderSPI::inUse = false;
 
ClassIdentifier MtEncoderSPI::info = {
         .name = "MagnTek SPI" ,
         .id=CLSID_ENCODER_MTSPI,
 };
const ClassIdentifier MtEncoderSPI::getInfo(){
    return info;
}
 
std::array<uint8_t,256> MtEncoderSPI::tableCRC __attribute__((section (".ccmram")));
 
MtEncoderSPI::MtEncoderSPI() : SPIDevice(ENCODER_SPI_PORT,ENCODER_SPI_PORT.getFreeCsPins()[0]), CommandHandler("mtenc",CLSID_ENCODER_MTSPI,0),cpp_freertos::Thread("MTENC",256,42) {
    MtEncoderSPI::inUse = true;
    this->spiConfig.peripheral.BaudRatePrescaler = spiPort.getClosestPrescaler(10e6).first; // 10MHz max
    this->spiConfig.peripheral.FirstBit = SPI_FIRSTBIT_MSB;
    this->spiConfig.peripheral.CLKPhase = SPI_PHASE_2EDGE;
    this->spiConfig.peripheral.CLKPolarity = SPI_POLARITY_HIGH;
    this->spiConfig.cspol = true;
 
    //Init CRC-8 table
    makeCrcTable(tableCRC,POLY,8); // Mt6825, Poly X8+X2+X (+1)
 
    restoreFlash(); // Also configures SPI port
    spiPort.reserveCsPin(this->spiConfig.cs);
 
    CommandHandler::registerCommands();
    registerCommand("cs", MtEncoderSPI_commands::cspin, "CS pin",CMDFLAG_GET | CMDFLAG_SET);
    registerCommand("pos", MtEncoderSPI_commands::pos, "Position",CMDFLAG_GET | CMDFLAG_SET);
    registerCommand("errors", MtEncoderSPI_commands::errors, "Parity error count",CMDFLAG_GET);
    registerCommand("mode", MtEncoderSPI_commands::mode, "Encoder mode (MT6825=0;MT6835=1)",CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING);
    registerCommand("speed", MtEncoderSPI_commands::speed, "SPI speed preset",CMDFLAG_GET | CMDFLAG_SET | CMDFLAG_INFOSTRING);
    registerCommand("reg", MtEncoderSPI_commands::reg, "Read/Write register",CMDFLAG_GETADR | CMDFLAG_SETADR | CMDFLAG_DEBUG);
    registerCommand("save", MtEncoderSPI_commands::save, "Save to memory",CMDFLAG_GET | CMDFLAG_DEBUG);
    this->Start();
}
 
MtEncoderSPI::~MtEncoderSPI() {
    MtEncoderSPI::inUse = false;
    spiPort.freeCsPin(this->spiConfig.cs);
}
 
void MtEncoderSPI::restoreFlash(){
 
    uint16_t conf_int = Flash_ReadDefault(ADR_MTENC_CONF1, 0);
 
    uint8_t cspin = conf_int & 0x3;
    MtEncoderSPI_mode mode = static_cast<MtEncoderSPI_mode>(conf_int >> 8);
 
    uint8_t offsetShift = 2;
    if(mode == MtEncoderSPI_mode::mt6835){
        offsetShift = 5;
    }
    offset = Flash_ReadDefault(ADR_MTENC_OFS, 0) << offsetShift;
    setMode(mode);
    setCsPin(cspin);
    setSpiSpeed((conf_int >> 2) & 0x3);
}
 
void MtEncoderSPI::saveFlash(){
    uint8_t offsetShift = 2;
    if(mode == MtEncoderSPI_mode::mt6835){
        offsetShift = 5;
    }
    uint16_t conf_int = this->cspin & 0x3;
    conf_int |= (this->spiSpeedPreset & 0x3) << 2;
    conf_int |= ((uint8_t)mode & 0xf) << 8;
    Flash_Write(ADR_MTENC_CONF1, conf_int);
    Flash_Write(ADR_MTENC_OFS, offset >> offsetShift);
}
 
 
void MtEncoderSPI::Run(){
    while(true){
        requestNewDataSem.Take(); // Wait until a position is requested
        //spiPort.receive_DMA(spi_buf, bytes, this); // Receive next frame
        updateAngleStatus();
        this->WaitForNotification();  // Wait until DMA is finished
 
        if(updateAngleStatusCb()){
            int overflowLim = getCpr() >> 1;
            if(curAngleInt-lastAngleInt > overflowLim){ // Underflowed
                rotations--;
            }
            else if(lastAngleInt-curAngleInt > overflowLim){ // Overflowed
                rotations++;
            }
            lastAngleInt = curAngleInt;
 
            curPos = rotations * getCpr() + curAngleInt; // Update position
        }else{
            errors++;
        }
        lastUpdateTick = HAL_GetTick();
        waitForUpdateSem.Give();
        updateInProgress = false;
    }
}
 
void MtEncoderSPI::setCsPin(uint8_t cspin){
    spiPort.freeCsPin(this->spiConfig.cs);
    this->cspin = std::min<uint8_t>(spiPort.getCsPins().size(), cspin);
    this->spiConfig.cs = *spiPort.getCsPin(this->cspin);
    initSPI();
    spiPort.reserveCsPin(this->spiConfig.cs);
}
 
void MtEncoderSPI::initSPI(){
    spiPort.takeSemaphore();
 
    spiPort.configurePort(&this->spiConfig.peripheral);
    spiPort.giveSemaphore();
}

uint8_t MtEncoderSPI::readSpi(uint16_t addr){
    if(mode == MtEncoderSPI_mode::mt6825){
        uint8_t txbuf[2] = {(uint8_t)(addr | 0x80),0};
        uint8_t rxbuf[2] = {0,0};
        spiPort.transmitReceive(txbuf, rxbuf, 2, this,100);
        return rxbuf[1];
    }else if(mode == MtEncoderSPI_mode::mt6835){
        uint8_t txbuf[3] = {(uint8_t)((addr & 0xf00) | 0x30),(uint8_t)(addr & 0xff),0};
        uint8_t rxbuf[3] = {0,0,0};
        spiPort.transmitReceive(txbuf, rxbuf, 3, this,100);
        return rxbuf[2];
    }
}
 
void MtEncoderSPI::writeSpi(uint16_t addr,uint8_t data){
    if(mode == MtEncoderSPI_mode::mt6825){
        uint8_t txbuf[2] = {(uint8_t)(addr & 0xff),data};
        spiPort.transmit(txbuf, 2, this,100);
    }else if(mode == MtEncoderSPI_mode::mt6835){
        uint8_t txbuf[3] = {(uint8_t)((addr & 0xf00) | 0x60),(uint8_t)(addr & 0xff),data};
        spiPort.transmit(txbuf, 3, this,100);
    }
 
}

bool MtEncoderSPI::saveEeprom(){
    if(mode != MtEncoderSPI_mode::mt6835){
        return false;
    }
    if(mode == MtEncoderSPI_mode::mt6835){
        uint8_t txbuf[3] = {0xC0,0x00,0x00};
        uint8_t rxbuf[3] = {0,0,0};
        spiPort.transmitReceive(txbuf, rxbuf, 3, this,100);
        return rxbuf[2] == 0x55;
    }
    return false;
}
 
void MtEncoderSPI::setPos(int32_t pos){
    offset = curPos - pos;
}
 
 
void MtEncoderSPI::spiTxRxCompleted(SPIPort* port){
 
    if(updateInProgress){
        NotifyFromISR();
        //updateAngleStatusCb();
        memcpy(rxbuf,rxbuf_t,sizeof(rxbuf));
    }
}
 

void MtEncoderSPI::updateAngleStatus(){
 
    if(mode == MtEncoderSPI_mode::mt6825){
        uint8_t txbufNew[5] = {0x03 | 0x80,0,0,0,0};
        memcpy(this->txbuf,txbufNew,5);
        spiPort.transmitReceive_DMA(txbuf, rxbuf_t, 4, this);
    }else if(mode == MtEncoderSPI_mode::mt6835){
        uint8_t txbufNew[6] = {0xA0,0x03,0,0,0,0};
        memcpy(this->txbuf,txbufNew,6);
        spiPort.transmitReceive_DMA(txbuf, rxbuf_t, 6, this);
    }
 
 
}
 
bool MtEncoderSPI::updateAngleStatusCb(){
    bool parity_ok = false;
 
    if(mode == MtEncoderSPI_mode::mt6825){
        uint32_t angle17_10 = rxbuf[1];
        uint32_t angle9_4 = rxbuf[2];
        uint32_t angle3_0 = rxbuf[3];
 
        // Parity check byte 2
        uint8_t pc1 = angle17_10 ^ angle17_10 >> 1;
        pc1 = pc1 ^ pc1 >> 2;
        pc1 = pc1 ^ pc1 >> 4;
 
        uint8_t pc1_2 = angle9_4 ^ angle9_4 >> 1;
        pc1_2 = pc1_2 ^ pc1_2 >> 2;
        pc1_2 = pc1_2 ^ pc1_2 >> 4;
 
        // Parity check byte 1
        angle3_0 = angle3_0 >> 2; // shift 2
        uint8_t pc2 = (angle3_0) ^ (angle3_0) >> 1;
        pc2 = pc2 ^ pc2 >> 2;
        pc2 = pc2 ^ pc2 >> 4;
 
        nomag =    (angle9_4 & 0x02) >> 1;
        overspeed = (angle3_0 & 0x04) >> 2;
        angle9_4 =  (angle9_4 & 0xFC) >> 2;
        angle3_0 =  angle3_0 >> 2;//(angle3_0 & 0xF0) >> 4;
 
 
        parity_ok = !(pc2 & 1) && ((pc1 & 1) == (pc1_2 & 1));
 
        curAngleInt = (angle17_10 << 10) | (angle9_4 << 4) | (angle3_0);
 
    }else if(mode == MtEncoderSPI_mode::mt6835){
        uint32_t angle20_13 = rxbuf[2];
        uint32_t angle12_5 = rxbuf[3];
        uint32_t angle4_0 = (rxbuf[4] & 0xF8) >> 3;
 
        uint8_t status = rxbuf[4] & 0x7;
        nomag =    (status & 0x02) >> 1;
        overspeed = (status & 0x01);
        uint8_t crc  = rxbuf[5];
 
        curAngleInt = (angle20_13 << 13) | (angle12_5 << 5) | (angle4_0);
        uint8_t calccrc = calculateCrc8(tableCRC, rxbuf+2, 3, 0);
        parity_ok = calccrc == crc;
    }
 
 
    this->updateInProgress = false;
 
    return parity_ok; // ok if both bytes have even parity
}
 
int32_t MtEncoderSPI::getPos(){
 
    return getPosAbs() - offset;
}
 
int32_t MtEncoderSPI::getPosAbs(){
    if(updateInProgress){ // If a transfer is still in progress return the last result
        return curPos;
    }
    updateInProgress = true;
    requestNewDataSem.Give(); // Start transfer
    if(HAL_GetTick() - lastUpdateTick > waitThresh)
        waitForUpdateSem.Take(waitThresh); // Wait a bit
 
    return curPos;
}
 
uint32_t MtEncoderSPI::getCpr(){
    switch(mode){
    case MtEncoderSPI_mode::mt6825:
        return 262144;
    case MtEncoderSPI_mode::mt6835:
        return 2097152;
    default:
        return 0; // Not possible
    }
}
 
void MtEncoderSPI::setMode(MtEncoderSPI::MtEncoderSPI_mode mode){
    this->mode = mode;
    // Reset variables
    this->curPos = 0;
    this->curAngleInt = 0;
    this->lastAngleInt = 0;
    this->rotations = 0;
    this->errors = 0;
    this->nomag = false;
    this->overspeed = false;
}
 
void MtEncoderSPI::setSpiSpeed(uint8_t preset){
    if(preset == spiSpeedPreset){
        return; // Ignore if no change
    }
    spiSpeedPreset = clip<uint8_t,uint8_t>(preset,0,spispeeds.size());
    this->spiConfig.peripheral.BaudRatePrescaler = spiPort.getClosestPrescaler(spispeeds[spiSpeedPreset]).first;
    initSPI();
}
 
 
CommandStatus MtEncoderSPI::command(const ParsedCommand& cmd,std::vector<CommandReply>& replies){
    switch(static_cast<MtEncoderSPI_commands>(cmd.cmdId)){
    case MtEncoderSPI_commands::cspin:
        if(cmd.type==CMDtype::get){
            replies.emplace_back(this->cspin+1);
        }else if(cmd.type==CMDtype::set){
            this->setCsPin(cmd.val-1);
        }else{
            return CommandStatus::ERR;
        }
        break;
 
    case MtEncoderSPI_commands::pos:
        if(cmd.type==CMDtype::get){
            replies.emplace_back(getPos());
        }else if(cmd.type==CMDtype::set){
            this->setPos(cmd.val);
        }else{
            return CommandStatus::ERR;
        }
        break;
    case MtEncoderSPI_commands::errors:
        replies.emplace_back(errors);
        break;
    case MtEncoderSPI_commands::mode:
        if(cmd.type==CMDtype::get){
            replies.emplace_back((uint8_t)mode);
        }else if(cmd.type==CMDtype::set){
            this->setMode((MtEncoderSPI_mode)cmd.val);
        }else if(cmd.type==CMDtype::info){
            replies.emplace_back("MT6825:0\nMT6835:1");
        }else{
            return CommandStatus::ERR;
        }
        break;
 
    case MtEncoderSPI_commands::speed:
    {
        if(cmd.type == CMDtype::get){
            replies.emplace_back(spiSpeedPreset);
        }else if(cmd.type == CMDtype::set){
            setSpiSpeed(cmd.val);
        }else if(cmd.type == CMDtype::info){
            for(uint8_t i = 0; i<spispeeds.size();i++){
                replies.emplace_back(std::to_string(this->spiPort.getClosestPrescaler(spispeeds[i]).second)  + ":" + std::to_string(i)+"\n");
            }
        }else{
            return CommandStatus::ERR;
        }
        break;
    }
    case MtEncoderSPI_commands::reg:
    {
        if(cmd.type == CMDtype::getat){
            replies.emplace_back(readSpi(cmd.adr));
        }else if(cmd.type == CMDtype::setat){
            writeSpi(cmd.adr, cmd.val);
        }else{
            return CommandStatus::ERR;
        }
        break;
    }
    case MtEncoderSPI_commands::save:
    {
        if(cmd.type == CMDtype::get){
            replies.emplace_back(saveEeprom() ? 1 : 0);
        }
        break;
    }
    default:
        return CommandStatus::NOT_FOUND;
    }
 
    return CommandStatus::OK;
}
 
#endif