// 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_ > 500) sendFrame(frameQueue_.front()); } return frameQueue_.empty(); } /// Envoie un packet void Proto::send (const Frame & Frame) { frameQueue_.push(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 (uint8_t command, const char *format, int a0, int a1, int a2, int a3) { // Constitution de la frame Proto::Frame frame; int nbArg = strlen(format); frame.command = command; // Conversion et saisie des aguments if(nbArg == 1) newArgFrame(frame, format[0], a0); if (nbArg == 2) { newArgFrame(frame, format[0], a0); newArgFrame(frame, format[1], a1); } if (nbArg == 3) { newArgFrame(frame, format[0], a0); newArgFrame(frame, format[1], a1); newArgFrame(frame, format[2], a2); } if (nbArg == 4) { newArgFrame(frame, format[0], a0); newArgFrame(frame, format[1], a1); newArgFrame(frame, format[2], a2); newArgFrame(frame, format[3], a3); } send(frame); } bool Proto::decode (const Proto::Frame &frame) { //On teste si des arguments sont présents(ca serait pas bon) if(!frame.args.empty()) return false; else return true; } bool Proto::decode (const Frame &frame, const char *format, int &a0) { // On teste le format de la frame if(!verifyFrame(frame, format, 1)) return false; //On décode et on envoie a0 = decodeArg(frame, format, 0); return true; } bool Proto::decode (const Frame &frame, const char *format, int &a0, int &a1) { // On vérifie le format de la frame if(!verifyFrame(frame, format, 2)) return false; // On décode et on envoie a0 = decodeArg(frame, format, 0); a1 = decodeArg(frame, format, 1); return true; } bool Proto::decode (const Frame &frame, const char *format, int &a0, int &a1, int &a2) { // On vérifie le format de la frame if(!verifyFrame(frame, format, 3)) return false; // On décode et on envoie a0 = decodeArg(frame, format, 0); a1 = decodeArg(frame, format, 1); a2 = decodeArg(frame, format, 2); return true; } bool Proto::decode (const Frame &frame, const char *format, int &a0, int &a1, int &a2, int &a3) { // On vérifie le format de la frame if(!verifyFrame(frame, format, 3)) return false; // On décode et on envoie a0 = decodeArg(frame, format, 0); a1 = decodeArg(frame, format, 1); a2 = decodeArg(frame, format, 2); a3 = decodeArg(frame, format, 3); return true; } /// 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) { //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 if(receivedChar == '\n' && revState_ == 3) { revState_ = 0; return true; } //Pour les autres charactères //Si on attend la commande else if(revState_ == 1) { currentFrame_.command = ((uint8_t)hex2digit( receivedChar )) << 4; revState_ = 2; } else if(revState_ == 2) { currentFrame_.command |= (uint8_t)hex2digit( receivedChar ); revState_ = 3; } else if(revState_ == 3) { currentFrame_.args.push_back(((uint8_t) hex2digit( receivedChar )) << 4); revState_ = 3; } //Si revState == 0 alors on jette } } return false; } 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 < (int)frame.args.size(); i++) //le cast est pour virer un warning { 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(); } void Proto::newArgFrame(Proto::Frame & frame, char format, int arg) { switch(format) { case 'b': case 'B': frame.args.push_back((uint8_t)arg); break; case 'w': case 'W': frame.args.push_back((uint8_t)(arg >> 8)); frame.args.push_back((uint8_t)arg); break; case 'd': case 'D': frame.args.push_back((uint8_t)(arg >> 24)); frame.args.push_back((uint8_t)(arg >> 16)); frame.args.push_back((uint8_t)(arg >> 8)); frame.args.push_back((uint8_t)arg); break; } } int Proto::argsFrameSize(const char *format, int nbArg) { int size = 0; if(nbArg == 0) nbArg = strlen(format); for(int i = 0; i < nbArg; i++) size += argSize(format[i]); return size; } int Proto::argSize(char format) { switch(format) { case 'b': case 'B': return 1; case 'w': case 'W': return 2; case 'd': case 'D': return 4; default: return 0; } } bool Proto::verifyFrame(const Frame &frame, const char *format, int nbArg) { //Teste si il y a bien le bon nombre d'argument if ((int)strlen(format) != nbArg)//Un cast pour shooter un warning return false; if ((int)frame.args.size() != argsFrameSize(format))//un cast pour virer un warning return false; //Voir pour des test plus approffondi si possible et necessaire return true; } int Proto::decodeArg(const Frame & frame, const char *format, int numArg) { int argDecoded = 0; int beginArg = argsFrameSize(format, numArg); switch(format[numArg]) { case 'b': argDecoded = (int)frame.args[beginArg]; break; case 'B': { int8_t temp = (int8_t)frame.args[beginArg]; argDecoded = (int) temp; break; } case 'w': argDecoded = (int)frame.args[beginArg] << 8 |(int)frame.args[beginArg + 1]; break; case 'W': { int8_t temp1 = (int8_t)frame.args[beginArg]; argDecoded = (int)temp1 << 8 |(int)frame.args[beginArg + 1]; break; } case 'd': argDecoded = (int)frame.args[beginArg] << 24 |(int)frame.args[beginArg + 1] << 16 |(int)frame.args[beginArg + 2] << 8 |(int)frame.args[beginArg + 3]; break; case 'D': int8_t temp1 = (int8_t)frame.args[beginArg]; argDecoded = (int)temp1 << 24 |(int)frame.args[beginArg + 1] << 16 |(int)frame.args[beginArg + 2] << 8 |(int)frame.args[beginArg + 3]; break; } return argDecoded; } bool Proto::Frame::operator==(const Frame& frame) { return this->command == frame.command && this->args == frame.args; }