// 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 #include #include /// 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) { bool reGet; // Récupération 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 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); sync(); } /// 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, 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); } } } 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) { send(command, format, a0, a1, a2, a3, 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); } 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(frame.args.size()) != argsFrameSize(format)) return false; // On décode et on envoie decodeArg(frame, format, a0, a1, a2, a3); 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*/) { serial_.wait (timeout); return sync (); } /// 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(d) << 4); revState_ = 3; } break; case 3: d = hex2digit (receivedChar); if (d == hexInvalid) erreur = true; else { currentFrame_.args.back() |= static_cast(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 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::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(arg)); break; case 'w': case 'W': frame.args.push_back(static_cast(arg >> 8)); frame.args.push_back(static_cast(arg)); break; case 'd': case 'D': frame.args.push_back(static_cast(arg >> 24)); frame.args.push_back(static_cast(arg >> 16)); frame.args.push_back(static_cast(arg >> 8)); frame.args.push_back(static_cast(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 temp[4]; int pos = 0; for(int i = 0; *format != '\0'; format++,i++) { switch(*format) { case 'b': temp[i] = static_cast(frame.args[pos]); pos++; break; case 'B': { int8_t t = static_cast(frame.args[pos]); temp[i] = static_cast(t); pos++; break; } case 'w': temp[i] = static_cast(frame.args[pos]) << 8 |static_cast(frame.args[pos + 1]); pos += 2; break; case 'W': { int8_t t = static_cast(frame.args[pos]); temp[i] = static_cast(t) << 8 |static_cast(frame.args[pos + 1]); pos += 2; break; } case 'd': temp[i] = static_cast(frame.args[pos]) << 24 |static_cast(frame.args[pos + 1]) << 16 |static_cast(frame.args[pos + 2]) << 8 |static_cast(frame.args[pos + 3]); pos += 4; break; case 'D': int8_t t = static_cast(frame.args[pos]); temp[i] = static_cast(t) << 24 |static_cast(frame.args[pos + 1]) << 16 |static_cast(frame.args[pos + 2]) << 8 |static_cast(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 (os, " ")); os << '>'; return os; }