motor_message.cc

Go to the documentation of this file.

#include <ubiquity_motor/motor_message.h>
// #include <ubiquity_motor/motor_message_registers.h>
#include <ros/console.h>
#include <numeric>

uint8_t const MotorMessage::valid_types[] = {TYPE_READ, TYPE_WRITE,
                                             TYPE_RESPONSE, TYPE_ERROR};

uint8_t const MotorMessage::valid_registers[] = {REG_STOP_START,
                                                 REG_BRAKE_STOP,
                                                 REG_CRUISE_STOP,
                                                 REG_LEFT_PWM,
                                                 REG_RIGHT_PWM,
                                                 REG_LEFT_SPEED_SET,
                                                 REG_RIGHT_SPEED_SET,
                                                 REG_LEFT_RAMP,
                                                 REG_RIGHT_RAMP,
                                                 REG_LEFT_ODOM,
                                                 REG_RIGHT_ODOM,
                                                 REG_DEADMAN,
                                                 REG_LEFT_CURRENT,
                                                 REG_RIGHT_CURRENT,
                                                 REG_ERROR_COUNT,
                                                 REG_5V_MAIN_ERROR,
                                                 REG_5V_AUX_ERROR,
                                                 REG_12V_MAIN_ERROR,
                                                 REG_12V_AUX_ERROR,
                                                 REG_5V_MAIN_OL,
                                                 REG_5V_AUX_OL,
                                                 REG_12V_MAIN_OL,
                                                 REG_12V_AUX_OL,
                                                 REG_LEFT_MOTOR_ERROR,
                                                 REG_RIGHT_MOTOR_ERROR,
                                                 REG_PARAM_P,
                                                 REG_PARAM_I,
                                                 REG_PARAM_D,
                                                 REG_PARAM_C,
                                                 REG_LED_1,
                                                 REG_LED_2,
                                                 REG_HARDWARE_VERSION,
                                                 REG_FIRMWARE_VERSION,
                                                 REG_BATTERY_VOLTAGE,
                                                 REG_5V_MAIN_CURRENT,
                                                 REG_12V_MAIN_CURRENT,
                                                 REG_5V_AUX_CURRENT,
                                                 REG_12V_AUX_CURRENT,
                                                 REG_LEFT_SPEED_MEASURED,
                                                 REG_RIGHT_SPEED_MEASURED,
                                                 REG_BOTH_SPEED_SET,
                                                 REG_MOVING_BUF_SIZE,
                                                 REG_BOTH_ODOM,
                                                 REG_LIMIT_REACHED,
                                                 REG_BOTH_ERROR,
                                                 DEBUG_50,
                                                 DEBUG_51,
                                                 DEBUG_52,
                                                 DEBUG_53,
                                                 DEBUG_54,
                                                 DEBUG_55,
                                                 DEBUG_56,
                                                 DEBUG_57,
                                                 DEBUG_58};

void MotorMessage::setType(MotorMessage::MessageTypes type) {
    if (verifyType(type)) {
        this->type = type;
    }
}

MotorMessage::MessageTypes MotorMessage::getType() const {
    return static_cast<MotorMessage::MessageTypes>(this->type);
}

void MotorMessage::setRegister(MotorMessage::Registers reg) {
    if (verifyRegister(reg)) {
        this->register_addr = reg;
    }
}

MotorMessage::Registers MotorMessage::getRegister() const {
    return static_cast<MotorMessage::Registers>(this->register_addr);
}

void MotorMessage::setData(int32_t data) {
    // Spilt 32 bit data (system byte order) into 4 8bit elements in big endian
    // (network byte order)
    this->data[3] = (data >> 0) & 0xFF;
    this->data[2] = (data >> 8) & 0xFF;
    this->data[1] = (data >> 16) & 0xFF;
    this->data[0] = (data >> 24) & 0xFF;
}

int32_t MotorMessage::getData() const {
    // Take big endian (network byte order) elements and return 32 bit int
    return (this->data[0] << 24) | (this->data[1] << 16) |
           (this->data[2] << 8) | (this->data[3] << 0);
}

RawMotorMessage MotorMessage::serialize() const {
    RawMotorMessage out;
    out[0] = delimeter;
    out[1] = (protocol_version << 4) | type;
    out[2] = register_addr;
    std::copy(data.begin(), data.end(), out.begin() + 3);
    out[7] = generateChecksum(out);
    return out;
}

int MotorMessage::deserialize(const RawMotorMessage &serialized) {
    if (serialized[0] == delimeter) {
        if ((serialized[1] & 0xF0) == (protocol_version << 4)) {
            if (generateChecksum(serialized) == serialized[7]) {
                if (verifyType(serialized[1] & 0x0F)) {
                    if (verifyRegister(serialized[2])) {
                        this->type = serialized[1] & 0x0F;
                        this->register_addr = serialized[2];
                        std::copy(serialized.begin() + 3,
                                  serialized.begin() + 7, data.begin());
                        return 0;
                    } else
                        return 5;
                } else
                    return 4;
            } else
                return 3;
        } else
            return 2;
    } else
        return 1;

    // TODO use exceptions instead of cryptic error codes

    // ERROR codes returned:
    // 1 First char not delimiter
    // 2 wrong protocol version
    // 3 bad checksum
    // 4 bad type
    // 5 bad register
}

int MotorMessage::verifyType(uint8_t t) {
    // Return 1 good
    // Return 0 for bad
    for (size_t i = 0; i < sizeof(valid_types) / sizeof(valid_types[0]); ++i) {
        if (t == valid_types[i]) return 1;
    }
    return 0;
}

int MotorMessage::verifyRegister(uint8_t r) {
    // Return 1 good
    // Return 0 for bad
    for (size_t i = 0; i < sizeof(valid_registers) / sizeof(valid_registers[0]);
         ++i) {
        if (r == valid_registers[i]) return 1;
    }
    return 0;
}

uint8_t MotorMessage::generateChecksum(const RawMotorMessage &data) {
    int sum = std::accumulate(data.begin() + 1, data.end() - 1, 0);

    if (sum > 0xFF) {
        int tmp;
        tmp = sum >> 8;
        tmp = tmp << 8;
        return 0xFF - (sum - tmp);
    } else {
        return 0xFF - sum;
    }
}