Page tree

Versions Compared

Key

  • This line was added.
  • This line was removed.
  • Formatting was changed.

...

Code Block
titleparameters access
static inline param_t * GetParamByIndexgetParamByIndex(uint16_t index)
{
    if(index >= ARRAY_SIZE(parameters)) 
    {
        return NULL;
    }
    return &parameters[index];
}


static inline param_t * GetParamByNamegetParamByName(uint8_t * name)
{
    for(uint16_t i = 0; i < ARRAY_SIZE(parameters); i++)
    {
        if(strncmp(name, parameters[i].name,strlen(parameters[i].name)) == 0) 
        {
              return &parameters[i];
        }
    }  
    return NULL;
}

...

Code Block
titleonTransferReceived
void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer)
{
    if ((transfer->transfer_type == CanardTransferTypeRequest) &&
    (transfer->data_type_id == UAVCAN_GET_NODE_INFO_DATA_TYPE_ID))
    {
        canardGetNodeInfoHandlegetNodeInfoHandleCanard(transfer);
    } 
    
    if(transfer->data_type_id == UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID)
    { 
        canardRawcmdHandlerawcmdHandleCanard(transfer);
    }
    
    if(transfer->data_type_id == UAVCAN_PROTOCOL_PARAM_GETSET_ID)
    { 
        canardGetsetHandlegetsetHandleCanard(transfer);
    }
}

We should also write handler for UAVCAN_PROTOCOL_PARAM_GETSET_ID. Here it is:

Code Block
titlecanard_getset_handle
void canardGetsetHandle(CanardRxTransfer* transfer)
{
    uint16_t index = 0xFFFF;
    uint8_t tag    = 0;
    int offset     = 0;
    int64_t val    = 0;

    canardDecodeScalar(transfer, offset,  13, false, &index);
    offset += 13;
    canardDecodeScalar(transfer, offset, 3, false, &tag);
    offset += 3;

    if(tag == 1)
    {
        canardDecodeScalar(transfer, offset, 64, false, &val);
        offset += 64;
    } 

    uint16_t n = transfer->payload_len - offset / 8 ;
    uint8_t name[16]      = "";
    for(int i = 0; i < n; i++)
    {
        canardDecodeScalar(transfer, offset, 8, false, &name[i]);
        offset += 8;
    }

    param_t * p = NULL;

    if(strlen((char const*)name))
    {
        p = GetParamByNamegetParamByName(name);
    }
    else
    {
        p = GetParamByIndexgetParamByIndex(index);
    }

    if((p)&&(tag == 1))
    {
        p->val = val;
    }

    uint8_t  buffer[64] = "";
    uint16_t len = canardEncodeParam(p, buffer);
    int result = canardRequestOrRespond(&g_canard,
                                        transfer->source_node_id,
                                        UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE,
                                        UAVCAN_PROTOCOL_PARAM_GETSET_ID,
                                        &transfer->transfer_id,
                                        transfer->priority,
                                        CanardResponse,
                                        &buffer[0],
                                        (uint16_t)len);
  
}

...

Code Block
titlecanardEncodeParam
uint16_t canardEncodeParamencodeParamCanard(param_t * p, uint8_t * buffer)
{
uint8_t n     = 0;
int offset    = 0;
uint8_t tag   = 1;
if(p==NULL)
{   
    tag = 0;
    canardEncodeScalar(buffer, offset, 5, &n);
    offset += 5;
    canardEncodeScalar(buffer, offset,3, &tag);
    offset += 3;
    
    canardEncodeScalar(buffer, offset, 6, &n);
    offset += 6;
    canardEncodeScalar(buffer, offset,2, &tag);
    offset += 2;
    
    canardEncodeScalar(buffer, offset, 6, &n);
    offset += 6;
    canardEncodeScalar(buffer, offset, 2, &tag);
    offset += 2;
    buffer[offset / 8] = 0;
    return ( offset / 8 + 1 );
}
canardEncodeScalar(buffer, offset, 5,&n);
offset += 5;
canardEncodeScalar(buffer, offset, 3, &tag);
offset += 3;
canardEncodeScalar(buffer, offset, 64, &p->val);
offset += 64;

canardEncodeScalar(buffer, offset, 5, &n);
offset += 5;
canardEncodeScalar(buffer, offset, 3, &tag);
offset += 3;
canardEncodeScalar(buffer, offset, 64, &p->defval);
offset += 64;

canardEncodeScalar(buffer, offset, 6, &n);
offset += 6;
canardEncodeScalar(buffer, offset, 2, &tag);
offset += 2;
canardEncodeScalar(buffer, offset, 64, &p->max);
offset += 64;

canardEncodeScalar(buffer, offset, 6, &n);
offset += 6;
canardEncodeScalar(buffer, offset,2,&tag);
offset += 2;
canardEncodeScalar(buffer, offset,64,&p->min);
offset += 64;

memcpy(&buffer[offset / 8], p->name, strlen((char const*)p->name));
/* See important note below */
return  (offset/8 + strlen((char const*)p->name)); 
}

...