Enhancing saturation information in STRAIN

[marcoaccame]

The STRAIN board uses the 7th byte of CAN messages ICUBCANPROTO_PER_AS_MSG__FORCE_VECTOR and ICUBCANPROTO_PER_AS_MSG__TORQUE_VECTOR to hold information about saturation of any of its 6 the acquisition channels.

However, @DanielePucci and @francesco-romano need some more information.

They have asked to have the saturation information not on a global basis but expressed per each channel. Moreover, they need to know also if saturation has happened on lower or on upper limit of the range.

@valegagge @davege89 @randaz81

[marcoaccame]

After a meeting with @marcoaccame, @randaz81, @DanielePucci and @francesco-romano, the preferred solution emerged to be the following.

The messages ICUBCANPROTO_PER_AS_MSG__FORCE_VECTOR and ICUBCANPROTO_PER_AS_MSG__TORQUE_VECTOR keeps on being of length 7.

The 7th byte of the messages is split into groups of two bits, where:

• the first group is equal in both messages and has value either 0 or 1 in both. It expresses the legacy information about presence or absence of saturation.
• the second, third, and fourth groups contain the info about the saturation in each channel, where the first three channels are in ICUBCANPROTO_PER_AS_MSG__FORCE_VECTOR and the second three in ICUBCANPROTO_PER_AS_MSG__TORQUE_VECTOR. Their values can be 0 for no saturation, 1 for saturation towards low limit, 2 for saturation towards upper limit.

The following C code should clarify some more.

typedef enum
{
    saturationNONE  = 0,
    saturationLOW   = 1,
    saturationHIGH  = 2
} saturationInfo_t;

typedef struct
{
    uint8_t thereIsSaturationInAtLeastOneChannel    : 2;    // is either 0 or 1
    uint8_t saturationInChannel_0                   : 2;    // use values from saturationInfo_t
    uint8_t saturationInChannel_1                   : 2;    // use values from saturationInfo_t
    uint8_t saturationInChannel_2                   : 2;    // use values from saturationInfo_t   
} the7thByte_of_ICUBCANPROTO_PER_AS_MSG__FORCE_VECTOR;

typedef struct
{
    uint8_t thereIsSaturationInAtLeastOneChannel    : 2;    // is either 0 or 1
    uint8_t saturationInChannel_3                   : 2;    // use values from saturationInfo_t
    uint8_t saturationInChannel_4                   : 2;    // use values from saturationInfo_t
    uint8_t saturationInChannel_5                   : 2;    // use values from saturationInfo_t   
} the7thByte_of_ICUBCANPROTO_PER_AS_MSG__TORQUE_VECTOR;

The rationale behind this convention is the following.

• On CAN robots, we can keep on evaluating the 7th byte being 0 or NOT 0. That will work both with existing and with future FW of STRAIN.
• On ETH robots, we can use the extra bits on the 7th byte to give some more information.
• We still keep the 8th byte available for future extra information.

We evaluated other two solutions which we discarded for minor reasons.

1. Add a new message with dedicated extra information about saturation which could be emitted only when saturation happens. Discarded because ... we can add it later on if we want.
2. Add an extra 8th byte in ICUBCANPROTO_PER_AS_MSG__FORCE_VECTOR and ICUBCANPROTO_PER_AS_MSG__TORQUE_VECTOR. Discarded because ... better keep it for future needs.

[marcoaccame]

We would like to implement this as soon as possible in order to allow smooth testing on the purple robot.

Can you please @valegagge manage the FW part on the STRAIN and produce a new build?

Also, @davege89 can you modify the diagnostics messages you have already added in the EMS so that they are extended with this new information?

Thanks to all.

[marcoaccame]

In commit robotology/icub-main@bbc6b9e I have modified the CanBusAnalogSensor.cpp so that it can works seamlessly with both versions of STRAIN messages.

Thanks @randaz81 for the tip.

[marcoaccame]

I am writing an open request of clarification due to the lack of documentation about the format of messages ICUBCANPROTO_PER_AS_MSG__FORCE_VECTOR and ICUBCANPROTO_PER_AS_MSG__TORQUE_VECTOR.

From the analysis of code of CanBusAnalogSensor.cpp

       if (type==0x03) //analog data
        {
            if (id==boardId)
            {
                timeStamp=Time::now();
                switch (dataFormat)
                {
                    case ANALOG_FORMAT_8_BIT:
                        ret=decode8(buff, msgid, data.data());
                        status=IAnalogSensor::AS_OK;
                        break;
                    case ANALOG_FORMAT_16_BIT:
                        if (len==6)
                        {
                            ret=decode16(buff, msgid, data.data());
                            status=IAnalogSensor::AS_OK;
                        }
                        else
                        {
                            if ((len==7) && (buff[6] != 0)) // changed from "== 1" in order to maintain compatibility with extra overflow information in byte 6. 
                            {
                                status=IAnalogSensor::AS_OVF;
                            }
                            else
                            {
                                status=IAnalogSensor::AS_ERROR;
                            }
                            ret=decode16(buff, msgid, data.data());
                        }
                        break;
                    default:
                        status=IAnalogSensor::AS_ERROR;
                        ret=false;
                }
            }
        }

bool CanBusAnalogSensor::decode16(const unsigned char *msg, int msg_id, double *data)
{
    const char groupId=(msg_id & 0x00f);
    int baseIndex=0;
    {
        switch (groupId)
        {
        case 0xA:
            {
                for(int k=0;k<3;k++)
                    {
                        data[k]=(((unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
                        if (useCalibration>0)
                        {
                            data[k]=data[k]*scaleFactor[k]/float(0x8000);
                        }
                    }
            }
            break;
        case 0xB:
            {
                for(int k=0;k<3;k++)
                    {
                        data[k+3]=(((unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
                        if (useCalibration>0)
                        {
                            data[k+3]=data[k+3]*scaleFactor[k+3]/float(0x8000);
                        }
                    }
            }
            break;
        case 0xC:
            {} //skip these, they are not for us
            break;
        case 0xD:
            {} //skip these, they are not for us
            break;
        default:
            yWarning("Got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
            return false;
            break;
        }
        //@@@DEBUG ONLY
        //fprintf(stderr, "   %+8.1f %+8.1f %+8.1f %+8.1f %+8.1f %+8.1f\n",data[0],data[1],data[2],data[3],data[4],data[5],data[6]);
    }

    return true;
}

I would deduce that:

1. the length of messages ICUBCANPROTO_PER_AS_MSG__FORCE_VECTOR and ICUBCANPROTO_PER_AS_MSG__TORQUE_VECTOR can be either 6 or 7.
2. If 6 there is no overflow.
3. if 7 then there is overflow and the 7th byte MUST be non zero otherwise it triggers an AS_ERROR.

Am I correct? Please @valegagge can you check in the FW if it is like that?

This information is useful in the context of collecting info for the issue #9.

[DanielePucci]

👍

[valegagge]

@marcoaccame : yes! if no channel is saturated than messages' length is 6.

In order to save space for further information, we can avoid to use first pair of bits because it summerizes saturation info of tre channels and backward compatibility is mantained.

typedef enum
{
    saturationNONE  = 0,
    saturationLOW   = 1,
    saturationHIGH  = 2
} saturationInfo_t;

typedef struct
{
    uint8_t saturationInChannel_0                   : 2;    // use values from saturationInfo_t
    uint8_t saturationInChannel_1                   : 2;    // use values from saturationInfo_t
    uint8_t saturationInChannel_2                   : 2;    // use values from saturationInfo_t   
    uint8_t dummy                                            :2;    //unused
} the7thByte_of_ICUBCANPROTO_PER_AS_MSG__FORCE_VECTOR;

typedef struct
{
    uint8_t saturationInChannel_3                   : 2;    // use values from saturationInfo_t
    uint8_t saturationInChannel_4                   : 2;    // use values from saturationInfo_t
    uint8_t saturationInChannel_5                   : 2;    // use values from saturationInfo_t   
    uint8_t dummy                                            :2;    //unused
} the7thByte_of_ICUBCANPROTO_PER_AS_MSG__TORQUE_VECTOR;

[valegagge]

strain firmware done!

I commited only source code. I'm waiting for ems fw before commiting executable.

[DanielePucci]

👍

[davege89]

@DanielePucci and @francesco-romano, as you already know the feature is now included in the new firmware version (2.16) for the EMS ( robotology/icub-firmware-build@544eff7 ).

Could you please close this issue after some testing on that?
Thanks!

[DanielePucci]

The new diagnostics seem to work. Very nice job guys. @valegagge @davege89 @marcoaccame

[DanielePucci]

P.S. The tests we carried on consisted in disabling the calibration matrix - so that raw measurements can be directly read at high level - and then in checking that when the raw measurements saturated, the diagnostics sends the associated saturation message informing about the channel that is saturating, and the saturation direction (up or down).

[davege89]

Just to clarify, this is the meaning of the single fields of the message sent to robotInterface:

• source device: CAN bus of the STRAIN board
• source address: address of the STRAIN board
• code: unique ID to identify the diagnostic message (category: hardware, value: strain saturation)
• p16: channel saturated, with a zero-based indexing (0...5)
• p64: count of saturation in the last second of samples (around 1000), divided in:
  • Most significant 32 bits: number of upward saturation
  • Least signifcant 32 bits: number of downward saturation

[DanielePucci]

@francesco-romano @traversaro