Diff: ftlib/ftlibclasscom.cpp

Revision:

0:7da612835693

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ftlib/ftlibclasscom.cpp	Wed Jun 15 19:12:25 2011 +0000
@@ -0,0 +1,456 @@
+#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;
+}
+