// 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" /// Constructeur. Proto::Proto(Receiver &receiver) :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 = true; //Récupération de la frame while(reGet) { if(reGet = getFrame()) { //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 fiable) { if(fiable) { 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 fiable) { // 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,fiable); } /// permet d'envoyer un packet pas fiable void Proto::send_pas_fiable (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; //tant que le tampon n'est pas vide, on teste while((receivedChar = serial_.getchar()) != -1) { //si la donnée n'est pas erronnée if(receivedChar != 0xff) // XXX Heu, c'est vraiment ça????? { //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 == '\n' && revState_ == 2) { revState_ = 0; return true; } //Pour les autres charactères //Si on attend la commande else switch(revState_) { case 1: currentFrame_.command = receivedChar; revState_ = 2; break; case 2: // XXX Bite de poids fort à gauche dans la frame?? currentFrame_.args.push_back(static_cast( hex2digit( receivedChar )) << 4); revState_ = 3; break; case 3: currentFrame_.args.back() |= static_cast( hex2digit( receivedChar )); revState_ = 2; break; } //Si revState == 0 alors on jette } } return false; } /// Envoie la frame dans l'AVR void Proto::sendFrame(const Frame & frame) { //envoyer le bang serial_.putchar('!'); //Envoyer la commande serial_.putchar(digit2hex(frame.command >> 4)); serial_.putchar(digit2hex(frame.command & 0x0f)); //Envoyer les arguments for(int i = 0; i < static_cast(frame.args.size()); i++) { serial_.putchar(digit2hex(frame.args[i] >> 4)); serial_.putchar(digit2hex(frame.args[i] & 0x0f)); } //Envoyer le retour chariot serial_.putchar('\n'); //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; case 'w': case 'W': size += 1; case 'd': case 'D': size += 1; default: size += 1; } 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; }