1

/*****************************************************************************

     2

 *

     3

 * $Id$

     4

 *

     5

 ****************************************************************************/

     6

     7

#include <iostream>

     8

#include <iomanip>

     9

using namespace std;

    10

    11

#include "globals.h"

    12

#include "coe_datatypes.h"

    13

    14

/****************************************************************************/

    15

    16

const char *help_sdo_upload =

    17

    "[OPTIONS]\n"

    18

    "\n"

    19

    "\n"

    20

    "Command-specific options:\n";

    21

    22

/****************************************************************************/

    23

    24

void command_sdo_upload(void)

    25

{

    26

    stringstream strIndex, strSubIndex;

    27

    int sval;

    28

    ec_ioctl_slave_sdo_upload_t data;

    29

    unsigned int uval;

    30

    const CoEDataType *dataType = NULL;

    31

    32

    if (slavePosition < 0) {

    33

        stringstream err;

    34

        err << "'sdo_upload' requires a slave! Please specify --slave.";

    35

        throw MasterDeviceException(err.str());

    36

    }

    37

    data.slave_position = slavePosition;

    38

    39

    if (commandArgs.size() != 2) {

    40

        stringstream err;

    41

        err << "'sdo_upload' takes two arguments!";

    42

        throw MasterDeviceException(err.str());

    43

    }

    44

    45

    strIndex << commandArgs[0];

    46

    strIndex

    47

        >> resetiosflags(ios::basefield) // guess base from prefix

    48

        >> data.sdo_index;

    49

    if (strIndex.fail()) {

    50

        stringstream err;

    51

        err << "Invalid Sdo index '" << commandArgs[0] << "'!";

    52

        throw MasterDeviceException(err.str());

    53

    }

    54

    55

    strSubIndex << commandArgs[1];

    56

    strSubIndex

    57

        >> resetiosflags(ios::basefield) // guess base from prefix

    58

        >> uval;

    59

    if (strSubIndex.fail() || uval > 0xff) {

    60

        stringstream err;

    61

        err << "Invalid Sdo subindex '" << commandArgs[1] << "'!";

    62

        throw MasterDeviceException(err.str());

    63

    }

    64

    data.sdo_entry_subindex = uval;

    65

    66

    if (dataTypeStr != "") { // data type specified

    67

        if (!(dataType = findDataType(dataTypeStr))) {

    68

            stringstream err;

    69

            err << "Invalid data type '" << dataTypeStr << "'!";

    70

            throw MasterDeviceException(err.str());

    71

        }

    72

    } else { // no data type specified: fetch from dictionary

    73

        ec_ioctl_slave_sdo_entry_t entry;

    74

    75

        masterDev.open(MasterDevice::Read);

    76

    77

        try {

    78

            masterDev.getSdoEntry(&entry, slavePosition,

    79

                    data.sdo_index, data.sdo_entry_subindex);

    80

        } catch (MasterDeviceException &e) {

    81

            stringstream err;

    82

            err << "Failed to determine Sdo entry data type. "

    83

                << "Please specify --type.";

    84

            throw ExecutionFailureException(err);

    85

        }

    86

        if (!(dataType = findDataType(entry.data_type))) {

    87

            stringstream err;

    88

            err << "Pdo entry has unknown data type 0x"

    89

                << hex << setfill('0') << setw(4) << entry.data_type << "!"

    90

                << " Please specify --type.";

    91

            throw ExecutionFailureException(err);

    92

        }

    93

    }

    94

    95

    if (dataType->byteSize) {

    96

        data.target_size = dataType->byteSize;

    97

    } else {

    98

        data.target_size = DefaultBufferSize;

    99

    }

   100

   101

    data.target = new uint8_t[data.target_size + 1];

   102

   103

    masterDev.open(MasterDevice::Read);

   104

   105

	try {

   106

		masterDev.sdoUpload(&data);

   107

	} catch (MasterDeviceException &e) {

   108

        delete [] data.target;

   109

        throw e;

   110

    }

   111

   112

    masterDev.close();

   113

   114

    if (dataType->byteSize && data.data_size != dataType->byteSize) {

   115

        stringstream err;

   116

        err << "Data type mismatch. Expected " << dataType->name

   117

            << " with " << dataType->byteSize << " byte, but got "

   118

            << data.data_size << " byte.";

   119

        throw MasterDeviceException(err.str());

   120

    }

   121

   122

    cout << setfill('0');

   123

    switch (dataType->coeCode) {

   124

        case 0x0002: // int8

   125

            sval = *(int8_t *) data.target;

   126

            cout << sval << " 0x" << hex << setw(2) << sval << endl;

   127

            break;

   128

        case 0x0003: // int16

   129

            sval = le16tocpu(*(int16_t *) data.target);

   130

            cout << sval << " 0x" << hex << setw(4) << sval << endl;

   131

            break;

   132

        case 0x0004: // int32

   133

            sval = le32tocpu(*(int32_t *) data.target);

   134

            cout << sval << " 0x" << hex << setw(8) << sval << endl;

   135

            break;

   136

        case 0x0005: // uint8

   137

            uval = (unsigned int) *(uint8_t *) data.target;

   138

            cout << uval << " 0x" << hex << setw(2) << uval << endl;

   139

            break;

   140

        case 0x0006: // uint16

   141

            uval = le16tocpu(*(uint16_t *) data.target);

   142

            cout << uval << " 0x" << hex << setw(4) << uval << endl;

   143

            break;

   144

        case 0x0007: // uint32

   145

            uval = le32tocpu(*(uint32_t *) data.target);

   146

            cout << uval << " 0x" << hex << setw(8) << uval << endl;

   147

            break;

   148

        case 0x0009: // string

   149

            cout << string((const char *) data.target, data.data_size)

   150

                << endl;

   151

            break;

   152

        default:

   153

            printRawData(data.target, data.data_size);

   154

            break;

   155

    }

   156

   157

    delete [] data.target;

   158

}

   159

   160

/*****************************************************************************/