summaryrefslogtreecommitdiff
path: root/i/chuck/src/proto
diff options
context:
space:
mode:
Diffstat (limited to 'i/chuck/src/proto')
-rw-r--r--i/chuck/src/proto/Makefile.defs5
-rw-r--r--i/chuck/src/proto/proto.cc457
-rw-r--r--i/chuck/src/proto/proto.hh140
-rw-r--r--i/chuck/src/proto/test_proto.cc96
4 files changed, 698 insertions, 0 deletions
diff --git a/i/chuck/src/proto/Makefile.defs b/i/chuck/src/proto/Makefile.defs
new file mode 100644
index 0000000..b351026
--- /dev/null
+++ b/i/chuck/src/proto/Makefile.defs
@@ -0,0 +1,5 @@
+PROGRAMS += test_proto
+
+proto_OBJECTS = proto.o $(serial_OBJECTS) $(log_OBJECTS) $(utils_OBJECTS) $(timer_OBJECTS)
+
+test_proto_OBJECTS = test_proto.o $(proto_OBJECTS) $(tester_OBJECTS)
diff --git a/i/chuck/src/proto/proto.cc b/i/chuck/src/proto/proto.cc
new file mode 100644
index 0000000..50e363d
--- /dev/null
+++ b/i/chuck/src/proto/proto.cc
@@ -0,0 +1,457 @@
+// proto.cc
+// robert - programme du robot 2005. {{{
+//
+// Copyright (C) 2005 Nicolas Haller
+//
+// Robot APB Team/Efrei 2005.
+// Web: http://assos.efrei.fr/robot/
+// Email: robot AT efrei DOT fr
+//
+// This program is free software; you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation; either version 2 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// }}}
+
+#include "timer/timer.hh"
+#include "utils/hexa.hh"
+#include "proto.hh"
+
+#include <iostream>
+#include <algorithm>
+#include <iterator>
+
+/// Constructeur.
+Proto::Proto(Receiver &receiver)
+ : log_ ("proto"), receiver_(receiver), tLastSend_(0), revState_(0)
+{
+}
+
+/// Ouvre le port série.
+void
+Proto::open(const std::string &ttyname)
+{
+ serial_.open(ttyname);
+}
+
+/// Ferme le port série
+void
+Proto::close(void)
+{
+ serial_.close();
+}
+
+/// Teste si tout les packets ont été envoyés et aquités, sinon, essaye de
+/// le faire.
+bool
+Proto::sync(void)
+{
+ // Récupération de la frame
+ while (getFrame())
+ {
+ log_ ("recv", Log::debug) << "frame" << currentFrame_;
+ // Si la frame est un aquittement
+ if(currentFrame_ == frameQueue_.front())
+ {
+ // on vire la commande de la queue
+ frameQueue_.pop();
+ // 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
+ else
+ receiver_.receive(currentFrame_.command, currentFrame_);
+ }
+ // On regarde depuis combien de temps on a envoyé une commande
+ if(!frameQueue_.empty())
+ {
+ // Si on dépasse la milliseconde, on renvoie
+ if(Timer::getProgramTime() - tLastSend_ >= timeout_)
+ sendFrame(frameQueue_.front());
+ }
+ return frameQueue_.empty();
+}
+
+/// Envoie un packet
+void
+Proto::send (const Frame & frame, bool reliable)
+{
+ if(reliable)
+ {
+ if(frameQueue_.empty())
+ tLastSend_ = -timeout_;
+ frameQueue_.push(frame);
+ }
+ else
+ sendFrame(frame);
+}
+
+
+/// Envois un packet. COMMAND est la commande à envoyer, FORMAT, donne le
+/// format et le nombre de paramètres ('b' : 8 bits, 'w' : 16 bits, 'd' :
+/// 32 bits, majuscule pour signé).
+void
+Proto::send (char command, const char *format, int a0, int a1,
+ int a2, int a3, int a4, int a5, bool reliable)
+{
+ // Constitution de la frame
+ Proto::Frame frame;
+ frame.command = command;
+
+ if (format[0] != '\0')
+ {
+ newArgFrame(frame, format[0],a0);
+ if (format[1] != '\0')
+ {
+ newArgFrame(frame, format[1],a1);
+ if (format[2] != '\0')
+ {
+ newArgFrame(frame, format[2],a2);
+ if (format[3] != '\0')
+ {
+ newArgFrame(frame, format[3],a3);
+ if(format[4] != '\0')
+ {
+ newArgFrame(frame, format[4],a4);
+ if(format[5] != '\0')
+ newArgFrame(frame, format[5],a5);
+ }
+
+ }
+ }
+ }
+ }
+ send(frame,reliable);
+}
+
+/// Send a string of size number.
+void
+Proto::sendStr (char command, const char *str, int size, bool reliable)
+{
+ Proto::Frame frame;
+
+ frame.command = command;
+
+ for (int compt = 0; compt < size; compt++)
+ newArgFrame (frame, 'b', str[compt]);
+
+ send (frame,reliable);
+}
+
+/// permet d'envoyer un packet pas fiable
+void
+Proto::sendUnreliable (char command, const char *format, int a0, int a1,
+ int a2, int a3, int a4, int a5)
+{
+ send(command, format, a0, a1, a2, a3, a4, a5, false);
+}
+
+/// Teste si le packet correspond au format et décode les valeurs. Utilise
+/// le même format que send.
+bool
+Proto::decode (const Proto::Frame &frame)
+{
+ int dummy;
+ return decode(frame, "", dummy, dummy, dummy, dummy, dummy);
+}
+
+bool
+Proto::decode (const Frame &frame, const char *format, int &a0)
+{
+ int dummy;
+ return decode(frame, format, a0, dummy, dummy, dummy, dummy);
+}
+
+bool
+Proto::decode (const Frame &frame, const char *format, int &a0, int &a1)
+{
+ int dummy;
+ return decode(frame, format, a0, a1, dummy, dummy, dummy);
+}
+
+bool
+Proto::decode (const Frame &frame, const char *format, int &a0,
+ int &a1, int &a2)
+{
+ int dummy;
+ return decode(frame, format, a0, a1, a2, dummy, dummy);
+}
+
+bool
+Proto::decode (const Frame &frame, const char *format, int &a0,
+ int &a1, int &a2, int &a3)
+{
+ int dummy;
+ return decode(frame, format, a0, a1, a2, a3, dummy);
+}
+
+bool
+Proto::decode (const Frame &frame, const char *format, int &a0,
+ int &a1, int &a2, int &a3, int &a4)
+{
+ // 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
+ decodeArg(frame, format, a0, a1, a2, a3, a4);
+
+ return true;
+}
+
+/// Attend que des caractères soit disponible pendant un temps maximum en
+/// milliseconde et les traite. Renvois le résultat de sync (). Attention,
+/// fonction bloquante, n'utiliser que pour les tests.
+bool
+Proto::wait (int timeout/*-1*/)
+{
+ // On apelle serial_.wait avec un timeout toujours inf ou égal à proto::timeout_
+ serial_.wait (timeout < timeout_ && timeout != -1 ? timeout : timeout_);
+ return sync ();
+}
+
+/// Récupère le File Descriptor
+int Proto::getFd(void) const
+{
+ return serial_.getFd();
+}
+
+/// Récupère les infos de l'AVR pour construire une frame
+bool
+Proto::getFrame(void)
+{
+ 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
+ if(receivedChar == '!')
+ {
+ revState_ = 1;
+ currentFrame_.command = 0;
+ currentFrame_.args.clear();
+ }
+ // 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
+ else
+ switch(revState_)
+ {
+ case 1:
+ if (!isalpha (receivedChar) || receivedChar == '?')
+ erreur = true;
+ else
+ {
+ currentFrame_.command = receivedChar;
+ revState_ = 2;
+ }
+ break;
+ case 2:
+ d = hex2digit (receivedChar);
+ if (d == hexInvalid)
+ erreur = true;
+ else
+ {
+ currentFrame_.args.push_back (static_cast<uint8_t>(d) << 4);
+ revState_ = 3;
+ }
+ break;
+ case 3:
+ d = hex2digit (receivedChar);
+ if (d == hexInvalid)
+ erreur = true;
+ else
+ {
+ currentFrame_.args.back() |= static_cast<uint8_t>(d);
+ revState_ = 2;
+ break;
+ }
+ }
+ // 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
+ log_("Erreur de reception", Log::debug)
+ << "commande inconnue" << "";
+ tLastSend_ = 0;
+ revState_ = 0;
+ return false;
+ }
+ }
+ return false;
+}
+
+/// Envoie la frame dans l'AVR
+void
+Proto::sendFrame(const Frame & frame)
+{
+ log_ ("send", Log::debug) << "frame" << frame;
+ // envoyer le bang
+ serial_.putchar('!');
+
+ // Envoyer la commande
+ serial_.putchar(frame.command);
+
+ // Envoyer les arguments
+ for(std::vector<uint8_t>::const_iterator it = frame.args.begin();
+ it != frame.args.end(); ++it)
+ {
+ serial_.putchar(digit2hex(*it >> 4));
+ serial_.putchar(digit2hex(*it & 0x0f));
+ }
+
+ // Envoyer le retour chariot
+ serial_.putchar('\r');
+
+ // actualiser le timer
+ tLastSend_ = Timer::getProgramTime();
+}
+
+/// Remplie une frame avec un argument
+void
+Proto::newArgFrame(Proto::Frame & frame, char format, int arg)
+{
+ switch(format)
+ {
+ case 'b':
+ case 'B':
+ frame.args.push_back(static_cast<uint8_t>(arg));
+ break;
+ case 'w':
+ case 'W':
+ frame.args.push_back(static_cast<uint8_t>(arg >> 8));
+ frame.args.push_back(static_cast<uint8_t>(arg));
+ break;
+ case 'd':
+ case 'D':
+ frame.args.push_back(static_cast<uint8_t>(arg >> 24));
+ frame.args.push_back(static_cast<uint8_t>(arg >> 16));
+ frame.args.push_back(static_cast<uint8_t>(arg >> 8));
+ frame.args.push_back(static_cast<uint8_t>(arg));
+ break;
+ }
+}
+
+/// Renvoie la taille necessaire du vecteur args pour un format donné
+int
+Proto::argsFrameSize(const char *format)
+{
+ int size = 0;
+ for(; *format != '\0'; format++)
+ switch(*format)
+ {
+ case 'b':
+ case 'B':
+ size += 1;
+ break;
+ case 'w':
+ case 'W':
+ size += 2;
+ break;
+ case 'd':
+ case 'D':
+ size += 4;
+ break;
+ default:
+ size += 0;
+ }
+ return size;
+}
+
+/// Décode un argument
+void
+Proto::decodeArg(const Frame & frame, const char *format, int &a0, int &a1, int &a2, int &a3, int &a4)
+{
+ int temp[5];
+ int pos = 0;
+
+ for(int i = 0; *format != '\0'; format++,i++)
+ {
+ switch(*format)
+ {
+ case 'b':
+ temp[i] = static_cast<int>(frame.args[pos]);
+ pos++;
+ break;
+ case 'B':
+ {
+ int8_t t = static_cast<int8_t>(frame.args[pos]);
+ temp[i] = static_cast<int>(t);
+ pos++;
+ break;
+ }
+ case 'w':
+ temp[i] = static_cast<int>(frame.args[pos]) << 8
+ |static_cast<int>(frame.args[pos + 1]);
+ pos += 2;
+ break;
+ case 'W':
+ {
+ int8_t t = static_cast<int8_t>(frame.args[pos]);
+ temp[i] = static_cast<int>(t) << 8
+ |static_cast<int>(frame.args[pos + 1]);
+ pos += 2;
+ break;
+ }
+ case 'd':
+ temp[i] = static_cast<int>(frame.args[pos]) << 24
+ |static_cast<int>(frame.args[pos + 1]) << 16
+ |static_cast<int>(frame.args[pos + 2]) << 8
+ |static_cast<int>(frame.args[pos + 3]);
+ pos += 4;
+ break;
+
+ case 'D':
+ {
+ int8_t t = static_cast<int8_t>(frame.args[pos]);
+ temp[i] = static_cast<int>(t) << 24
+ |static_cast<int>(frame.args[pos + 1]) << 16
+ |static_cast<int>(frame.args[pos + 2]) << 8
+ |static_cast<int>(frame.args[pos + 3]);
+ pos += 4;
+ break;
+ }
+ }
+ }
+ a0 = temp[0];
+ a1 = temp[1];
+ a2 = temp[2];
+ a3 = temp[3];
+ a4 = temp[4];
+}
+
+bool
+Proto::Frame::operator==(const Frame& frame)
+{
+ return this->command == frame.command && this->args == frame.args;
+}
+
+/// Affiche une frame.
+std::ostream &
+operator<< (std::ostream &os, const Proto::Frame &f)
+{
+ os << '<' << f.command << ' ';
+ std::copy (f.args.begin (), f.args.end (),
+ std::ostream_iterator<int> (os, " "));
+ os << '>';
+ return os;
+}
+
diff --git a/i/chuck/src/proto/proto.hh b/i/chuck/src/proto/proto.hh
new file mode 100644
index 0000000..a45db91
--- /dev/null
+++ b/i/chuck/src/proto/proto.hh
@@ -0,0 +1,140 @@
+#ifndef proto_hh
+#define proto_hh
+// proto.hh
+// robert - programme du robot 2005. {{{
+//
+// Copyright (C) 2005 Nicolas Schodet
+//
+// Robot APB Team/Efrei 2005.
+// Web: http://assos.efrei.fr/robot/
+// Email: robot AT efrei DOT fr
+//
+// This program is free software; you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation; either version 2 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// }}}
+
+#include "utils/non_copyable.hh"
+#include "serial/serial.hh"
+#include "log/log.hh"
+
+#include <queue>
+#include <stdint.h>
+
+/// Classe de dialogue avec une carte électronique par le port série.
+class Proto : public NonCopyable
+{
+ public:
+ class Receiver;
+ /// Packet.
+ struct Frame
+ {
+ char command;
+ std::vector<uint8_t> args;
+ bool operator==(const Frame& frame);
+ };
+ private:
+ Log log_;
+ Serial serial_;
+ Receiver &receiver_;
+
+ /// File d'attente des messages à aquiter.
+ typedef std::queue<Frame> FrameQueue;
+ FrameQueue frameQueue_;
+ //Date du dernier envoie
+ int tLastSend_;
+ //Temps entre les ré-émission
+ static const int timeout_ = 100;
+
+ /// Frame en cours de réception
+ Frame currentFrame_;
+ int revState_;
+ // Etat de la réception de la frame
+ // 0 - Rien reçu
+ // 1 - Bang reçu
+ // 2 - Commande reçu & nouveau argument
+ // 3 - char 1 argument reçu
+
+ public:
+ /// Constructeur.
+ Proto (Receiver &receiver);
+ /// Ouvre le port série.
+ void open (const std::string &ttyname);
+ /// Ferme le port série.
+ void close (void);
+ /// Teste si tout les packets ont été envoyés et aquités, sinon, essaye de
+ /// le faire.
+ bool sync (void);
+ /// Envois un packet.
+ void send (const Frame &frame, bool reliable = true);
+ /// Envois un packet. COMMAND est la commande à envoyer, FORMAT, donne le
+ /// format et le nombre de paramètres ('b' : 8 bits, 'w' : 16 bits, 'd' :
+ /// 32 bits, majuscule pour signé).
+ void send (char command, const char *format = "", int a0 = 0, int a1 = 0,
+ int a2 = 0, int a3 = 0, int a4 = 0, int a5 = 0, bool reliable = true);
+ /// Send a string of size number.
+ void sendStr (char command, const char *str, int size, bool reliable =
+ true);
+ /// permet d'envoyer un packet robert
+ void sendUnreliable (char command, const char *format, int a0,
+ int a1, int a2, int a3, int a4, int a5);
+ //@{
+ /// Teste si le packet correspond au format et décode les valeurs. Utilise
+ /// le même format que send.
+ static bool decode (const Frame &frame);
+ static bool decode (const Frame &frame, const char *format, int &a0);
+ static bool decode (const Frame &frame, const char *format, int &a0, int
+ &a1);
+ static bool decode (const Frame &frame, const char *format, int &a0, int
+ &a1, int &a2);
+ static bool decode (const Frame &frame, const char *format, int &a0, int
+ &a1, int &a2, int &a3);
+ static bool decode (const Frame &frame, const char *format, int &a0, int
+ &a1, int &a2, int &a3, int &a4);
+ //@}
+ /// Attend que des caractères soit disponible pendant un temps maximum en
+ /// milliseconde et les traite. Renvois le résultat de sync (). Attention,
+ /// fonction bloquante, n'utiliser que pour les tests.
+ bool wait (int timeout = -1);
+ /// Récupère le File Descriptor
+ int getFd(void) const;
+
+ public:
+ /// Les clients de Proto doivent dériver de Receiver.
+ class Receiver
+ {
+ public:
+ virtual ~Receiver(void) {};
+ /// Recoit un packet.
+ virtual void receive (char command, const Frame &frame) = 0;
+ };
+ private:
+ /// Récupère les infos de l'AVR pour construire une frame
+ bool getFrame(void);
+ /// Envoie la frame dans l'AVR
+ void sendFrame(const Frame & frame);
+ /// Remplie une frame avec un argument
+ void newArgFrame(Proto::Frame & frame, char format, int arg);
+ /// Renvoie la taille necessaire du vecteur args pour un format donné
+ static int argsFrameSize(const char *format);
+ /// Décode un argument
+ static void decodeArg(const Frame & frame, const char *format, int &a0,
+ int &a1, int &a2, int &a3, int &a4);
+};
+
+/// Affiche une frame.
+std::ostream &
+operator<< (std::ostream &os, const Proto::Frame &f);
+
+#endif // proto_hh
diff --git a/i/chuck/src/proto/test_proto.cc b/i/chuck/src/proto/test_proto.cc
new file mode 100644
index 0000000..01cac96
--- /dev/null
+++ b/i/chuck/src/proto/test_proto.cc
@@ -0,0 +1,96 @@
+// test_proto.cc
+// robert - programme du robot 2006. {{{
+//
+// Copyright (C) 2005 Nicolas Haller
+//
+// Robot APB Team/Efrei 2006.
+// Web: http://assos.efrei.fr/robot/
+// Email: robot AT efrei DOT fr
+//
+// This program is free software; you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation; either version 2 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// }}}
+#include "proto.hh"
+#include "timer/timer.hh"
+#include "tester/tester.hh"
+#include "parser/parser.hh"
+
+#include <iostream>
+#include <stdexcept>
+
+class TestProto : public Tester, Proto::Receiver
+{
+ private:
+ Proto proto_;
+ public:
+ // Constructor
+ TestProto (int argc, char ** argv)
+ : Tester (argc, argv), proto_(*this){}
+ void preRun (void)
+ {
+ Interpreter &interpreter = getInterpreter ();
+ // Récupère le tty de config
+ proto_.open(config_.get<std::string>("testProto.tty"));
+ // Add functions.
+ interpreter.add ("s", Interpreter::memFunc ( *this, &TestProto::send ),
+ "Fonction send (string commande, string format, "
+ "int arg...)");
+ interpreter.add ("w", Interpreter::memFunc ( proto_, &Proto::wait ),
+ "Fonction wait ()");
+ }
+ void postRun(void)
+ {
+ proto_.close();
+ }
+ void receive (char command, const Proto::Frame &frame)
+ {
+ std::cout << "received " << frame << std::endl;
+ }
+ void send (const Parser::AnyList & al, bool dryrun)
+ {
+ Parser::AnyList::const_iterator it = al.begin();
+ // arguments envoyé à la fonctions Proto::send
+ int arg[6];
+ // command avr
+ char command = any_cast<char> (*it);
+ // format des arguments
+ std::string format = any_cast<std::string> (*++it);
+ // Récupération des arguments
+ for (unsigned i = 0; i < 6; ++i)
+ {
+ if(i < al.size() - 2)
+ arg[i] = any_cast<int> (*++it);
+ else
+ arg[i] = 0;
+ }
+ if (!dryrun)
+ proto_.send(command, format.c_str(), arg[0], arg[1], arg[2],
+ arg[3], arg[4], arg[5]);
+ }
+};
+
+int
+main (int argc, char **argv)
+{
+ try
+ {
+ TestProto tp (argc, argv);
+ tp.run ();
+ }
+ catch (const std::exception &e)
+ {
+ std::cerr << e.what () << std::endl;
+ }
+}