search

robotology / icub-firmware

iCub Firmware
Other
12 stars 30 forks source link

The `amc` board uses `ICC` and `CAN` to move the wrist of `ergoCub`. #474

Closed marcoaccame closed 8 months ago

marcoaccame commented 8 months ago

This PR belongs to a set of five PRs on icub-firmware, icub-firmware-shared, icub-firmware-build, icub-main and robots-configuration.

They introduce a new MC service that supports actuators located on the second core of the new dual core ETH boards that run the outer control loop at 1 ms. So far the only dual core board we use is the amc which runs on the first CM7 core and uses an actuator board on its CM4 core: the amc2c.

The core features of the new MC service are:

The above two features are for now supported only by the amc and amc2c boards but will be soon supported by the next dual core board, the amcfoc, and by future ones.

The PRs directly touch the code and binaries of the amc and amc2c boards that use advfoc and ICC but also of the amcbldc that shares some environmental code with the amc2c.

The legacy ETH boards (the ems, mc4plus and mc2plus) are not dual core and don't need the advfoc features. However, the PRs touches them as well because we have changed the UDP protocol to carry the MS service configuration.

There are also changes in icub-main to parse the extra information of the MC service and in robots-configuration for the new xml files.

Description of how the advfoc works

In here is a brief description of how the advfoc works for the case of the wrist of ergocub from YRI down to the HW communication across the two core of the amc board.

The xml configuration

The xml description uses the type eomn_serv_MC_advfoc that can manage actuators on CAN and also on ICC even in mixed location. Moreover, each actuator can be of mixed board type and hence with different application and protocol versions.

<group name="SERVICE">

<!--
    migration to inter-core-communication (ICC) 

    the legacy foc MC service type with CAN communication uses the following
    - SERVICE.type = eomn_serv_MC_foc
    - SERVICE.PROPERTIES.JOINTMAPPING.ACTUATOR:
      - .type =     foc         foc         foc
      - .onboard =  any-string  any-string  any-string -> it uses the first board on PROPERTIES.CANBOARDS
      - .port =     CAN1:1      CAN1:2      CAN1:3   

    to enable the advfoc MC service type and inter-core-communication vs amc2c, do the following
    - change the SERVICE.type to be: 
      - SERVICE.type = eomn_serv_MC_advfoc
    - change the SERVICE.PROPERTIES.JOINTMAPPING.ACTUATOR to be:
      - .type =     advfoc      advfoc      advfoc
      - .onboard =  amcbldc     amcbldc     amc2c
      - .port =     CAN1:1      CAN1:2      ICC1:3          

    to enable the advfoc MC service type and CAN communication vs amc2c, do the following
    - change the SERVICE.type to be: 
      - SERVICE.type = eomn_serv_MC_advfoc
    - change the SERVICE.PROPERTIES.JOINTMAPPING.ACTUATOR to be:
      - .type =     advfoc      advfoc      advfoc
      - .onboard =  amcbldc     amcbldc     amc2c
      - .port =     CAN1:1      CAN1:2      CAN1:3                  

 -->

    <param name="type"> eomn_serv_MC_advfoc </param>

    <group name="PROPERTIES">

        <group name="ETHBOARD">
            <param name="type"> amc </param>
        </group>

        <!-- NOTE: 
             keep amcbldc first in the column if you use the eomn_serv_MC_foc, 
             because the parsing of eomn_serv_MC_foc gets only the first board
         -->
        <group name="CANBOARDS">
            <param name="type">                 amcbldc amc2c       </param> 
            <group name="PROTOCOL">
                <param name="major">            2       2           </param>
                <param name="minor">            0       0           </param>
            </group>
            <group name="FIRMWARE">
                <param name="major">            2       3           </param>
                <param name="minor">            0       0           </param>
                <param name="build">            10      0           </param>
            </group>
        </group>

        <group name="JOINTMAPPING">

            <group name="ACTUATOR">
                <param name="type">             advfoc          advfoc          advfoc      </param>
                <param name="onboard">          amcbldc         amcbldc         amc2c       </param>
                <param name="port">             CAN1:1          CAN1:2          ICC1:3      </param>
            </group>

            <group name="ENCODER1">
                <param name="type">             aea3            aea3            aea3        </param>
                <param name="port">             CONN:J5_X1      CONN:J5_X2      CONN:J5_X3  </param>
                <param name="position">         atjoint         atjoint         atjoint     </param>
                <param name="resolution">       16384           16384           16384       </param>
                <param name="tolerance">        0.4             0.4             0.4         </param>
            </group>

            <group name="ENCODER2">
                <param name="type">             none            none            none        </param>
                <param name="port">             CAN1:1:0        CAN1:2:0        CAN1:3:0    </param>
                <param name="position">         atmotor         atmotor         atmotor     </param>
                <param name="resolution">       0               0               0           </param>
                <param name="tolerance">        0               0               0           </param>
            </group>

        </group>

    </group>

</group>

Code listing. File w/ service description. 

<group name="ADVFOC">
    <param name="HasHallSensor">         1             1             1           </param>
    <param name="HasTempSensor">         0             0             0           </param>
    <param name="HasRotorEncoder">       0             0             0           </param>
    <param name="HasRotorEncoderIndex">  0             0             0           </param>
    <param name="HasSpeedEncoder">       0             0             0           </param>
    <param name="RotorIndexOffset">      0             0             0           </param>
    <param name="MotorPoles">            14            14            14          </param>
</group>

Code listing. File w/ mechanical properties traditionally keeps also the properties of the actuators. I left them in the same place but I have changed the name of the group to be uniform w/ the advfoc type.

The transport over UDP

This information is transported over UDP inside the traditional struct eOmn_service_cmmnds_command_t that has been enhanced in its eOmn_serv_config_data_mc_t field to contain a new description for the case eomn_serv_MC_advfoc. 

---
title: From YRI to the amc board
---

stateDiagram-v2

    YRI --> amc : set(eOmn_service_cmmnds_command_t sc = {advfoc})

Figure. The xml files are parsed by YRI and their information is transported to the amc board using a eOmn_service_cmmnds_command_t struct.

In here the changed data structures.

typedef union                               
{   // max(324, 28, 316, 324, 316, 328, 340, 332)
    eOmn_serv_config_data_mc_foc_t          foc_based;
    eOmn_serv_config_data_mc_mc4_t          mc4_based;
    eOmn_serv_config_data_mc_mc4plus_t      mc4plus_based;
    eOmn_serv_config_data_mc_mc4plusmais_t  mc4plusmais_based;
    eOmn_serv_config_data_mc_mc2plus_t      mc2plus;
    eOmn_serv_config_data_mc_mc2pluspsc_t   mc2pluspsc;
    eOmn_serv_config_data_mc_mc4plusfaps_t  mc4plusfaps;
    eOmn_serv_config_data_mc_advfoc_t       advfoc; // <-- new struct
} eOmn_serv_config_data_mc_t;               EO_VERIFYsizeof(eOmn_serv_config_data_mc_t, 340)

Code listing. The struct eOmn_serv_config_data_mc_t hold the new eOmn_serv_config_data_mc_advfoc_t struct.

In particular, it is the new struct eOmn_serv_config_data_mc_advfoc_t alongside with its new fields that does the job.

typedef struct
{
    uint8_t                 type;       // use eOmc_actuator_t
    eOlocation_t            location;
    eObrd_info_t            board; 
} eOmc_adv_actuator_descriptor_t;   EO_VERIFYsizeof(eOmc_adv_actuator_descriptor_t, 8)

typedef struct
{
    uint8_t     type;           // use eOmc_encoder_t
    uint8_t     port : 5;       // use eObrd_port_t or eObrd_portmais_t or eObrd_portpsc_t
    uint8_t     pos  : 3;       // use eOmc_position_t
    uint8_t     ffu[2];
} eOmc_adv_encoder_descriptor_t;   EO_VERIFYsizeof(eOmc_adv_encoder_descriptor_t, 4)  

typedef struct 
{   // 8+4+4
    eOmc_adv_actuator_descriptor_t  actuator;
    eOmc_adv_encoder_descriptor_t   encoder1;
    eOmc_adv_encoder_descriptor_t   encoder2;
} eOmc_adv_jomo_descriptor_t;       EO_VERIFYsizeof(eOmc_adv_jomo_descriptor_t, 16)

typedef struct                          
{   // 4+4*16=24
    eOarray_head_t                      head;
    eOmc_adv_jomo_descriptor_t          data[4];
} eOmc_arrayof_4advjomodescriptors_t;   EO_VERIFYsizeof(eOmc_arrayof_4advjomodescriptors_t, 68)

typedef struct
{   // 68+264 = 332   
    eOmc_arrayof_4advjomodescriptors_t      arrayof4advjomodescriptors;  
    eOmc_adv4jomo_coupling_t                adv4jomocoupling;  
} eOmn_serv_config_data_mc_advfoc_t;        EO_VERIFYsizeof(eOmn_serv_config_data_mc_advfoc_t, 332)

Code listing. Now each actuator is described by the eOmc_adv_actuator_descriptor_tstruct that contains type of board, its application and protocol version and its location.

typedef enum
{ 
    eobus_can1 = 0, eobus_can2 = 1, eobus_local = 2, eobus_icc1 = 3, eobus_icc2 = 4, eobus_ffu1 = 5, eobus_ffu2 = 6, eobus_none = 7             
} eObus_t; 

typedef struct
{
    uint8_t bus:3; // use only values from eObus_t
    uint8_t ffu:1; 
    uint8_t adr:4;      
} eOlocation_t; EO_VERIFYsizeof(eOlocation_t, 1)

Code listing. The new compact eOlocation_t struct allows CAN, local and inter-core locations.

Inside the amc board

The eOmn_service_cmmnds_command_t is handled by the CM7 application which runs new code to manage the MC service and to communicate towards the second CM4 core.

The handling of the service configuration

The received MC service configuration eOmn_service_cmmnds_command_t is managed by the object embot::app::eth::theServices that calls the embot::app::eth::Service interface of Category::mc that we have loaded.

In this PR I have added only for the amc board (and later boards) a new class, the embot::app::eth::theServiceMC, that still manages the Type::MC_foc as now but also a brand new Type::MC_advfoc .

The theServiceMC activates a dedicated implementation depending on the case Type::MC_foc or Type::MC_advfoc that will trigger the relevant objects.

As an example, for the case of the advfoc the implementation in class embot::app::eth::service::impl::mc::AGENTadvfoc calls the traditional EOtheCANmapping and EOtheCANservice to manage CAN locations and the new embot::app::eth::theICCmapping and embot::app::eth::theICCservice to manage communication with ICC-located actuators.

---
title: The internal of the amc board
---

stateDiagram-v2

    amc --> theServices 
    theServices --> theServiceMC : process(eOmn_service_cmmnds_command_t sc = {advfoc})

    state amc {
        afoc: AGENTfoc        
        [*] --> theServices
        theServiceMC
        theServiceMC --> afoc
        theServiceMC --> AGENTadvfoc
        AGENTadvfoc --> EOtheCANmapping
        AGENTadvfoc --> EOtheCANservice
        EOtheCANservice --> OtherCANboards
        OtherCANboards --> EOtheCANservice
        AGENTadvfoc --> theICCmapping
        AGENTadvfoc --> theICCservice
        theICCservice --> OtherCoreBoard
        OtherCoreBoard --> theICCservice

    }

Figure. The object ServiceMC reuses legacy C objects for CAN communication and new objects for ICC communication.

The new theICCservice and theICCmapping objects

The theICCservice is responsible to send messages from one core to another and to handle received ones. The theICCmapping is used to associate received messages to MC entities.

The object theICCservice holds the following objects:

Its internals use the following mechanism for transmitting (reception is dual).


sequenceDiagram
    user ->> txFIFO: put()
    activate txFIFO
    txFIFO ->> user: release()
    deactivate txFIFO

Figure. Step 1 of transmission: the user, for instance the CM7 core of the amc, puts a messages in the TX FIFO.

sequenceDiagram
    user ->> txThread: flush() sends a tx event
    activate user
    deactivate user
    activate user
    activate txThread
    txThread ->> txFIFO: get all items
    activate txFIFO
    txFIFO ->> txThread: 
    deactivate txFIFO
    deactivate txThread
    txThread ->> shared hw icc LTR: transmit MEM
    shared hw icc LTR ->> txThread: 
    shared hw icc LTR ->> user: release
    deactivate user

Figure. Step 2 of transmission: when the user has put all the required messages, it starts transmission in blocking mode. The TX threads picks up all the messages in the TX FIFO and sends them all together to a shared shared embot::hw::icc::LTR. It waits until the receiving part, for instance the CM4 core, releases the shared embot::hw::icc::LTR.

sequenceDiagram
    shared hw icc LTR ->> rxThread: send rx event
    activate rxThread
    rxThread ->> shared hw icc LTR: get MEM and send ACK to teh other core 
    activate shared hw icc LTR
    shared hw icc LTR ->> rxThread: 
    deactivate shared hw icc LTR
    rxThread ->> rxFIFO: copy MEM into rxFIFO
    rxFIFO ->> rxThread: 
    deactivate rxThread

Figure. Step 3 of transmission: the receiver had previously registered the shared embot::hw::icc::LTR to alert its RX thread of any incoming data. The RX thread gets the data inside embot::hw::icc::LTR, copy that inside its RX FIFO and finally releases the shared embot::hw::icc::LTR so that the other core is unlocked.

Mergeability

The new features have been extensively tested over the months (yep, the ICC work began in November 2023) during development and with final tests during the last two weeks:

So, they can be safely merged.

Linked PRs