Subversion Repositories HomeAutomation

Rev

Rev 1911 | Rev 2037 | 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_ = boost::algorithm::to_lower_copy((std::string)id);
  41.     //this->id_ = boost::algorithm::to_upper_copy((std::string)id);
  42.     this->id_ = (std::string)id;
  43.     this->information_.has_application_ = false;
  44.     this->information_.bios_version_ = 0;
  45.     this->information_.device_type_ = "N/A";
  46.     this->information_.last_active_ = 0;
  47.     this->information_.valid_ = false;
  48.    
  49.     this->current_offset_ = 0;
  50.     this->expected_ack_data_ = 0;
  51.     this->program_start_time_ = 0;
  52. }
  53.  
  54. Node::~Node()
  55. {
  56.  
  57. }
  58.  
  59. void Node::ConnectSlots(const SignalOnNewState::slot_type& slot_on_new_state)
  60. {
  61.     this->signal_on_new_state_.connect(slot_on_new_state);
  62. }
  63.  
  64. Node::Id Node::GetId()
  65. {
  66.     return this->id_;
  67. }
  68.  
  69. Node::Information Node::GetInformation()
  70. {
  71.     return this->information_;
  72. }
  73.  
  74. void Node::SetupStateMachine()
  75. {
  76.     // Normal flow
  77.     Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_BIOS_START,        STATE_NORM_ONLINE);
  78.     Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_APP_START,         STATE_NORM_LIST);
  79.     Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_HEARTBEAT,         STATE_NORM_LIST);
  80.     Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_PROGRAM_BIOS,      STATE_BPGM_OFFLINE);
  81.     Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_PROGRAM_APP,       STATE_APGM_OFFLINE);
  82.     Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_RESET,             STATE_NO_CHANGE);
  83.    
  84.     Node::AddTransition(STATE_NORM_ONLINE,           EVENT_BIOS_START,        STATE_NORM_ONLINE);
  85.     Node::AddTransition(STATE_NORM_ONLINE,           EVENT_APP_START,         STATE_NORM_LIST);
  86.     Node::AddTransition(STATE_NORM_ONLINE,           EVENT_HEARTBEAT,         STATE_NORM_LIST);
  87.     Node::AddTransition(STATE_NORM_ONLINE,           EVENT_PROGRAM_BIOS,      STATE_BPGM_OFFLINE);
  88.     Node::AddTransition(STATE_NORM_ONLINE,           EVENT_PROGRAM_APP,       STATE_APGM_OFFLINE);
  89.     Node::AddTransition(STATE_NORM_ONLINE,           EVENT_RESET,             STATE_NORM_OFFLINE);
  90.    
  91.     Node::AddTransition(STATE_NORM_LIST,             EVENT_BIOS_START,        STATE_NORM_ONLINE);
  92.     Node::AddTransition(STATE_NORM_LIST,             EVENT_APP_START,         STATE_NORM_LIST);
  93.     Node::AddTransition(STATE_NORM_LIST,             EVENT_HEARTBEAT,         STATE_NO_CHANGE);
  94.     Node::AddTransition(STATE_NORM_LIST,             EVENT_LIST_DONE,         STATE_NORM_INITIALIZED);
  95.     Node::AddTransition(STATE_NORM_LIST,             EVENT_PROGRAM_BIOS,      STATE_BPGM_OFFLINE);
  96.     Node::AddTransition(STATE_NORM_LIST,             EVENT_PROGRAM_APP,       STATE_APGM_OFFLINE);
  97.     Node::AddTransition(STATE_NORM_LIST,             EVENT_RESET,             STATE_NORM_OFFLINE);
  98.    
  99.     Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_BIOS_START,        STATE_NORM_ONLINE);
  100.     Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_APP_START,         STATE_NORM_LIST);
  101.     Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_HEARTBEAT,         STATE_NO_CHANGE);
  102.     Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_PROGRAM_BIOS,      STATE_BPGM_OFFLINE);
  103.     Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_PROGRAM_APP,       STATE_APGM_OFFLINE);
  104.     Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_RESET,             STATE_NORM_OFFLINE);
  105.    
  106.    
  107.     // Program bios flow
  108.     Node::AddTransition(STATE_BPGM_OFFLINE,          EVENT_BIOS_START,        STATE_BPGM_START);
  109.    
  110.     Node::AddTransition(STATE_BPGM_START,            EVENT_PGM_ACK,           STATE_BPGM_DATA);
  111.     Node::AddTransition(STATE_BPGM_START,            EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
  112.  
  113.     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
  114.     Node::AddTransition(STATE_BPGM_DATA,             EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
  115.    
  116.     Node::AddTransition(STATE_BPGM_END,              EVENT_PGM_ACK,           STATE_BPGM_COPY);
  117.     Node::AddTransition(STATE_BPGM_END,              EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
  118.    
  119.     Node::AddTransition(STATE_BPGM_COPY,             EVENT_BIOS_START,        STATE_APGM_OFFLINE); // Remove application after bios upgrade
  120.        
  121.    
  122.     // Program application flow
  123.     Node::AddTransition(STATE_APGM_OFFLINE,          EVENT_BIOS_START,        STATE_APGM_START);
  124.    
  125.     Node::AddTransition(STATE_APGM_START,            EVENT_PGM_ACK,           STATE_APGM_DATA);
  126.     Node::AddTransition(STATE_APGM_START,            EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
  127.    
  128.     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
  129.     Node::AddTransition(STATE_APGM_DATA,             EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
  130.    
  131.     Node::AddTransition(STATE_APGM_END,              EVENT_PGM_ACK,           STATE_NORM_OFFLINE); // Return to normal flow
  132.     Node::AddTransition(STATE_APGM_END,              EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
  133.    
  134.    
  135.     Node::state_names_[STATE_INVALID] = "STATE_INVALID";
  136.     Node::state_names_[STATE_NO_CHANGE] = "STATE_NO_CHANGE";
  137.     Node::state_names_[STATE_NORM_OFFLINE] = "STATE_NORM_OFFLINE";
  138.     Node::state_names_[STATE_NORM_ONLINE] = "STATE_NORM_ONLINE";
  139.     Node::state_names_[STATE_NORM_LIST] = "STATE_NORM_LIST";
  140.     Node::state_names_[STATE_NORM_INITIALIZED] = "STATE_NORM_INITIALIZED";
  141.     Node::state_names_[STATE_BPGM_OFFLINE] = "STATE_BPGM_OFFLINE";
  142.     Node::state_names_[STATE_BPGM_START] = "STATE_BPGM_START";
  143.     Node::state_names_[STATE_BPGM_DATA] = "STATE_BPGM_DATA";
  144.     Node::state_names_[STATE_BPGM_END] = "STATE_BPGM_END";
  145.     Node::state_names_[STATE_BPGM_COPY] = "STATE_BPGM_COPY";
  146.     Node::state_names_[STATE_APGM_OFFLINE] = "STATE_APGM_OFFLINE";
  147.     Node::state_names_[STATE_APGM_START] = "STATE_APGM_START";
  148.     Node::state_names_[STATE_APGM_DATA] = "STATE_APGM_DATA";
  149.     Node::state_names_[STATE_APGM_END] = "STATE_APGM_END";
  150.    
  151.     Node::event_names_[EVENT_BIOS_START] = "EVENT_BIOS_START";
  152.     Node::event_names_[EVENT_APP_START] = "EVENT_APP_START";
  153.     Node::event_names_[EVENT_HEARTBEAT] = "EVENT_HEARTBEAT";
  154.     Node::event_names_[EVENT_PGM_ACK] = "EVENT_PGM_ACK";
  155.     Node::event_names_[EVENT_PGM_NACK] = "EVENT_PGM_NACK";
  156.     Node::event_names_[EVENT_LIST_DONE] = "EVENT_LIST_DONE";
  157.     Node::event_names_[EVENT_PROGRAM_BIOS] = "EVENT_PROGRAM_BIOS";
  158.     Node::event_names_[EVENT_PROGRAM_APP] = "EVENT_PROGRAM_APP";
  159.     Node::event_names_[EVENT_RESET] = "EVENT_RESET";
  160. }
  161.  
  162. void Node::AddTransition(Node::State current_state, Node::Event event, State target_state)
  163. {
  164.     Node::transitions_[current_state][event] = target_state;
  165. }
  166.  
  167. Node::State Node::GetTransitionTarget(Node::Event event)
  168. {
  169.     Node::TransitionList::iterator it1 = Node::transitions_.find(this->state_);
  170.    
  171.     if (it1 == Node::transitions_.end())
  172.     {
  173.         LOG.Warning("No transitions found for current state " + boost::lexical_cast<std::string>(this->state_));
  174.         return STATE_INVALID;
  175.     }
  176.    
  177.     Node::Transition::iterator it2 = it1->second.find(event);
  178.    
  179.     if (it2 == it1->second.end())
  180.     {
  181.         LOG.Warning("No transitions found for current state " + boost::lexical_cast<std::string>(this->state_) + " for event " + boost::lexical_cast<std::string>(event));
  182.         return STATE_INVALID;
  183.     }
  184.    
  185.     return it2->second;
  186. }
  187.  
  188. void Node::Trigger(Node::Event event, common::StringMap variables)
  189. {
  190.     this->ResetTimeout();
  191.    
  192.     State target_state = this->GetTransitionTarget(event);
  193.    
  194.     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]);
  195.    
  196.     if (event == EVENT_BIOS_START)
  197.     {
  198.         this->information_.has_application_ = boost::lexical_cast<int>(variables["HasApplication"]) > 0;
  199.         this->information_.bios_version_ = boost::lexical_cast<unsigned int>(variables["BiosVersion"]);
  200.         this->information_.device_type_ = variables["DeviceType"];
  201.         this->information_.valid_ = true;
  202.     }
  203.    
  204.     if (target_state == STATE_INVALID)
  205.     {
  206.         this->SendReset();
  207.         target_state = STATE_NORM_OFFLINE;
  208.     }
  209.     else if (target_state == STATE_NO_CHANGE)
  210.     {
  211.         return;
  212.     }
  213.    
  214.     if ((this->state_ == STATE_APGM_END || this->state_ == STATE_BPGM_END) && event == EVENT_PGM_ACK)
  215.     {
  216.         unsigned int checksum = boost::lexical_cast<unsigned int>(variables["Data"]);
  217.        
  218.         if (this->expected_ack_data_ != checksum)
  219.         {
  220.             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...");
  221.  
  222.             this->SendReset();
  223.  
  224.             target_state = STATE_NORM_OFFLINE;
  225.         }
  226.         else if (this->state_ == STATE_BPGM_END)
  227.         {
  228.             float speed = (float)this->code_->GetLength() / (float)(time(NULL) - this->program_start_time_);
  229.            
  230.             LOG.Info("Data was successfully transferred to " + this->id_ + ", speed was " + boost::lexical_cast<std::string>(speed) + " B/s");
  231.            
  232.             LOG.Debug("Sending application programming copy to node " + this->id_);
  233.             can::Message* payload = new can::Message("nmt", "", "", 0, "Pgm_Copy");
  234.            
  235.             unsigned int source0 = GET_LOW_BYTE_16(this->start_offset_);
  236.             unsigned int source1 = GET_HIGH_BYTE_16(this->start_offset_);
  237.            
  238.             unsigned int destination0 = GET_LOW_BYTE_16(this->code_->GetAddressLower());
  239.             unsigned int destination1 = GET_HIGH_BYTE_16(this->code_->GetAddressLower());
  240.            
  241.             unsigned int length0 = GET_LOW_BYTE_16(this->code_->GetLength());
  242.             unsigned int length1 = GET_HIGH_BYTE_16(this->code_->GetLength());
  243.            
  244.             payload->SetVariable("Source0", boost::lexical_cast<std::string>(source0));
  245.             payload->SetVariable("Source1", boost::lexical_cast<std::string>(source1));
  246.             payload->SetVariable("Destination0", boost::lexical_cast<std::string>(destination0));
  247.             payload->SetVariable("Destination1", boost::lexical_cast<std::string>(destination1));
  248.             payload->SetVariable("Length0", boost::lexical_cast<std::string>(length0));
  249.             payload->SetVariable("Length1", boost::lexical_cast<std::string>(length1));
  250.            
  251.             broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
  252.         }
  253.         else
  254.         {
  255.             float speed = (float)this->code_->GetLength() / (float)(time(NULL) - this->program_start_time_);
  256.            
  257.             LOG.Info("Programming was completed successfully on " + this->id_ + ", speed was " + boost::lexical_cast<std::string>(speed) + " B/s");
  258.         }
  259.     }
  260.    
  261.     if (target_state == STATE_NORM_LIST)
  262.     {
  263.         this->SendListRequest();
  264.     }
  265.     else if (target_state == STATE_BPGM_OFFLINE || target_state == STATE_APGM_OFFLINE)
  266.     {
  267.         if (event != EVENT_BIOS_START)
  268.         {
  269.             this->SendReset();
  270.         }
  271.     }
  272.     else if (target_state == STATE_APGM_START || target_state == STATE_BPGM_START)
  273.     {
  274.         if (!this->code_->IsValid())
  275.         {
  276.             LOG.Error("Code is not valid, aborting...");
  277.             target_state = STATE_NORM_OFFLINE;
  278.         }
  279.         else
  280.         {
  281.             LOG.Debug("Sending programming start to node " + this->id_);
  282.             can::Message* payload = new can::Message("nmt", "", "", 0, "Pgm_Start");
  283.             payload->SetVariable("HardwareId", boost::lexical_cast<std::string>(common::FromHex(this->id_)));
  284.            
  285.             this->start_offset_ = target_state == STATE_BPGM_START ? 0 : this->code_->GetAddressLower();
  286.            
  287.             unsigned int address0 = GET_LOW_BYTE_16(this->start_offset_);
  288.             unsigned int address1 = GET_HIGH_BYTE_16(this->start_offset_);
  289.            
  290.             this->expected_ack_data_ = SWAP_BYTE_ORDER_16(this->start_offset_);
  291.            
  292.             payload->SetVariable("Address0", boost::lexical_cast<std::string>(address0));
  293.             payload->SetVariable("Address1", boost::lexical_cast<std::string>(address1));
  294.             payload->SetVariable("Address2", "0"); // Not used?
  295.             payload->SetVariable("Address3", "0"); // Not used?
  296.            
  297.             this->current_offset_ = 0;
  298.            
  299.             broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
  300.            
  301.             this->program_start_time_ = time(NULL);
  302.         }
  303.     }
  304.     else if (target_state == STATE_APGM_DATA || target_state == STATE_BPGM_DATA)
  305.     {
  306.         unsigned int offset = boost::lexical_cast<unsigned int>(variables["Data"]);
  307.        
  308.         if ( this->expected_ack_data_ != offset)
  309.         {
  310.             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...");
  311.             target_state = STATE_NORM_OFFLINE;
  312.         }
  313.         else if (this->current_offset_ >= this->code_->GetLength())
  314.         {
  315.             LOG.Debug("Sending programming end to node " + this->id_);
  316.            
  317.             unsigned int checksum = this->code_->GetChecksum();
  318.             this->expected_ack_data_ = SWAP_BYTE_ORDER_16(checksum);
  319.            
  320.             can::Message* payload = new can::Message("nmt", "", "", 0, "Pgm_End");
  321.             broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
  322.            
  323.             target_state = target_state == STATE_BPGM_DATA ? STATE_BPGM_END : STATE_APGM_END;
  324.         }
  325.         else
  326.         {
  327.             LOG.Debug("Sending programming data to node " + this->id_);
  328.             can::Message* payload = new can::Message("nmt", "", "", 0, "Pgm_Data_48");
  329.            
  330.             unsigned int offset0 = GET_LOW_BYTE_16(this->start_offset_ + this->current_offset_);
  331.             unsigned int offset1 = GET_HIGH_BYTE_16(this->start_offset_ + this->current_offset_);
  332.            
  333.             this->expected_ack_data_ = SWAP_BYTE_ORDER_16(this->start_offset_ + this->current_offset_);
  334.            
  335.             payload->SetVariable("Offset0", boost::lexical_cast<std::string>(offset0));
  336.             payload->SetVariable("Offset1", boost::lexical_cast<std::string>(offset1));
  337.            
  338.             for (int n = 0; (this->current_offset_ < this->code_->GetLength()) && (n < 6); n++)
  339.             {
  340.                 //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_)));
  341.                 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_)));
  342.                 this->current_offset_++;
  343.             }
  344.            
  345.             broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
  346.         }
  347.     }
  348.     else if (target_state == STATE_BPGM_COPY)
  349.     {
  350.         LOG.Debug("Preparing to send null application to node " + this->id_);
  351.        
  352.         this->code_->Reset(false);
  353.        
  354.         this->code_->AddByte(0xFF);
  355.         this->code_->AddByte(0xFF);
  356.     }
  357.     else if (target_state == STATE_NORM_OFFLINE)
  358.     {
  359.         this->information_.valid_ = false;
  360.         this->SendReset();
  361.     }
  362.    
  363.    
  364.     if (this->state_ != target_state)
  365.     {
  366.         this->signal_on_new_state_(this->id_, this->state_, target_state);
  367.     }
  368.    
  369.     this->state_ = target_state;
  370. }
  371.  
  372. bool Node::CheckTimeout()
  373. {
  374.     if (this->state_ == STATE_NORM_OFFLINE)
  375.     {
  376.         return true;
  377.     }
  378.    
  379.     if (this->code_.use_count() == 0 && this->information_.last_active_ + 10 < time(NULL))
  380.     {
  381.         this->state_ = STATE_NORM_OFFLINE;
  382.         return false;
  383.     }
  384.    
  385.     return true;
  386. }
  387.  
  388. void Node::ResetTimeout()
  389. {
  390.     this->information_.last_active_ = time(NULL);
  391. }
  392.  
  393. void Node::Reset()
  394. {
  395.     common::StringMap variables;
  396.    
  397.     this->Trigger(EVENT_RESET, variables);
  398. }
  399.  
  400. void Node::ProgramApplication(Code::Pointer code)
  401. {
  402.     this->code_ = code;
  403.    
  404.     common::StringMap variables;
  405.    
  406.     this->Trigger(EVENT_PROGRAM_APP, variables);
  407. }
  408.  
  409. void Node::ProgramBios(Code::Pointer code)
  410. {
  411.     this->code_ = code;
  412.    
  413.     common::StringMap variables;
  414.    
  415.     this->Trigger(EVENT_PROGRAM_BIOS, variables);
  416. }
  417.  
  418. void Node::SendReset()
  419. {
  420.     LOG.Info("Sending node reset to " + this->id_);
  421.     can::Message* payload = new can::Message("nmt", "", "", 0, "Reset");
  422.     payload->SetVariable("HardwareId", boost::lexical_cast<std::string>(common::FromHex(this->id_)));
  423.    
  424.     broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
  425. }
  426.  
  427. void Node::SendListRequest()
  428. {
  429.     LOG.Debug("Sending module listing on node " + this->id_);
  430.     can::Message* payload = new can::Message("mnmt", "To_Owner", "", 0, "List");
  431.     payload->SetVariable("HardwareId", boost::lexical_cast<std::string>(common::FromHex(this->id_)));
  432.    
  433.     broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
  434. }
  435.  
  436. }; // namespace control
  437. }; // namespace atom
  438.