File content as of revision 0:295b7e1c12f3:

#include "ScilabSerial.h"

extern UnbufferedSerial dbgPort;

/**
 * @brief
 * @note
 * @param
 * @retval
 */
ScilabSerial::ScilabSerial(PinName tx, PinName rx, int baud /*= 115200*/ ) :
    SerialBase(tx, rx, baud),
    _rxComplete(false),
    _rxBufLen(0),
    _rxDataLen(0),
    _deltaT(0),
    _mpu6050Available(false)
{
    attach(callback(this, &ScilabSerial::onRx));

    // Analog inputs
    for (size_t i = 0; i < sizeof(_analogPinName) / sizeof(*_analogPinName); i++) {
        // indexes -> pin names
        if (_analogPinName[i] != NC)
            _analogIns[i] = new AnalogIn(_analogPinName[i]);
        else
            _analogIns[i] = NULL;
    }

    // PWM outputs
    for (size_t i = 0; i < 4; i++) {
        // indexes 0 to 3 -> pin names D4 to D7
        if (_digitalPinName[i + 4] != NC)
            _pwmOuts[i] = new PwmOut(_digitalPinName[i + 4]);
        else
            _pwmOuts[i] = NULL;
    }

    // Digital read write
    for (size_t i = 0; i < sizeof(_digitalInOuts) / sizeof(*_digitalInOuts); i++)
        _digitalInOuts[i] = NULL;

    // Servo
    for (size_t i = 0; i < sizeof(_servos) / sizeof(*_servos); i++)
        _servos[i] = NULL;

    // MPU6050
    _mpu6050 = NULL;

    // Interrup-in counters
    for (size_t i = 0; i < sizeof(_intrInCounters) / sizeof(*_intrInCounters); i++)
        _intrInCounters[i] = NULL;

    // DC motors
    for (size_t i = 0; i < sizeof(_dcMotors) / sizeof(*_dcMotors); i++)
        _dcMotors[i] = NULL;
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::onRx()
{
    // If the '\n' character was used as 'packet-end' delimiter then we could have:
    //
    //    while (readable()) {
    //        volatile char   c = _base_getc();
    //        if (c != '\n') {
    //            if (rxBufLen >= RX_MAX)
    //                rxBufLen = 0;                   // rxBuf overflow
    //            rxBuf[rxBufLen++] = c;
    //        }
    //        else {
    //            // packet complete
    //            memset(rxData, 0, RX_MAX);          // clear rxData
    //            memcpy(rxData, rxBuf, rxBufLen);    // copy rxBuf to rxData
    //            rxDataLen = rxBufLen;               // set rxData length
    //            memset(rxBuf, 0, RX_MAX);           // clear rxBuf
    //            rxBufLen = 0;
    //        }
    //    }
    // Unfortunately there is neither packet length information in the packet
    // nor 'packet-end' delimiter is used.
    // That's why we have to figure out the packet length on the fly based on it's content:
    volatile char   c = _base_getc();       // Read a byte from UART

    _rxBuf[_rxBufLen++] = c;                // Store the byte in the receive buffer

    switch (_rxBuf[0]) {
        case 'A':                           // Analog read
        case 'G':                           // MPU6050
        case 'R':                           // Revision request
            if (_rxBufLen == 2)
                _rxComplete = true;
            break;

        case 'W':                           // PWM (analog) write
        case 'I':                           // Interrupt-in counter
            if (_rxBufLen == 3)
                _rxComplete = true;
            break;

        case 'D':                           // Digital read/write
            if (_rxBufLen == 3)
                if (_rxBuf[1] == 'r')
                    _rxComplete = true;
            break;

        case 'S':                           // Servo motor
            if (_rxBufLen == 3)
                if (_rxBuf[1] != 'w')
                    _rxComplete = true;
            break;

        case 'E':                           // Encoder
            if (_rxBufLen == 3)
                if (_rxBuf[1] != 'a')
                    _rxComplete = true;
            break;

        case 'C':
            if (_rxBufLen == 5)
                _rxComplete = true;

        case 'M':                           // DC motor
            if (_rxBufLen == 3)
                if (_rxBuf[1] == 'r')
                    _rxComplete = true;
            break;
    }

    if ((_rxBuf[0] != 'C') && (_rxBufLen == 4))
        _rxComplete = true;

    if (_rxComplete) {
        _rxComplete = false;
        memset(_rxData, 0, RX_MAX);         // reset rxData
        memcpy(_rxData, _rxBuf, _rxBufLen); // copy rxBuf to rxData
        _rxDataLen = _rxBufLen;             // set rxData length
        memset(_rxBuf, 0, RX_MAX);          // reset rxBuf
        _rxBufLen = 0;
    }

//    dbgPort.write((void*)(& _rxData), _rxDataLen);
//    dbgPort.write("\r\n", 2);

}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
ssize_t ScilabSerial::write(const void* buffer, size_t length)
{
    const char*     ptr = (const char*)buffer;
    const char*     end = ptr + length;

    lock();
    while (ptr != end) {
        _base_putc(*ptr++);
    }

    unlock();

    return ptr - (const char*)buffer;
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::run()
{
    while (1) {
        if (_mpu6050Available)
            mpu6050UpdateData();

        if (_rxDataLen > 0) {
            _rxDataLen = 0;
            switch (_rxData[0]) {
                case 'A':   // Analog read
                    analogRead();
                    break;

                case 'W':   // PWM (analog) write
                    pwmWrite();
                    break;

                case 'D':   // Digital
                    digitalReadWrite();
                    break;

                case 'S':   // Servo motor
                    servo();
                    break;

                case 'G':   // MPU6050
                    mpu6050();
                    break;

                case 'I':   // Interrupt-in counter
                    intrInCounter();
                    break;

                case 'E':   // Encoder
                    encoder();
                    break;

                case 'C':   // DC motor init
                    dcMotorInit();
                    break;

                case 'M':   // DC motor
                    dcMotor();
                    break;

                case 'R':   // Revision
                    revision();
                    break;
            }
        }
    }
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::analogRead()
{
    int     pin = _rxData[1] - '0';                     // analog pin number 0 .. 5
    uint8_t i = pin;                                    // index of corresponding analogIn 0 .. 5

    if ((pin >= 0 && pin <= 5) && (_analogIns[i] != NULL)) {
        uint16_t    word = _analogIns[i]->read_u16();   // get decimal value 0 .. 65535
        float       val = word / 64.0f;                 // map 16bit range to 10bit range

        word = round(val);
        write((uint8_t*) &word, sizeof(uint16_t));      // send it to Scilab
    }
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::pwmWrite()
{
    int     pin = _rxData[1] - '0';                 // digital pin number 4 .. 7
    uint8_t i = pin - 4;                            // index of corresponding analogOut (PWMOut) 0 .. 3

    if ((pin >= 4 && pin <= 7) && (_pwmOuts[i] != NULL)) {
        uint8_t val = _rxData[2];                   // the duty cycle: between 0 (always off) and 255 (always on)
        float   dutyCycle = (float)val / 255.0f;    // 0.0f (representing on 0%) and 1.0f (representing on 100%)

        _pwmOuts[i]->write(dutyCycle);
    }
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::digitalReadWrite()
{
    int     pin = _rxData[2] - '0';         // digital pin number 8 .. 13
    uint8_t i = pin - 8;                    // index of corresponding digitalInOut 0 .. 7

    if ((pin >= 8 && pin <= 13) && (_digitalPinName[pin] != NC)) {
        char            cmd = _rxData[1];   // command
        PinDirection    pinDirection;       // pin direction (PIN_INPUT, PIN_OUTPUT)
        char            c;
        uint8_t         val;

        switch (cmd) {
            case 'a':                       // 'a'-> activate
                pinDirection = (PinDirection) (_rxData[3] - '0');
                if (_digitalInOuts[i] != NULL)
                    delete _digitalInOuts[i];
                _digitalInOuts[i] = new DigitalInOut(_digitalPinName[pin], pinDirection, PullUp, 0);
                break;

            case 'r':                       // 'r'-> read
                c = _digitalInOuts[i]->read() + '0';
                write((uint8_t*) &c, 1);
                break;

            case 'w':                       // 'w'-> write
                val = _rxData[3] - '0';

                if (val == 0 || val == 1)
                    _digitalInOuts[i]->write(val);
                break;
        }
    }
    else
        _digitalInOuts[i] = NULL;
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::servo()
{
    int     pin = _rxData[2] - '0' + 1; // digital pin number 2 .. 3
    uint8_t i = pin - 2;                // index of corresponding servo 0 .. 1
    uint8_t val;

    if ((pin == 2 || pin == 3) && (_digitalPinName[pin] != NC)) {
        char    cmd = _rxData[1];
        switch (cmd) {
            case 'a':                   // 'a' -> activate servo
                if (_servos[i] != NULL)
                    delete _servos[i];
                _servos[i] = new Servo(_digitalPinName[pin]);
                break;

            case 'd':                   // 'd'-> delete servo
                delete _servos[i];
                _servos[i] = NULL;
                break;

            case 'w':                   // 'w'-> write to servo position in degree (0 .. 180)
                val = _rxData[3];
                if (val >= 0 && val <= 180)
                    _servos[i]->position(val);
                break;
        }
    }
    else
        _servos[i] = NULL;
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::mpu6050()
{
    uint8_t cmd = _rxData[1];
    uint16_t data[9];

    if (I2C_SCL != NC && I2C_SDA != NC) {
        switch (cmd) {
            case 'a':                       // 'a'-> acivate
                if (_mpu6050 != NULL)
                    delete _mpu6050;
                _mpu6050 = new MPU6050();
                _mpu6050Available = _mpu6050->init();
                _mpu6050Timer.start();
                break;

            case 't':                       // 't' -> test
                if (_mpu6050->selfTestOK()) {
                    _mpu6050->reset();      // Reset registers to default in preparation for device calibration
                    _mpu6050->calibrate();  // Calibrate gyro and accelerometers, load biases in bias registers
                    _mpu6050Available = _mpu6050->init();
                    _mpu6050Timer.reset();
                    _mpu6050Timer.start();
                    write("OK", 2);
                }
                else {
                    _mpu6050Available = false;
                    write("KO", 2);
                }
                break;

            case 'r':                       // 'r'-> read
                if (_mpu6050Available) {
                    data[0] = _mpu6050->yaw();
                    data[1] = _mpu6050->pitch();
                    data[2] = _mpu6050->roll();
                    data[3] = _mpu6050->accelX;
                    data[4] = _mpu6050->accelY;
                    data[5] = _mpu6050->accelZ;
                    data[6] = _mpu6050->gyroX;
                    data[7] = _mpu6050->gyroY;
                    data[8] = _mpu6050->gyroZ;
                    write(data, sizeof(data));  // send data to scilab
                }
                break;
        }
    }
    else
        _mpu6050 = NULL;
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::mpu6050UpdateData()
{
    // check if data is available after interrupt

    if (_mpu6050->dataReady()) {
        _mpu6050->accel();
        _mpu6050->gyro();

        uint32_t    _now = _mpu6050Timer.read_us();
        _mpu6050Timer.reset();

        float   _deltaT = float(_now / 1e-6);   // set integration time in seconds

        _mpu6050->madgwickFilter(_deltaT);      // apply filter to estimate yaw, pitch and roll
    }
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::intrInCounter()
{
    int     pin = _rxData[2] - '0'; // digital pin number 8 .. 13
    uint8_t i = pin - 8;            // index of corresonding IntCounter 0 .. 7

    if ((pin >= 8 && pin <= 13) && (_digitalPinName[pin] != NC)) {
        uint8_t val;
        char    cmd = _rxData[1];
        switch (cmd) {
            case 'a':               // 'a' -> activate a counter
                if (_intrInCounters[i] != NULL)
                    delete _intrInCounters[i];
                _intrInCounters[i] = new IntrInCounter(_digitalPinName[pin]);
                break;

            case 'p':               // 'p' -> get position of a counter
                val = _intrInCounters[i]->read();
                write(&val, 1);     // send value to scilab (0 .. 256)
                break;

            case 'r':               // 'r' -> release an interrupt counter
                delete _intrInCounters[i];
                break;

            case 'z':               // 'z' -> set to zero (reset) a counter
                _intrInCounters[i]->reset();
                break;
        }
    }
    else
        _intrInCounters[i] = NULL;
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::encoder()
{
    uint8_t i = _rxData[2] - '0';           // encoder index 0 .. 1

    if ((i >= 0 && i <= 1) && (_digitalPinName[i])) {
        int     pinA, pinB;
        int8_t  pos;
        int     mode;
        char    cmd = _rxData[1];           // command

        switch (cmd) {
            case 'a':                       // 'a' -> activate an encoder
                i == 0 ? pinA = 8 : pinA = 10;
                pinB = pinA + 1;            // pinB -> 9 or 11
                mode = _rxData[3] - '0';    // '1' -> up chanA, '2' -> up/down chanA, '4' -> up/down chanA and chanB
                if (_encoders[i] != NULL)
                    delete _encoders[i];
                _encoders[i] = new Encoder(_digitalPinName[pinA], _digitalPinName[pinB], mode);
                break;

            case 'p':                       // 'p' -> get position of an encoder
                pos = _encoders[i]->position();

                write(&pos, 1);             // send value to scilab (0 .. 256)
                break;

            case 'r':                       // 'r' -> release an encoder
                delete _encoders[i];
                break;

            case 'z':                       // 'z' -> set to zero (reset) the position of an encoder
                _encoders[i]->reset();
                break;
        }
    }
    else
        _encoders[i] = NULL;
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::dcMotorInit()
{
    uint8_t pin1, pin2;
    uint8_t driverType;
    uint8_t i = _rxData[1] - '0';               // motor index

    if ((i < 2) && (_digitalPinName[i] != NC)) {
        pin1 = _rxData[2];                      // pin1 is 2 or 4
        if (pin1 == 2 || pin1 == 4) {
            pin2 = _rxData[3] - '0';            // pin2 is 3 or 5
            if (pin2 == 3 || pin2 == 5) {
                driverType = _rxData[4] - '0';  //  driver: 0 -> L293, 1 -> L298
                if (driverType >= 0 && driverType <= 1) {
                    if (_dcMotors[i] != NULL)
                        delete _dcMotors[i];
                    _dcMotors[i] = new DcMotor(_digitalPinName[pin1], _digitalPinName[pin2], driverType);
                    write("OK", 2);             // tell scilab that motor initialization is complete
                }
            }
        }
    }
    else
        _dcMotors[i] = NULL;
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::dcMotor()
{
    int     pin1, pin2;
    int     mode;
    char    cmd;
    float   speed;
    uint8_t i = _rxData[1] - '0';   // motor index

    if ((i < 2) && (_dcMotors[i] != NULL)) {
        cmd = _rxData[2] - '0';     // direction of rotation or release
        switch (cmd) {
            case 0:                 // clockwise
            case 1:                 // counter clockwise
                speed = _rxData[3]; // speed
                _dcMotors[i]->run(cmd, speed);
                break;

            case 'r':               // release
                delete _dcMotors[i];
                break;
        }
    }
}

/**
 * @brief
 * @note
 * @param
 * @retval
 */
void ScilabSerial::revision()
{
    write("v5", 2);
}