File content as of revision 1:4676e8b9b357:

#include "mbed.h"
#include "ftlibclasscom.h"
vector<ftcommdev*> ftcommdev::devs;

ftcommdev::ftcommdev(Serial *s, unsigned t, unsigned c): ftdev(t, 0) {
    device = s;
    port = 0;
    FT_TRANSFER_AREA *area = new FT_TRANSFER_AREA;
    memset(area, 0, sizeof(struct _FT_TRANSFER_AREA));
    area->TransferAktiv = 0;
    //query_time = INTERFACE_QUERY_TIME_SERIAL;
    area->RfModulNr = -1;
    if (t == FT_INTELLIGENT_IF)
        area->BusModules = 1;
    else if (t == FT_INTELLIGENT_IF_SLAVE)
        area->BusModules = 2;
    ta = area;
}

//blocking
int ftcommdev::read(Serial *stream, unsigned char *buf, int n, int timeout_ms) {
    Timer t;
    t.start();
    int i = 0;
    while (t.read_ms() < timeout_ms && i < n) {
        if (stream->readable())
            buf[i++] = stream->getc();
    }
    return i;
}

int ftcommdev::write(Serial *d, unsigned char *ptr, int n, int timeout_ms) {
    Timer t;
    t.start();
    int i = 0;
    while (t.read_ms() < timeout_ms && i < n) {
        if (d->writeable())
            d->putc(ptr[i++]);
    }
    return i;
}

//non-blocking
int ftcommdev::write() {
    windex = 0;
    writeByte(); //write the first byte, callback will take care of the rest
    return num_write;
}
void ftcommdev::writeByte() {
    if (windex < num_write) {
        device->putc(out[windex++]);
        if (windex == num_write)
            rindex = 0;
    }
}
void ftcommdev::readByte() {
    if (rindex < num_read) {
        in[rindex++] = device->getc();
        if (rindex == num_read) {
            FtThreadEnd();
        }
    }
}

ftcommdev* ftcommdev::OpenFtCommDevice(unsigned sDevice, unsigned dwTyp, unsigned dwZyklus, unsigned *pdwError) {//makes a (blocking) comm request to the interface
    Serial *dev = 0;
    switch (sDevice) {
        case 1:
            dev = new Serial(p9, p10);
            break;
        case 2:
            dev = new Serial(p13, p14);
            break;
        case 3:
            dev = new Serial(p28, p27);
            break;
//      default:  dev = new Serial(p9, p10); break;
        default:
//            dev = &viaUsb;
            break;
    }
    ftcommdev* com = OpenFtCommDevice(dev, dwTyp, dwZyklus, pdwError);
    if (com) {
        com->port = sDevice;
        printf("opening of COM%d OK\n", sDevice);
    }
    return com;
}

ftcommdev* ftcommdev::OpenFtCommDevice(Serial *dev, unsigned dwTyp, unsigned dwZyklus, unsigned *pdwError) {//makes a (blocking) comm request to the interface
    unsigned char in[5];
    unsigned char on[] = " ft-Robo-ON-V1";
    on[0] = 0xA1;
    ftcommdev* ret = 0;
    if (dwTyp != FT_ROBO_IF_COM) {
        ret = new ftcommdevii(dev, dwTyp, dwZyklus);
        dev->baud(BAUDRATE_II);
    } else {// dwTyp == FT_ROBO_IF_COM
        ret = new ftcommdev(dev, dwTyp, dwZyklus);
        dev->baud(BAUDRATE_RI);
    }
    printf("about to send '%s'\n", on);
    if (dwTyp == FT_ROBO_IF_COM) {
        int sent = write(dev, on, strlen((const char*)on), 100/*ms*/);
//        int sent = dev->printf("%s", on);
        printf("sent %d bytes to COM\n", sent);
        if (sent == strlen((const char*)on)) {
            if (read(dev, in, 5, 1000/*ms*/) == 5) {              //   if (dev->scanf("%5c", in) == 1) { does not work, could have to do with NUL chars or needs a lookahead char
                printf("%02X: interface version: %d.%d.%d.%d\n", in[0], in[4], in[3], in[2], in[1]);
                ret->fw = *(unsigned*)(in+1);
                if ((in[0] ^ on[0]) ==  0x0FFU) {
                    printf("opening of %s OK\n", "UNKNOWN");
                } else {
                    printf("return code is %02X but should be %02X\n", in[0], ~on[0]&0xFF);
                    delete ret;
                    return NULL;
                }
            } else {
                printf("read did not return 5\n");
                delete ret;
                return NULL;
            }
        } else {
            printf("only %d chars were sent i.o %d\n", sent, strlen((const char*)on));
            delete ret;
            return NULL;
        }
    }
    devs.push_back(ret);
    return ret;
}

unsigned ftcommdev::CloseFtDevice() {//sends a comm request
    unsigned char off = 0xA2;
    unsigned char in[1];

    while (ta->TransferAktiv != 0) {
        fprintf(stderr, "Transfer ta still active\n");
        sleep(1);
    }

    if (type == FT_ROBO_IF_COM) {
        if (write(device, &off, 1, 500) != 1 || read(device, in, 1, 1000) != 1 || (in[0]^off != 0xFF)) {
            fprintf(stderr, "CloseFtDevice: Error communicating with serial\n");
        }
    }
    for (vector<ftcommdev*>::iterator it = devs.begin(); it < devs.end(); it++)
        if (*it == this) {
            devs.erase(it);
            delete this;
            return 0;
        }
    return 0;
}

unsigned ftcommdev::GetFtSerialNr() {
    int ret;
    unsigned char buffer[35] = { 0 };
    if (sn > 0)
        return sn;
    switch (type) {
        case FT_INTELLIGENT_IF:
        case FT_INTELLIGENT_IF_SLAVE:
            return 0;
        case FT_ROBO_IF_COM:
            buffer[0] = 0xf0;
            buffer[1] = 0x02;
            ret = write(device, buffer, 2, 500);
            if (ret != 2) {
                fprintf(stderr, "Error writing msg 0xF0 0x02\n");
                return 0;
            }
            ret = read(device, buffer, 5, 1000);
            if (ret != 5) {
                fprintf(stderr, "Error reading msg 0xF0 0x02\n");
                return 0;
            }
            //sn = buffer[1] + buffer[2]*100 + buffer[3]*10000 + buffer[4]*1000000;
            sn = *(unsigned*)(buffer+1);
            break;
        default:
            return FTLIB_ERR_NOT_SUPPORTED;
    }
    return sn;
}

char * ftcommdev::GetFtLongNameStrg() {
    switch (type) {
        case FT_INTELLIGENT_IF:
            return strdup("Intelligent Interface");
        case FT_INTELLIGENT_IF_SLAVE:
            return strdup("Intelligent Interface with Slave");
        case FT_ROBO_IF_IIM:
            return strdup("Robo Interface in Intelligent Interface mode");
        case FT_ROBO_IF_COM:
            return strdup("Robo Interface on Com");
        default:
            return strdup("Unknown interface type");
    }
}

char * ftcommdev::GetFtShortNameStrg() {
    switch (type) {
        case FT_INTELLIGENT_IF:
            return strdup("IIF");
        case FT_INTELLIGENT_IF_SLAVE:
            return strdup("IIF w/Slave");
        case FT_ROBO_IF_IIM:
            return strdup("RI_IIM");
        case FT_ROBO_IF_COM:
            return strdup("RICom");
        default:
            return strdup("Unknown");
    }
}

void ftcommdev::poll() {
    for (int i = 0; i < devs.size(); i++) {
        if (devs[i]->triggered) {
            if (devs[i]->guardedFtThreadBegin())
                devs[i]->triggered = 0;
        }
    }
}

void ftcommdev::FtThreadInit() {//setup buffers for this type of interface
    device->attach(this, &ftcommdev::writeByte, Serial::TxIrq);
    device->attach(this, &ftcommdev::readByte, Serial::RxIrq);
    ftdev::FtThreadInit();
    out[0] = 0xf2;
    num_write = 17;
    num_read = 21;
}

//here the real data exchange starts
void ftcommdev::FtThreadBegin() {//called every 10ms to issue a request, should be non-blocking
    if (!test_and_set()) {//return when transferarea is in use
        busy = false; //release the mutex, otherwise the thread effectively stops
        return;//return because there is no point in sending a nonsense request, alternatively the lock can be ignored in which case the data may be inconsistent
    }
    out[1] = ta->M_Main;
    out[2] = (ta->MPWM_Main[0] & 0x7) | (ta->MPWM_Main[1]<<3 & 0x38) | (ta->MPWM_Main[2]<<6 & 0xC0);
    out[3] = (ta->MPWM_Main[2] & 0x1) | (ta->MPWM_Main[3]<<1 & 0xE) | (ta->MPWM_Main[4]<<4 & 0x70) | (ta->MPWM_Main[5]<<7 & 0x80);
    out[4] = (ta->MPWM_Main[5] & 0x3) | (ta->MPWM_Main[6]<<2 & 0x1C) | (ta->MPWM_Main[7]<<5 & 0xE0);
    out[5] = ta->M_Sub1;
    out[6] = (ta->MPWM_Sub1[0] & 0x7) | (ta->MPWM_Sub1[1]<<3 & 0x38) | (ta->MPWM_Sub1[2]<<6 & 0xC0);
    out[7] = (ta->MPWM_Sub1[2] & 0x1) | (ta->MPWM_Sub1[3]<<1 & 0xE) | (ta->MPWM_Sub1[4]<<4 & 0x70) | (ta->MPWM_Sub1[5]<<7 & 0x80);
    out[8] = (ta->MPWM_Sub1[5] & 0x3) | (ta->MPWM_Sub1[6]<<2 & 0x1C) | (ta->MPWM_Sub1[7]<<5 & 0xE0);
    out[9] = ta->M_Sub2;
    out[10] = (ta->MPWM_Sub2[0] & 0x7) | (ta->MPWM_Sub2[1]<<3 & 0x38) | (ta->MPWM_Sub2[2]<<6 & 0xC0);
    out[11] = (ta->MPWM_Sub2[2] & 0x1) | (ta->MPWM_Sub2[3]<<1 & 0xE) | (ta->MPWM_Sub2[4]<<4 & 0x70) | (ta->MPWM_Sub2[5]<<7 & 0x80);
    out[12] = (ta->MPWM_Sub2[5] & 0x3) | (ta->MPWM_Sub2[6]<<2 & 0x1C) | (ta->MPWM_Sub2[7]<<5 & 0xE0);
    out[13] = ta->M_Sub3;
    out[14] = (ta->MPWM_Sub3[0] & 0x7) | (ta->MPWM_Sub3[1]<<3 & 0x38) | (ta->MPWM_Sub3[2]<<6 & 0xC0);
    out[15] = (ta->MPWM_Sub3[2] & 0x1) | (ta->MPWM_Sub3[3]<<1 & 0xE) | (ta->MPWM_Sub3[4]<<4 & 0x70) | (ta->MPWM_Sub3[5]<<7 & 0x80);
    out[16] = (ta->MPWM_Sub3[5] & 0x3) | (ta->MPWM_Sub3[6]<<2 & 0x1C) | (ta->MPWM_Sub3[7]<<5 & 0xE0);
    out[17]    = 0;
    out[18]    = 0;
    out[19]    = 0;
    out[20]    = 0;
    out[21]    = 0;
    out[22]    = 0;
    out[23]    = 0;
    out[24]    = 0;
    out[25]    = 0;
    out[26]    = 0;
    out[27]    = 0;
    out[28]    = 0;
    out[29]    = 0;
    out[30]    = 0;
    out[31]    = 0;

    increment();//release the lock on shared memeory
    write();
}
#if 0
void ftcommdev::FtThreadEnd() {//called by the receiver/dma callback when the reply is complete
    if (!test_and_set()) {//skip when busy
        busy = false;
        return;
    }
    ta->ChangeEg = ta->E_Main != in[0] || ta->E_Sub1 != in[1] || ta->E_Sub2 != in[2] || ta->E_Sub3 != in[3];
    ta->E_Main = in[0];
    ta->E_Sub1 = in[1];
    ta->E_Sub2 = in[2];
    ta->E_Sub3 = in[3];
    ta->ChangeAn = 1; //assume that analog always changes (noise)
    ta->AX = in[4];
    ta->AY = in[5];
    ta->A1 = in[6];
    ta->A2 = in[7];
    ta->AX |= (in[8] & 0x3) << 8;
    ta->AY |= (in[8] & 0xC) << 6;
    ta->A1 |= (in[8] & 0x30) << 4;
    ta->A2 |= (in[8] & 0xC0) << 2;
    ta->AZ = in[9];
    ta->D1 = in[10];
    ta->D2 = in[11];
    ta->AV = in[12];
    ta->AZ |= (in[13] & 0x3) << 8;
    ta->D1 |= (in[13] & 0xC) << 6;
    ta->D2 |= (in[13] & 0x30) << 4;
    ta->AV |= (in[13] & 0xC0) << 2;
    if (ta->IRKeys != in[14])
        ta->ChangeIr = 1;
    ta->IRKeys = in[14];
    ta->BusModules = in[15];
    // 16
    ta->AXS1 = in[17];
    ta->AXS2 = in[18];
    ta->AXS3 = in[19];
    ta->AXS1 |= (in[20] & 0x3) << 8;
    ta->AXS2 |= (in[20] & 0xC) << 6;
    ta->AXS3 |= (in[20] & 0x30) << 4;
    // 21
    ta->AVS1 = in[22];
    ta->AVS2 = in[23];
    ta->AVS3 = in[24];
    ta->AVS1 |= (in[25] & 0x3) << 8;
    ta->AVS2 |= (in[25] & 0xC) << 6;
    ta->AVS3 |= (in[25] & 0x30) << 4;
    // 26...42
    increment();
    interface_connected = 1;
    if (ne.NotificationCallback) {
        (*ne.NotificationCallback)(ne.Context);
    }
    busy = false;
}
#endif

void ftcommdev::FtThreadFinish() {//called by StopFtTransferArea
    device->attach(0,Serial::RxIrq);
    device->attach(0,Serial::TxIrq);
    ftdev::FtThreadFinish();
}

void ftcommdevii::FtThreadBegin() {//called every 10ms to issue a request, should be non-blocking
    if (!test_and_set()) {//return when transferarea is in use
        busy = false; //release the mutex, otherwise the thread effectively stops
        return;//return because there is no point in sending a nonsense request, alternatively the lock can be ignored in which case the data may be inconsistent
    }
    out[1] = ta->M_Main;
    out[2] = ta->M_Sub1;

    // For the II we need to simulate different speeds here
    for (int iCurMotor = 0; iCurMotor < 7; iCurMotor++) {
        if (ta->MPWM_Main[iCurMotor] < ii_speed) out[1] &= ~(1 << iCurMotor);
        if (ta->MPWM_Sub1[iCurMotor] < ii_speed) out[2] &= ~(1 << iCurMotor);
    }
    ii_speed++;
    if (ii_speed > 7) ii_speed = 0;

    num_write=2;
    switch (cycle) {
        case 0:
            num_read=3;
            out[0] = 0xc5;
            break;
        case 1:
            num_read=3;
            out[0] = 0xc9;
            break;
        default:
            num_read=1;
            out[0] = 0xc1;
            break;
    }
    if (type == FT_INTELLIGENT_IF_SLAVE) {
        num_write++;
        num_read++;
        out[0]++;
    }
    if (cycle++ > analogcycle) cycle=0;
    increment();//release the lock on shared memeory
    write();
}

void ftcommdevii::FtThreadEnd() {//called by the receiver/dma callback when the reply is complete
    if (!test_and_set()) {//skip when busy
        busy = false;
        return;
    }
    ta->ChangeEg = ta->E_Main != in[0];
    ta->E_Main = in[0];
    ta->BusModules = type==FT_INTELLIGENT_IF_SLAVE;
    switch (out[0]) {
        case 0xc1:
            break;
        case 0xc5:
            ta->AX = in[1] | (8<<in[2]);
            break;
        case 0xc9:
            ta->AY = in[1] | (8<<in[2]);
            break;
        case 0xc2:
            ta->ChangeEg = ta->ChangeEg || ta->E_Sub1 != in[1];
            ta->E_Sub1 = in[1];
            break;
        case 0xc6:
            ta->ChangeEg = ta->ChangeEg || ta->E_Sub1 != in[1];
            ta->E_Sub1 = in[1];
            ta->AX = in[2] | (8<<in[3]);
            break;
        case 0xca:
            ta->ChangeEg = ta->ChangeEg || ta->E_Sub1 != in[1];
            ta->E_Sub1 = in[1];
            ta->AY = in[2] | (8<<in[3]);
            break;
    }
    increment();
    interface_connected = 1;
    if (ne.NotificationCallback) {
        (*ne.NotificationCallback)(ne.Context);
    }
    busy = false;
}

unsigned ftcommdev::SetFtDistanceSensorMode(unsigned dwMode, unsigned dwTol1, unsigned dwTol2, unsigned dwLevel1, unsigned dwLevel2, unsigned dwRepeat1, unsigned dwRepeat2) {
    unsigned char buffer[11];
 
    if (type != FT_ROBO_IF_COM)
        return FTLIB_ERR_NOT_SUPPORTED;

    buffer[0] = 0xf1;
    buffer[1] = 0x01;
    buffer[2] = dwMode;
    buffer[3] = dwTol1;
    buffer[4] = dwTol2;
    buffer[5] = dwLevel1;
    buffer[6] = dwLevel1>>8;
    buffer[7] = dwLevel2;
    buffer[8] = dwLevel2>>8;
    buffer[9] = dwRepeat1;
    buffer[10] = dwRepeat2;

    if ((write(device, buffer, 11, 500)) != 11 || (read(device, buffer, 1, 500)) != 1 || buffer[0] != 0x01) {
        fprintf(stderr, "SetFtDistanceSensorMode: Error communicating with serial\n");
        return buffer[0];
    }
    usleep(100000); // wait before continue, else it doesn't always work
    return FTLIB_ERR_SUCCESS;
}

unsigned ftcommdev::pgm_message(unsigned code, unsigned dwMemBlock) {
    unsigned char buffer[2];
    if (type != FT_ROBO_IF_COM) return FTLIB_ERR_NOT_SUPPORTED;
    buffer[0] = code;
    buffer[1] = dwMemBlock;
    if ((write(device, buffer, 2, 500)) != 2 || (read(device, buffer, 1, 500)) != 1) {
        return FTLIB_ERR_IF_NO_PROGRAM;
    }
    if ((buffer[0]) == 0x1) return FTLIB_ERR_SUCCESS;
    else return FTLIB_ERR_IF_NO_PROGRAM;
}