summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--2005/i/robert/src/proto/proto.cc94
1 files changed, 55 insertions, 39 deletions
diff --git a/2005/i/robert/src/proto/proto.cc b/2005/i/robert/src/proto/proto.cc
index 78177f7..1246956 100644
--- a/2005/i/robert/src/proto/proto.cc
+++ b/2005/i/robert/src/proto/proto.cc
@@ -57,27 +57,27 @@ bool
Proto::sync(void)
{
bool reGet;
- //Récupération de la frame
+ // Récupération de la frame
while (reGet = getFrame())
{
log_ ("recv") << "frame" << currentFrame_;
- //Si la frame est un aquittement
+ // Si la frame est un aquittement
if(currentFrame_ == frameQueue_.front())
{
- //on vire la commande de la queue
+ // on vire la commande de la queue
frameQueue_.pop();
- //Et on envoie la suivante si elle existe
+ // Et on envoie la suivante si elle existe
if(!frameQueue_.empty())
sendFrame(frameQueue_.front());
}
- //Si c'est une nouvelle commande, on l'envoie avec receive
+ // Si c'est une nouvelle commande, on l'envoie avec receive
else
receiver_.receive(currentFrame_.command, currentFrame_);
}
- //On regarde depuis combien de temps on a envoyé une commande
+ // On regarde depuis combien de temps on a envoyé une commande
if(!frameQueue_.empty())
{
- //Si on dépasse la milliseconde, on renvoie
+ // Si on dépasse la milliseconde, on renvoie
if(Timer::getProgramTime() - tLastSend_ >= timeout_)
sendFrame(frameQueue_.front());
}
@@ -172,7 +172,7 @@ bool
Proto::decode (const Frame &frame, const char *format, int &a0,
int &a1, int &a2, int &a3)
{
- //Teste si il y a bien le bon nombre d'argument
+ // Teste si il y a bien le bon nombre d'argument
if (static_cast<int>(frame.args.size()) != argsFrameSize(format))
return false;
// On décode et on envoie
@@ -195,56 +195,72 @@ Proto::wait (int timeout/*-1*/)
bool
Proto::getFrame(void)
{
- int receivedChar;
- //tant que le tampon n'est pas vide, on teste
+ int receivedChar, d;
+ bool erreur = false;
+ // tant que le tampon n'est pas vide, on teste
while((receivedChar = serial_.getchar()) != -1)
{
- //Si on reçoit un bang
+ // Si on reçoit un bang
if(receivedChar == '!')
{
revState_ = 1;
currentFrame_.command = 0;
currentFrame_.args.clear();
}
- //Si on reçoit le retour chariot et que on reçevait les args
+ // Si on reçoit le retour chariot et que on reçevait les args
else if(receivedChar == '\r' && revState_ == 2)
{
revState_ = 0;
return true;
}
- //Pour les autres charactères
- //Si on attend la commande
+ // Pour les autres charactères
+ // Si on attend la commande
else
switch(revState_)
{
case 1:
- if (!isalpha (receivedChar))
+ if (!isalpha (receivedChar) || receivedChar == 'e')
+ erreur = true;
+ else
{
- Erreur.
+ currentFrame_.command = receivedChar;
+ revState_ = 2;
}
- currentFrame_.command = receivedChar;
- revState_ = 2;
break;
case 2:
d = hex2digit (receivedChar);
if (d == hexInvalid)
+ erreur = true;
+ else
{
- Erreur.
+ currentFrame_.args.push_back (static_cast<uint8_t>(d) << 4);
+ revState_ = 3;
}
- currentFrame_.args.push_back (static_cast<uint8_t>(d) << 4);
- revState_ = 3;
break;
case 3:
d = hex2digit (receivedChar);
if (d == hexInvalid)
+ erreur = true;
+ else
{
- Erreur.
+ currentFrame_.args.back() |= static_cast<uint8_t>(d);
+ revState_ = 2;
+ // Si on reçoit 0xff comme argument, erreur
+ if(currentFrame_.args.back() == 0xff)
+ erreur = true;
+ break;
}
- currentFrame_.args.back() |= static_cast<uint8_t>(d);
- revState_ = 2;
- break;
- }
- //Si revState == 0 alors on jette
+ }
+ // Si revState == 0 alors on jette
+ // Si on a reçu une erreur on renvoie
+ if(erreur)
+ {
+ // On renvoie en mettant le compteur à 0, la commande sera
+ // renvoyer de retour à sync
+ tLastSend_ = 0;
+ revState_ = 0;
+ return false;
+ }
}
return false;
}
@@ -254,23 +270,24 @@ void
Proto::sendFrame(const Frame & frame)
{
log_ ("send") << "frame" << frame;
- //envoyer le bang
+ // envoyer le bang
serial_.putchar('!');
- //Envoyer la commande
+ // Envoyer la commande
serial_.putchar(frame.command);
- //Envoyer les arguments
- for(int i = 0; i < static_cast<int>(frame.args.size()); i++)
+ // Envoyer les arguments
+ for(std::vector<uint8_t>::const_iterator it = frame.args.begin();
+ it != frame.args.end(); it++)
{
- serial_.putchar(digit2hex(frame.args[i] >> 4));
- serial_.putchar(digit2hex(frame.args[i] & 0x0f));
+ serial_.putchar(digit2hex(*it >> 4));
+ serial_.putchar(digit2hex(*it & 0x0f));
}
- //Envoyer le retour chariot
+ // Envoyer le retour chariot
serial_.putchar('\r');
- //actualiser le timer
+ // actualiser le timer
tLastSend_ = Timer::getProgramTime();
}
@@ -304,7 +321,6 @@ int
Proto::argsFrameSize(const char *format)
{
int size = 0;
- Bla !
for(; *format != '\0'; format++)
switch(*format)
{
@@ -313,12 +329,12 @@ Proto::argsFrameSize(const char *format)
size += 1;
case 'w':
case 'W':
- size += 1;
+ size += 2;
case 'd':
case 'D':
- size += 1;
+ size += 4;
default:
- size += 1;
+ size += 0;
}
return size;
}