Subversion Repositories HomeAutomation

Rev

Rev 1642 | Rev 1788 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | SVN | RSS feed

  1. /*
  2.  *
  3.  *  Copyright (C) 2010  Mattias Runge
  4.  *
  5.  *  This program is free software; you can redistribute it and/or modify
  6.  *  it under the terms of the GNU General Public License as published by
  7.  *  the Free Software Foundation; either version 2 of the License, or
  8.  *  (at your option) any later version.
  9.  *
  10.  *  This program is distributed in the hope that it will be useful,
  11.  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13.  *  GNU General Public License for more details.
  14.  *
  15.  *  You should have received a copy of the GNU General Public License along
  16.  *  with this program; if not, write to the Free Software Foundation, Inc.,
  17.  *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  18.  *
  19.  */
  20.  
  21. #include "Node.h"
  22.  
  23. #include <boost/date_time/posix_time/posix_time.hpp>
  24. #include <boost/lexical_cast.hpp>
  25.  
  26. #include "Manager.h"
  27. #include "can/Message.h"
  28. #include "broker/Manager.h"
  29.  
  30. namespace atom {
  31. namespace control {
  32.  
  33. Node::TransitionList Node::transitions_;
  34. std::map<Node::State, std::string> Node::state_names_;
  35. std::map<Node::Event, std::string> Node::event_names_;
  36.    
  37. Node::Node(Node::Id id) : LOG("control::Node")
  38. {
  39.     this->state_ = STATE_NORM_OFFLINE;
  40.     this->id_ = id;
  41.     this->current_offset_ = 0;
  42.     this->expected_ack_data_ = 0;
  43.     this->program_start_time_ = 0;
  44. }
  45.  
  46. Node::~Node()
  47. {
  48.  
  49. }
  50.  
  51. void Node::ConnectSlots(const SignalOnNewState::slot_type& slot_on_new_state)
  52. {
  53.     this->signal_on_new_state_.connect(slot_on_new_state);
  54. }
  55.  
  56. Node::Id Node::GetId()
  57. {
  58.     return this->id_;
  59. }
  60.  
  61. Node::State Node::GetState()
  62. {
  63.     return this->state_;
  64. }
  65.  
  66. void Node::SetupStateMachine()
  67. {
  68.     // Normal flow
  69.     Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_BIOS_START,        STATE_NORM_ONLINE);
  70.     Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_APP_START,         STATE_NORM_LIST);
  71.     Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_HEARTBEAT,         STATE_NORM_LIST);
  72.     Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_PROGRAM_BIOS,      STATE_BPGM_OFFLINE);
  73.     Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_PROGRAM_APP,       STATE_APGM_OFFLINE);
  74.     Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_RESET,             STATE_NO_CHANGE);
  75.    
  76.     Node::AddTransition(STATE_NORM_ONLINE,           EVENT_BIOS_START,        STATE_NORM_ONLINE);
  77.     Node::AddTransition(STATE_NORM_ONLINE,           EVENT_APP_START,         STATE_NORM_LIST);
  78.     Node::AddTransition(STATE_NORM_ONLINE,           EVENT_HEARTBEAT,         STATE_NORM_LIST);
  79.     Node::AddTransition(STATE_NORM_ONLINE,           EVENT_PROGRAM_BIOS,      STATE_BPGM_OFFLINE);
  80.     Node::AddTransition(STATE_NORM_ONLINE,           EVENT_PROGRAM_APP,       STATE_APGM_OFFLINE);
  81.     Node::AddTransition(STATE_NORM_ONLINE,           EVENT_RESET,             STATE_NORM_OFFLINE);
  82.    
  83.     Node::AddTransition(STATE_NORM_LIST,             EVENT_BIOS_START,        STATE_NORM_ONLINE);
  84.     Node::AddTransition(STATE_NORM_LIST,             EVENT_APP_START,         STATE_NORM_LIST);
  85.     Node::AddTransition(STATE_NORM_LIST,             EVENT_HEARTBEAT,         STATE_NO_CHANGE);
  86.     Node::AddTransition(STATE_NORM_LIST,             EVENT_LIST_DONE,         STATE_NORM_INITIALIZED);
  87.     Node::AddTransition(STATE_NORM_LIST,             EVENT_PROGRAM_BIOS,      STATE_BPGM_OFFLINE);
  88.     Node::AddTransition(STATE_NORM_LIST,             EVENT_PROGRAM_APP,       STATE_APGM_OFFLINE);
  89.     Node::AddTransition(STATE_NORM_LIST,             EVENT_RESET,             STATE_NORM_OFFLINE);
  90.    
  91.     Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_BIOS_START,        STATE_NORM_ONLINE);
  92.     Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_APP_START,         STATE_NORM_LIST);
  93.     Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_HEARTBEAT,         STATE_NO_CHANGE);
  94.     Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_PROGRAM_BIOS,      STATE_BPGM_OFFLINE);
  95.     Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_PROGRAM_APP,       STATE_APGM_OFFLINE);
  96.     Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_RESET,             STATE_NORM_OFFLINE);
  97.    
  98.    
  99.     // Program bios flow
  100.     Node::AddTransition(STATE_BPGM_OFFLINE,          EVENT_BIOS_START,        STATE_BPGM_START);
  101.    
  102.     Node::AddTransition(STATE_BPGM_START,            EVENT_PGM_ACK,           STATE_BPGM_DATA);
  103.     Node::AddTransition(STATE_BPGM_START,            EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
  104.  
  105.     Node::AddTransition(STATE_BPGM_DATA,             EVENT_PGM_ACK,           STATE_BPGM_DATA); // If there is no more data to send STATE_PGM_END will be set instead
  106.     Node::AddTransition(STATE_BPGM_DATA,             EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
  107.    
  108.     Node::AddTransition(STATE_BPGM_END,              EVENT_PGM_ACK,           STATE_BPGM_COPY);
  109.     Node::AddTransition(STATE_BPGM_END,              EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
  110.    
  111.     Node::AddTransition(STATE_BPGM_COPY,             EVENT_BIOS_START,        STATE_APGM_OFFLINE); // Remove application after bios upgrade
  112.        
  113.    
  114.     // Program application flow
  115.     Node::AddTransition(STATE_APGM_OFFLINE,          EVENT_BIOS_START,        STATE_APGM_START);
  116.    
  117.     Node::AddTransition(STATE_APGM_START,            EVENT_PGM_ACK,           STATE_APGM_DATA);
  118.     Node::AddTransition(STATE_APGM_START,            EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
  119.    
  120.     Node::AddTransition(STATE_APGM_DATA,             EVENT_PGM_ACK,           STATE_APGM_DATA); // If there is no more data to send STATE_PGM_END will be set instead
  121.     Node::AddTransition(STATE_APGM_DATA,             EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
  122.    
  123.     Node::AddTransition(STATE_APGM_END,              EVENT_PGM_ACK,           STATE_NORM_OFFLINE); // Return to normal flow
  124.     Node::AddTransition(STATE_APGM_END,              EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
  125.    
  126.    
  127.     Node::state_names_[STATE_INVALID] = "STATE_INVALID";
  128.     Node::state_names_[STATE_NO_CHANGE] = "STATE_NO_CHANGE";
  129.     Node::state_names_[STATE_NORM_OFFLINE] = "STATE_NORM_OFFLINE";
  130.     Node::state_names_[STATE_NORM_ONLINE] = "STATE_NORM_ONLINE";
  131.     Node::state_names_[STATE_NORM_LIST] = "STATE_NORM_LIST";
  132.     Node::state_names_[STATE_NORM_INITIALIZED] = "STATE_NORM_INITIALIZED";
  133.     Node::state_names_[STATE_BPGM_OFFLINE] = "STATE_BPGM_OFFLINE";
  134.     Node::state_names_[STATE_BPGM_START] = "STATE_BPGM_START";
  135.     Node::state_names_[STATE_BPGM_DATA] = "STATE_BPGM_DATA";
  136.     Node::state_names_[STATE_BPGM_END] = "STATE_BPGM_END";
  137.     Node::state_names_[STATE_BPGM_COPY] = "STATE_BPGM_COPY";
  138.     Node::state_names_[STATE_APGM_OFFLINE] = "STATE_APGM_OFFLINE";
  139.     Node::state_names_[STATE_APGM_START] = "STATE_APGM_START";
  140.     Node::state_names_[STATE_APGM_DATA] = "STATE_APGM_DATA";
  141.     Node::state_names_[STATE_APGM_END] = "STATE_APGM_END";
  142.    
  143.     Node::event_names_[EVENT_BIOS_START] = "EVENT_BIOS_START";
  144.     Node::event_names_[EVENT_APP_START] = "EVENT_APP_START";
  145.     Node::event_names_[EVENT_HEARTBEAT] = "EVENT_HEARTBEAT";
  146.     Node::event_names_[EVENT_PGM_ACK] = "EVENT_PGM_ACK";
  147.     Node::event_names_[EVENT_PGM_NACK] = "EVENT_PGM_NACK";
  148.     Node::event_names_[EVENT_LIST_DONE] = "EVENT_LIST_DONE";
  149.     Node::event_names_[EVENT_PROGRAM_BIOS] = "EVENT_PROGRAM_BIOS";
  150.     Node::event_names_[EVENT_PROGRAM_APP] = "EVENT_PROGRAM_APP";
  151.     Node::event_names_[EVENT_RESET] = "EVENT_RESET";
  152. }
  153.  
  154. void Node::AddTransition(Node::State current_state, Node::Event event, State target_state)
  155. {
  156.     Node::transitions_[current_state][event] = target_state;
  157. }
  158.  
  159. Node::State Node::GetTransitionTarget(Node::Event event)
  160. {
  161.     Node::TransitionList::iterator it1 = Node::transitions_.find(this->state_);
  162.    
  163.     if (it1 == Node::transitions_.end())
  164.     {
  165.         LOG.Warning("No transitions found for current state " + boost::lexical_cast<std::string>(this->state_));
  166.         return STATE_INVALID;
  167.     }
  168.    
  169.     Node::Transition::iterator it2 = it1->second.find(event);
  170.    
  171.     if (it2 == it1->second.end())
  172.     {
  173.         LOG.Warning("No transitions found for current state " + boost::lexical_cast<std::string>(this->state_) + " for event " + boost::lexical_cast<std::string>(event));
  174.         return STATE_INVALID;
  175.     }
  176.    
  177.     return it2->second;
  178. }
  179.  
  180. void Node::Trigger(Node::Event event, common::StringMap variables)
  181. {
  182.     this->ResetTimeout();
  183.    
  184.     State target_state = this->GetTransitionTarget(event);
  185.    
  186.     LOG.Debug("Trigger called on " + this->id_ + ", current state: " + Node::state_names_[this->state_] + ", event: " + Node::event_names_[event] + ", target state: "  + Node::state_names_[target_state]);
  187.    
  188.     if (target_state == STATE_INVALID)
  189.     {
  190.         this->SendReset();
  191.         target_state = STATE_NORM_OFFLINE;
  192.     }
  193.     else if (target_state == STATE_NO_CHANGE)
  194.     {
  195.         return;
  196.     }
  197.    
  198.     if ((this->state_ == STATE_APGM_END || this->state_ == STATE_BPGM_END) && event == EVENT_PGM_ACK)
  199.     {
  200.         unsigned int checksum = boost::lexical_cast<unsigned int>(variables["Data"]);
  201.        
  202.         if (this->expected_ack_data_ != checksum)
  203.         {
  204.             LOG.Warning("Checksum received in ACK was incorrect got " + boost::lexical_cast<std::string>(checksum) + " expected " + boost::lexical_cast<std::string>(this->expected_ack_data_) + ", aborting...");
  205.         }
  206.         else if (this->state_ == STATE_BPGM_END)
  207.         {
  208.             float speed = (float)this->code_->GetLength() / (float)(time(NULL) - this->program_start_time_);
  209.            
  210.             LOG.Info("Data was successfully transferred to " + this->id_ + ", speed was " + boost::lexical_cast<std::string>(speed) + " B/s");
  211.            
  212.             LOG.Debug("Sending application programming copy to node " + this->id_);
  213.             can::Message* payload = new can::Message("nmt", "", "", 0, "Pgm_Copy");
  214.            
  215.             unsigned int source0 = GET_LOW_BYTE_16(this->start_offset_);
  216.             unsigned int source1 = GET_HIGH_BYTE_16(this->start_offset_);
  217.            
  218.             unsigned int destination0 = GET_LOW_BYTE_16(this->code_->GetAddressLower());
  219.             unsigned int destination1 = GET_HIGH_BYTE_16(this->code_->GetAddressLower());
  220.            
  221.             unsigned int length0 = GET_LOW_BYTE_16(this->code_->GetLength());
  222.             unsigned int length1 = GET_HIGH_BYTE_16(this->code_->GetLength());
  223.            
  224.             payload->SetVariable("Source0", boost::lexical_cast<std::string>(source0));
  225.             payload->SetVariable("Source1", boost::lexical_cast<std::string>(source1));
  226.             payload->SetVariable("Destination0", boost::lexical_cast<std::string>(destination0));
  227.             payload->SetVariable("Destination1", boost::lexical_cast<std::string>(destination1));
  228.             payload->SetVariable("Length0", boost::lexical_cast<std::string>(length0));
  229.             payload->SetVariable("Length1", boost::lexical_cast<std::string>(length1));
  230.            
  231.             broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
  232.         }
  233.         else
  234.         {
  235.             float speed = (float)this->code_->GetLength() / (float)(time(NULL) - this->program_start_time_);
  236.            
  237.             LOG.Info("Programming was completed successfully on " + this->id_ + ", speed was " + boost::lexical_cast<std::string>(speed) + " B/s");
  238.         }
  239.     }
  240.    
  241.     if (target_state == STATE_NORM_LIST)
  242.     {
  243.         this->SendListRequest();
  244.     }
  245.     else if (target_state == STATE_BPGM_OFFLINE || target_state == STATE_APGM_OFFLINE)
  246.     {
  247.         if (event != EVENT_BIOS_START)
  248.         {
  249.             this->SendReset();
  250.         }
  251.     }
  252.     else if (target_state == STATE_APGM_START || target_state == STATE_BPGM_START)
  253.     {
  254.         if (!this->code_->IsValid())
  255.         {
  256.             LOG.Error("Code is not valid, aborting...");
  257.             target_state = STATE_NORM_OFFLINE;
  258.         }
  259.         else
  260.         {
  261.             LOG.Debug("Sending programming start to node " + this->id_);
  262.             can::Message* payload = new can::Message("nmt", "", "", 0, "Pgm_Start");
  263.             payload->SetVariable("HardwareId", boost::lexical_cast<std::string>(common::FromHex(this->id_)));
  264.            
  265.             this->start_offset_ = target_state == STATE_BPGM_START ? 0 : this->code_->GetAddressLower();
  266.            
  267.             unsigned int address0 = GET_LOW_BYTE_16(this->start_offset_);
  268.             unsigned int address1 = GET_HIGH_BYTE_16(this->start_offset_);
  269.            
  270.             this->expected_ack_data_ = SWAP_BYTE_ORDER_16(this->start_offset_);
  271.            
  272.             payload->SetVariable("Address0", boost::lexical_cast<std::string>(address0));
  273.             payload->SetVariable("Address1", boost::lexical_cast<std::string>(address1));
  274.             payload->SetVariable("Address2", "0"); // Not used?
  275.             payload->SetVariable("Address3", "0"); // Not used?
  276.            
  277.             this->current_offset_ = 0;
  278.            
  279.             broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
  280.            
  281.             this->program_start_time_ = time(NULL);
  282.         }
  283.     }
  284.     else if (target_state == STATE_APGM_DATA || target_state == STATE_BPGM_DATA)
  285.     {
  286.         unsigned int offset = boost::lexical_cast<unsigned int>(variables["Data"]);
  287.        
  288.         if ( this->expected_ack_data_ != offset)
  289.         {
  290.             LOG.Warning("Offset received in ACK was incorrect got " + boost::lexical_cast<std::string>(offset) + " expected " + boost::lexical_cast<std::string>(this->expected_ack_data_) + ", aborting...");
  291.             target_state = STATE_NORM_OFFLINE;
  292.         }
  293.         else if (this->current_offset_ >= this->code_->GetLength())
  294.         {
  295.             LOG.Debug("Sending programming end to node " + this->id_);
  296.            
  297.             unsigned int checksum = this->code_->GetChecksum();
  298.             this->expected_ack_data_ = SWAP_BYTE_ORDER_16(checksum);
  299.            
  300.             can::Message* payload = new can::Message("nmt", "", "", 0, "Pgm_End");
  301.             broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
  302.            
  303.             target_state = target_state == STATE_BPGM_DATA ? STATE_BPGM_END : STATE_APGM_END;
  304.         }
  305.         else
  306.         {
  307.             LOG.Debug("Sending programming data to node " + this->id_);
  308.             can::Message* payload = new can::Message("nmt", "", "", 0, "Pgm_Data_48");
  309.            
  310.             unsigned int offset0 = GET_LOW_BYTE_16(this->start_offset_ + this->current_offset_);
  311.             unsigned int offset1 = GET_HIGH_BYTE_16(this->start_offset_ + this->current_offset_);
  312.            
  313.             this->expected_ack_data_ = SWAP_BYTE_ORDER_16(this->start_offset_ + this->current_offset_);
  314.            
  315.             payload->SetVariable("Offset0", boost::lexical_cast<std::string>(offset0));
  316.             payload->SetVariable("Offset1", boost::lexical_cast<std::string>(offset1));
  317.            
  318.             for (int n = 0; (this->current_offset_ < this->code_->GetLength()) && (n < 6); n++)
  319.             {
  320.                 //LOG.Debug("Data" + boost::lexical_cast<std::string>(n) + "=" +  boost::lexical_cast<std::string>((unsigned int)this->code_->GetByte(this->code_->GetAddressLower() + this->current_offset_)));
  321.                 payload->SetVariable("Data" + boost::lexical_cast<std::string>(n), boost::lexical_cast<std::string>((unsigned int)this->code_->GetByte(this->code_->GetAddressLower() + this->current_offset_)));
  322.                 this->current_offset_++;
  323.             }
  324.            
  325.             broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
  326.         }
  327.     }
  328.     else if (target_state == STATE_BPGM_COPY)
  329.     {
  330.         LOG.Debug("Preparing to send null application to node " + this->id_);
  331.        
  332.         this->code_->Reset();
  333.        
  334.         this->code_->AddByte(0xFF);
  335.         this->code_->AddByte(0xFF);
  336.     }
  337.     else if (target_state == STATE_NORM_OFFLINE)
  338.     {
  339.         this->SendReset();
  340.     }
  341.    
  342.    
  343.     if (this->state_ != target_state)
  344.     {
  345.         this->signal_on_new_state_(this->id_, this->state_, target_state);
  346.     }
  347.    
  348.     this->state_ = target_state;
  349. }
  350.  
  351. bool Node::CheckTimeout()
  352. {
  353.     if (this->state_ == STATE_NORM_OFFLINE)
  354.     {
  355.         return true;
  356.     }
  357.    
  358.     if (this->code_.use_count() == 0 && this->last_active_ + 10 < time(NULL))
  359.     {
  360.         this->state_ = STATE_NORM_OFFLINE;
  361.         return false;
  362.     }
  363.    
  364.     return true;
  365. }
  366.  
  367. void Node::ResetTimeout()
  368. {
  369.     this->last_active_ = time(NULL);
  370. }
  371.  
  372. void Node::Reset()
  373. {
  374.     common::StringMap variables;
  375.    
  376.     this->Trigger(EVENT_RESET, variables);
  377. }
  378.  
  379. void Node::ProgramApplication(Code::Pointer code)
  380. {
  381.     this->code_ = code;
  382.    
  383.     common::StringMap variables;
  384.    
  385.     this->Trigger(EVENT_PROGRAM_APP, variables);
  386. }
  387.  
  388. void Node::ProgramBios(Code::Pointer code)
  389. {
  390.     this->code_ = code;
  391.    
  392.     common::StringMap variables;
  393.    
  394.     this->Trigger(EVENT_PROGRAM_BIOS, variables);
  395. }
  396.  
  397. void Node::SendReset()
  398. {
  399.     LOG.Info("Sending node reset to " + this->id_);
  400.     can::Message* payload = new can::Message("nmt", "", "", 0, "Reset");
  401.     payload->SetVariable("HardwareId", boost::lexical_cast<std::string>(common::FromHex(this->id_)));
  402.    
  403.     broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
  404. }
  405.  
  406. void Node::SendListRequest()
  407. {
  408.     LOG.Debug("Sending module listing on node " + this->id_);
  409.     can::Message* payload = new can::Message("mnmt", "To_Owner", "", 0, "List");
  410.     payload->SetVariable("HardwareId", boost::lexical_cast<std::string>(common::FromHex(this->id_)));
  411.    
  412.     broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
  413. }
  414.  
  415. }; // namespace control
  416. }; // namespace atom
  417.