% Ajuste dos parametros do controlador PID para o posicionamento do servo
%
% [rx,error,errorstr,tries]=applycontrol(H,scu_id,param,servos)
%
% Entradas:
% H        => Handler para comunicar com o Master
% scu_id   => Identificador do SCU alvo
% param    => Parametro a modificar
% servos   => Dados a aplicar
%
% Saídas:
% rx       => Mensagem de baixo nível recebida
% error    => Código de erro, se existente
% errorstr => String descritiva do erro
% tries    => Número de tentativas para efectuar a comunicação
%
% (c) Milton Ruas da Silva, 2006

function [rx,error,errorstr,tries]=applycontrol(H,scu_id,param,servos)

% Carregamento em memória das constantes
data;

% Construção da mensagem a enviar
message=buildmessage(OP_APPLY_CONTROL,scu_id,param,servos);
% Mensagem de resposta esperada
receive=uint8([MESSAGE_SUCESS message(2:end) zeros(1,USART_RECV_LENGTH-USART_SEND_LENGTH)]);

% Envio da mensagem de request e leitura da resposta
[rx,tries]=sendmessage(H,message);

% Retorno de um valor booleano indicador da coincidencia entre a mensagem
% recebida e a esperada
error=(sum(rx~=receive)>0);

if (~error)
    errorstr='';
else
    if (rx(1)==MESSAGE_INVREQ)
        errorstr='Invalid SCU ID!';
    elseif (rx(2)~=receive(2))
        errorstr='Type packet is different from the source!';
    elseif (sum(rx(3:end)~=receive(3:end))>0)
        errorstr='Data packets doesn''t match to the expected message!';
    else
        errorstr='Unknown error!';
    end
    cportreset(H);
end
