summaryrefslogtreecommitdiff
path: root/i/marvin/src/proto
diff options
context:
space:
mode:
authorhaller2005-11-17 11:10:17 +0000
committerhaller2005-11-17 11:10:17 +0000
commitda4a77ff094a0dc74414ec1de2cac08e8b7b60f4 (patch)
tree402c39246c164af44648fd98d2ea85500d58df58 /i/marvin/src/proto
parent164d8870bc058450e6967a594dd9033d088316af (diff)
Importation brut du code récupérable de robert
Diffstat (limited to 'i/marvin/src/proto')
-rw-r--r--i/marvin/src/proto/Makefile.defs8
-rw-r--r--i/marvin/src/proto/proto.cc433
-rw-r--r--i/marvin/src/proto/proto.hh135
-rw-r--r--i/marvin/src/proto/test_proto.cc124
4 files changed, 700 insertions, 0 deletions
diff --git a/i/marvin/src/proto/Makefile.defs b/i/marvin/src/proto/Makefile.defs
new file mode 100644
index 0000000..fa984bc
--- /dev/null
+++ b/i/marvin/src/proto/Makefile.defs
@@ -0,0 +1,8 @@
+PROGRAMS += test_proto
+
+proto_OBJECTS = proto.o
+
+test_proto_OBJECTS = test_proto.o $(proto_OBJECTS) $(utils_OBJECTS) \
+ $(serial_OBJECTS) $(timer_OBJECTS) $(log_OBJECTS)
+
+test_proto: $(test_proto_OBJECTS)
diff --git a/i/marvin/src/proto/proto.cc b/i/marvin/src/proto/proto.cc
new file mode 100644
index 0000000..a7ada32
--- /dev/null
+++ b/i/marvin/src/proto/proto.cc
@@ -0,0 +1,433 @@
+// 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 srie.
+void
+Proto::open(const std::string &ttyname)
+{
+ serial_.open(ttyname);
+}
+
+/// Ferme le port srie
+void
+Proto::close(void)
+{
+ serial_.close();
+}
+
+/// Teste si tout les packets ont t envoys et aquits, sinon, essaye de
+/// le faire.
+bool
+Proto::sync(void)
+{
+ bool reGet;
+ // Rcupration de la frame
+ while (reGet = getFrame())
+ {
+ //log_ ("recv") << "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 dpasse 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);
+ // XXX sync();
+}
+
+
+/// Envois un packet. COMMAND est la commande envoyer, FORMAT, donne le
+/// format et le nombre de paramtres ('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);
+}
+
+/// 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 dcode les valeurs. Utilise
+/// le mme format que send.
+bool
+Proto::decode (const Proto::Frame &frame)
+{
+ int dummy;
+ return decode(frame, "", 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);
+}
+
+bool
+Proto::decode (const Frame &frame, const char *format, int &a0, int &a1)
+{
+ int dummy;
+ return decode(frame, format, a0, a1, 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);
+}
+
+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
+ if (static_cast<int>(frame.args.size()) != argsFrameSize(format))
+ return false;
+ // On dcode et on envoie
+ decodeArg(frame, format, a0, a1, a2, a3);
+
+ return true;
+}
+
+/// Attend que des caractres soit disponible pendant un temps maximum en
+/// milliseconde et les traite. Renvois le rsultat 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_
+ return serial_.wait (timeout < timeout_ ? timeout : timeout_);
+}
+
+/// Rcupre le File Descriptor
+int Proto::getFd(void)
+{
+ return serial_.getFd();
+}
+
+/// Rcupre 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 reoit un bang
+ if(receivedChar == '!')
+ {
+ revState_ = 1;
+ currentFrame_.command = 0;
+ currentFrame_.args.clear();
+ }
+ // Si on reoit le retour chariot et que on reevait les args
+ else if(receivedChar == '\r' && revState_ == 2)
+ {
+ revState_ = 0;
+ return true;
+ }
+ // Pour les autres charactres
+ // Si on attend la commande
+ else
+ switch(revState_)
+ {
+ case 1:
+ if (!isalpha (receivedChar) || receivedChar == '?')
+ erreur = true;
+ else
+ {
+ currentFrame_.command = receivedChar;
+ revState_ = 2;
+ }
+ // On vrifie que le match n'est pas fini
+ 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 reu une erreur on renvoie
+ if(erreur)
+ {
+ // On renvoie en mettant le compteur 0, la commande sera
+ // renvoyer de retour sync
+ //log_("Erreur de reception") << "commande inconnue" << "";
+ tLastSend_ = 0;
+ revState_ = 0;
+ return false;
+ }
+ }
+ return false;
+}
+
+/// Envoie la frame dans l'AVR
+void
+Proto::sendFrame(const Frame & frame)
+{
+ //log_ ("send") << "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;
+}
+
+/// Dcode un argument
+void
+Proto::decodeArg(const Frame & frame, const char *format, int &a0, int &a1, int &a2, int &a3)
+{
+ int temp[4];
+ 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]);
+ break;
+ }
+ }
+ a0 = temp[0];
+ a1 = temp[1];
+ a2 = temp[2];
+ a3 = temp[3];
+}
+
+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/marvin/src/proto/proto.hh b/i/marvin/src/proto/proto.hh
new file mode 100644
index 0000000..11b1ae3
--- /dev/null
+++ b/i/marvin/src/proto/proto.hh
@@ -0,0 +1,135 @@
+#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 srie.
+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_ = 500;
+
+ /// Frame en cours de rception
+ Frame currentFrame_;
+ int revState_;
+ // Etat de la rception de la frame
+ // 0 - Rien reu
+ // 1 - Bang reu
+ // 2 - Commande reu & nouveau argument
+ // 3 - char 1 argument reu
+
+ public:
+ /// Constructeur.
+ Proto (Receiver &receiver);
+ /// Ouvre le port srie.
+ void open (const std::string &ttyname);
+ /// Ferme le port srie.
+ void close (void);
+ /// Teste si tout les packets ont t envoys et aquits, 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 paramtres ('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);
+ /// permet d'envoyer un packet robert
+ void Proto::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 dcode les valeurs. Utilise
+ /// le mme 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);
+ //@}
+ /// Attend que des caractres soit disponible pendant un temps maximum en
+ /// milliseconde et les traite. Renvois le rsultat de sync (). Attention,
+ /// fonction bloquante, n'utiliser que pour les tests.
+ bool wait (int timeout = -1);
+ /// Rcupre le File Descriptor
+ int getFd(void);
+
+ public:
+ /// Les clients de Proto doivent driver de Receiver.
+ class Receiver
+ {
+ public:
+ virtual ~Receiver(void) {};
+ /// Recoit un packet.
+ virtual void receive (char command, const Frame &frame) = 0;
+ };
+ private:
+ /// Rcupre 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);
+ /// Dcode un argument
+ static void decodeArg(const Frame & frame, const char *format, int &a0,
+ int &a1, int &a2, int &a3);
+};
+
+/// Affiche une frame.
+std::ostream &
+operator<< (std::ostream &os, const Proto::Frame &f);
+
+#endif // proto_hh
diff --git a/i/marvin/src/proto/test_proto.cc b/i/marvin/src/proto/test_proto.cc
new file mode 100644
index 0000000..9ec119d
--- /dev/null
+++ b/i/marvin/src/proto/test_proto.cc
@@ -0,0 +1,124 @@
+// test_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 "proto.hh"
+#include "timer/timer.hh"
+
+#include <iostream>
+#include <stdexcept>
+
+/// Classe de test pour proto.
+class TestProto : public Proto::Receiver
+{
+ void receive (char command, const Proto::Frame &frame)
+ {
+ std::cout << "received " << frame << std::endl;
+ }
+};
+
+/// Affiche un memo de suntaxe.
+void
+syntax (void)
+{
+ std::cout << "test_proto - test la classe de protocol srie.\n"
+ "Syntaxe : test_proto <tty> <...>\n"
+ " s <cmd> <args...> envois une commande\n"
+ " w <ms> attend pendant un nombre de millisecondes\n"
+ " ? affiche cet cran d'aide\n"
+ << std::endl;
+}
+
+int
+main (int argc, char **argv)
+{
+ try
+ {
+ int i;
+ if (argc < 2)
+ {
+ syntax ();
+ return 1;
+ }
+ TestProto testProto;
+ Proto proto (testProto);
+ proto.open (argv[1]);
+ i = 2;
+ while (i < argc)
+ {
+ bool reliable = true;
+ switch (argv[i][0])
+ {
+ case 'S':
+ reliable = false;
+ // no break;
+ case 's':
+ {
+ if (i + 2 >= argc)
+ throw std::runtime_error ("syntax error");
+ unsigned a;
+ int arg[4];
+ char c = argv[++i][0];
+ const char *format = argv[++i];
+ if (i + static_cast<int> (strlen (format)) >= argc)
+ throw std::runtime_error ("syntax error");
+ for (a = 0; a < 4 && a < strlen (format); a++)
+ arg[a] = atoi (argv[++i]);
+ proto.send (c, format, arg[0], arg[1], arg[2], arg[3],
+ reliable);
+ while (!proto.wait (-1))
+ ;
+ }
+ break;
+ case 'w':
+ {
+ int stop, t;
+ if (i + 1 >= argc)
+ throw std::runtime_error ("syntax error");
+ stop = atoi (argv[++i]) + Timer::getProgramTime ();
+ t = Timer::getProgramTime ();
+ while (t < stop)
+ {
+ proto.wait (stop - t);
+ t = Timer::getProgramTime ();
+ }
+ break;
+ }
+ case '?':
+ proto.close ();
+ syntax ();
+ return 0;
+ default:
+ throw std::runtime_error ("syntax error");
+ }
+ i++;
+ }
+ }
+ catch (const std::exception &e)
+ {
+ std::cerr << e.what () << std::endl;
+ syntax ();
+ return 1;
+ }
+ return 0;
+}