WebSVN – HomeAutomation – Diff – /trunk/PcSoftware/Atom1.5/src/control/Node.cpp


/*
 *
 *  Copyright (C) 2010  Mattias Runge
 *
 *  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.,
 *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 */
 
#include "Node.h"
 
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/lexical_cast.hpp>
 
#include "Manager.h"
#include "can/Message.h"
#include "broker/Manager.h"
 
namespace atom {
namespace control {
 
Node::TransitionList Node::transitions_;
std::map<Node::State, std::string> Node::state_names_;
std::map<Node::Event, std::string> Node::event_names_;
   
Node::Node(Node::Id id) : LOG("control::Node")
{
    this->state_ = STATE_NORM_OFFLINE;
    //this->id_ = boost::algorithm::to_lower_copy((std::string)id);
    //this->id_ = boost::algorithm::to_upper_copy((std::string)id);
    this->id_ = (std::string)id;
    this->information_.has_application_ = false;
    this->information_.bios_version_ = 0;
    this->information_.device_type_ = "N/A";
    this->information_.last_active_ = 0;
    this->information_.valid_ = false;
   
    this->current_offset_ = 0;
    this->expected_ack_data_ = 0;
    this->program_start_time_ = 0;
}
 
Node::~Node()
{
 
}
 
void Node::ConnectSlots(const SignalOnNewState::slot_type& slot_on_new_state)
{
    this->signal_on_new_state_.connect(slot_on_new_state);
}
 
Node::Id Node::GetId()
{
    return this->id_;
}
 
Node::Information Node::GetInformation()
{
    return this->information_;
}
 
void Node::SetupStateMachine()
{
    // Normal flow
    Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_BIOS_START,        STATE_NORM_ONLINE);
    Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_APP_START,         STATE_NORM_LIST);
    Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_HEARTBEAT,         STATE_NORM_LIST);
    Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_PROGRAM_BIOS,      STATE_BPGM_OFFLINE);
    Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_PROGRAM_APP,       STATE_APGM_OFFLINE);
    Node::AddTransition(STATE_NORM_OFFLINE,          EVENT_RESET,             STATE_NO_CHANGE);
   
    Node::AddTransition(STATE_NORM_ONLINE,           EVENT_BIOS_START,        STATE_NORM_ONLINE);
    Node::AddTransition(STATE_NORM_ONLINE,           EVENT_APP_START,         STATE_NORM_LIST);
    Node::AddTransition(STATE_NORM_ONLINE,           EVENT_HEARTBEAT,         STATE_NORM_LIST);
    Node::AddTransition(STATE_NORM_ONLINE,           EVENT_PROGRAM_BIOS,      STATE_BPGM_OFFLINE);
    Node::AddTransition(STATE_NORM_ONLINE,           EVENT_PROGRAM_APP,       STATE_APGM_OFFLINE);
    Node::AddTransition(STATE_NORM_ONLINE,           EVENT_RESET,             STATE_NORM_OFFLINE);
   
    Node::AddTransition(STATE_NORM_LIST,             EVENT_BIOS_START,        STATE_NORM_ONLINE);
    Node::AddTransition(STATE_NORM_LIST,             EVENT_APP_START,         STATE_NORM_LIST);
    Node::AddTransition(STATE_NORM_LIST,             EVENT_HEARTBEAT,         STATE_NO_CHANGE);
    Node::AddTransition(STATE_NORM_LIST,             EVENT_LIST_DONE,         STATE_NORM_INITIALIZED);
    Node::AddTransition(STATE_NORM_LIST,             EVENT_PROGRAM_BIOS,      STATE_BPGM_OFFLINE);
    Node::AddTransition(STATE_NORM_LIST,             EVENT_PROGRAM_APP,       STATE_APGM_OFFLINE);
    Node::AddTransition(STATE_NORM_LIST,             EVENT_RESET,             STATE_NORM_OFFLINE);
   
    Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_BIOS_START,        STATE_NORM_ONLINE);
    Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_APP_START,         STATE_NORM_LIST);
    Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_HEARTBEAT,         STATE_NO_CHANGE);
    Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_PROGRAM_BIOS,      STATE_BPGM_OFFLINE);
    Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_PROGRAM_APP,       STATE_APGM_OFFLINE);
    Node::AddTransition(STATE_NORM_INITIALIZED,      EVENT_RESET,             STATE_NORM_OFFLINE);
   
   
    // Program bios flow
    Node::AddTransition(STATE_BPGM_OFFLINE,          EVENT_BIOS_START,        STATE_BPGM_START);
   
    Node::AddTransition(STATE_BPGM_START,            EVENT_PGM_ACK,           STATE_BPGM_DATA);
    Node::AddTransition(STATE_BPGM_START,            EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
 
    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
    Node::AddTransition(STATE_BPGM_DATA,             EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
   
    Node::AddTransition(STATE_BPGM_END,              EVENT_PGM_ACK,           STATE_BPGM_COPY);
    Node::AddTransition(STATE_BPGM_END,              EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
   
    Node::AddTransition(STATE_BPGM_COPY,             EVENT_BIOS_START,        STATE_APGM_OFFLINE); // Remove application after bios upgrade
       
   
    // Program application flow
    Node::AddTransition(STATE_APGM_OFFLINE,          EVENT_BIOS_START,        STATE_APGM_START);
   
    Node::AddTransition(STATE_APGM_START,            EVENT_PGM_ACK,           STATE_APGM_DATA);
    Node::AddTransition(STATE_APGM_START,            EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
   
    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
    Node::AddTransition(STATE_APGM_DATA,             EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
   
    Node::AddTransition(STATE_APGM_END,              EVENT_PGM_ACK,           STATE_NORM_OFFLINE); // Return to normal flow
    Node::AddTransition(STATE_APGM_END,              EVENT_PGM_NACK,          STATE_NORM_OFFLINE); // Return to normal flow
   
   
    Node::state_names_[STATE_INVALID] = "STATE_INVALID";
    Node::state_names_[STATE_NO_CHANGE] = "STATE_NO_CHANGE";
    Node::state_names_[STATE_NORM_OFFLINE] = "STATE_NORM_OFFLINE";
    Node::state_names_[STATE_NORM_ONLINE] = "STATE_NORM_ONLINE";
    Node::state_names_[STATE_NORM_LIST] = "STATE_NORM_LIST";
    Node::state_names_[STATE_NORM_INITIALIZED] = "STATE_NORM_INITIALIZED";
    Node::state_names_[STATE_BPGM_OFFLINE] = "STATE_BPGM_OFFLINE";
    Node::state_names_[STATE_BPGM_START] = "STATE_BPGM_START";
    Node::state_names_[STATE_BPGM_DATA] = "STATE_BPGM_DATA";
    Node::state_names_[STATE_BPGM_END] = "STATE_BPGM_END";
    Node::state_names_[STATE_BPGM_COPY] = "STATE_BPGM_COPY";
    Node::state_names_[STATE_APGM_OFFLINE] = "STATE_APGM_OFFLINE";
    Node::state_names_[STATE_APGM_START] = "STATE_APGM_START";
    Node::state_names_[STATE_APGM_DATA] = "STATE_APGM_DATA";
    Node::state_names_[STATE_APGM_END] = "STATE_APGM_END";
   
    Node::event_names_[EVENT_BIOS_START] = "EVENT_BIOS_START";
    Node::event_names_[EVENT_APP_START] = "EVENT_APP_START";
    Node::event_names_[EVENT_HEARTBEAT] = "EVENT_HEARTBEAT";
    Node::event_names_[EVENT_PGM_ACK] = "EVENT_PGM_ACK";
    Node::event_names_[EVENT_PGM_NACK] = "EVENT_PGM_NACK";
    Node::event_names_[EVENT_LIST_DONE] = "EVENT_LIST_DONE";
    Node::event_names_[EVENT_PROGRAM_BIOS] = "EVENT_PROGRAM_BIOS";
    Node::event_names_[EVENT_PROGRAM_APP] = "EVENT_PROGRAM_APP";
    Node::event_names_[EVENT_RESET] = "EVENT_RESET";
}
 
void Node::AddTransition(Node::State current_state, Node::Event event, State target_state)
{
    Node::transitions_[current_state][event] = target_state;
}
 
Node::State Node::GetTransitionTarget(Node::Event event)
{
    Node::TransitionList::iterator it1 = Node::transitions_.find(this->state_);
   
    if (it1 == Node::transitions_.end())
    {
        LOG.Warning("No transitions found for current state " + boost::lexical_cast<std::string>(this->state_));
        return STATE_INVALID;
    }
   
    Node::Transition::iterator it2 = it1->second.find(event);
   
    if (it2 == it1->second.end())
    {
        LOG.Warning("No transitions found for current state " + boost::lexical_cast<std::string>(this->state_) + " for event " + boost::lexical_cast<std::string>(event));
        return STATE_INVALID;
    }
   
    return it2->second;
}
 
void Node::Trigger(Node::Event event, common::StringMap variables)
{
    if (!this->id_.empty())
    {
        this->ResetTimeout();
       
        State target_state = this->GetTransitionTarget(event);
       
        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]);
       
        if (event == EVENT_BIOS_START)
        {
            this->information_.has_application_ = boost::lexical_cast<int>(variables["HasApplication"]) > 0;
            this->information_.bios_version_ = boost::lexical_cast<unsigned int>(variables["BiosVersion"]);
            this->information_.device_type_ = variables["DeviceType"];
            this->information_.valid_ = true;
        }
       
        if (target_state == STATE_INVALID)
        {
            //this->SendReset();
            target_state = STATE_NORM_OFFLINE;
        }
        else if (target_state == STATE_NO_CHANGE)
        {
            return;
        }
       
        if ((this->state_ == STATE_APGM_END || this->state_ == STATE_BPGM_END) && event == EVENT_PGM_ACK)
        {
            unsigned int checksum = boost::lexical_cast<unsigned int>(variables["Data"]);
           
            if (this->expected_ack_data_ != checksum)
            {
                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...");
 
                this->SendReset();
 
                target_state = STATE_NORM_OFFLINE;
            }
            else if (this->state_ == STATE_BPGM_END)
            {
                float speed = (float)this->code_->GetLength() / (float)(time(NULL) - this->program_start_time_);
               
                LOG.Info("Data was successfully transferred to " + this->id_ + ", speed was " + boost::lexical_cast<std::string>(speed) + " B/s");
               
                LOG.Debug("Sending application programming copy to node " + this->id_);
                can::Message* payload = new can::Message("nmt", "", "", 0, "Pgm_Copy");
               
                unsigned int source0 = GET_LOW_BYTE_16(this->start_offset_);
                unsigned int source1 = GET_HIGH_BYTE_16(this->start_offset_);
               
                unsigned int destination0 = GET_LOW_BYTE_16(this->code_->GetAddressLower());
                unsigned int destination1 = GET_HIGH_BYTE_16(this->code_->GetAddressLower());
               
                unsigned int length0 = GET_LOW_BYTE_16(this->code_->GetLength());
                unsigned int length1 = GET_HIGH_BYTE_16(this->code_->GetLength());
               
                payload->SetVariable("Source0", boost::lexical_cast<std::string>(source0));
                payload->SetVariable("Source1", boost::lexical_cast<std::string>(source1));
                payload->SetVariable("Destination0", boost::lexical_cast<std::string>(destination0));
                payload->SetVariable("Destination1", boost::lexical_cast<std::string>(destination1));
                payload->SetVariable("Length0", boost::lexical_cast<std::string>(length0));
                payload->SetVariable("Length1", boost::lexical_cast<std::string>(length1));
               
                broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
            }
            else
            {
                float speed = (float)this->code_->GetLength() / (float)(time(NULL) - this->program_start_time_);
               
                LOG.Info("Programming was completed successfully on " + this->id_ + ", speed was " + boost::lexical_cast<std::string>(speed) + " B/s");
            }
        }
       
        if (target_state == STATE_NORM_LIST)
        {
            this->SendListRequest();
        }
        else if (target_state == STATE_BPGM_OFFLINE || target_state == STATE_APGM_OFFLINE)
        {
            this->current_offset_ = 0;
            if (event != EVENT_BIOS_START)
            {
                this->SendReset();
            }
        }
        else if (target_state == STATE_APGM_START || target_state == STATE_BPGM_START)
        {
            if (!this->code_->IsValid())
            {
                LOG.Error("Code is not valid, aborting...");
                target_state = STATE_NORM_OFFLINE;
            }
            else
            {
                LOG.Debug("Sending programming start to node " + this->id_);
                can::Message* payload = new can::Message("nmt", "", "", 0, "Pgm_Start");
                payload->SetVariable("HardwareId", boost::lexical_cast<std::string>(common::FromHex(this->id_)));
               
                this->start_offset_ = target_state == STATE_BPGM_START ? 0 : this->code_->GetAddressLower();
               
                unsigned int address0 = GET_LOW_BYTE_16(this->start_offset_);
                unsigned int address1 = GET_HIGH_BYTE_16(this->start_offset_);
               
                this->expected_ack_data_ = SWAP_BYTE_ORDER_16(this->start_offset_);
               
                payload->SetVariable("Address0", boost::lexical_cast<std::string>(address0));
                payload->SetVariable("Address1", boost::lexical_cast<std::string>(address1));
                payload->SetVariable("Address2", "0"); // Not used?
                payload->SetVariable("Address3", "0"); // Not used?
               
                this->current_offset_ = 0;
               
                broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
               
                this->program_start_time_ = time(NULL);
            }
        }
        else if (target_state == STATE_APGM_DATA || target_state == STATE_BPGM_DATA)
        {
            unsigned int offset = boost::lexical_cast<unsigned int>(variables["Data"]);
           
            if ( this->expected_ack_data_ != offset)
            {
                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...");
                target_state = STATE_NORM_OFFLINE;
            }
            else if (this->current_offset_ >= this->code_->GetLength())
            {
                LOG.Debug("Sending programming end to node " + this->id_);
               
                unsigned int checksum = this->code_->GetChecksum();
                this->expected_ack_data_ = SWAP_BYTE_ORDER_16(checksum);
               
                can::Message* payload = new can::Message("nmt", "", "", 0, "Pgm_End");
                broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
               
                target_state = target_state == STATE_BPGM_DATA ? STATE_BPGM_END : STATE_APGM_END;
            }
            else
            {
                LOG.Debug("Sending programming data to node " + this->id_);
                can::Message* payload = new can::Message("nmt", "", "", 0, "Pgm_Data_48");
               
                unsigned int offset0 = GET_LOW_BYTE_16(this->start_offset_ + this->current_offset_);
                unsigned int offset1 = GET_HIGH_BYTE_16(this->start_offset_ + this->current_offset_);
               
                this->expected_ack_data_ = SWAP_BYTE_ORDER_16(this->start_offset_ + this->current_offset_);
               
                payload->SetVariable("Offset0", boost::lexical_cast<std::string>(offset0));
                payload->SetVariable("Offset1", boost::lexical_cast<std::string>(offset1));
               
                for (int n = 0; (this->current_offset_ < this->code_->GetLength()) && (n < 6); n++)
                {
                    //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_)));
                    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_)));
                    this->current_offset_++;
                }
               
                broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
            }
        }
        else if (target_state == STATE_BPGM_COPY)
        {
            LOG.Debug("Preparing to send null application to node " + this->id_);
           
            this->code_->Reset(false);
           
            this->code_->AddByte(0xFF);
            this->code_->AddByte(0xFF);
        }
        else if (target_state == STATE_NORM_OFFLINE)
        {
            this->information_.valid_ = false;
            this->SendReset();
        }
       
       
        if (this->state_ != target_state)
        {
            this->signal_on_new_state_(this->id_, this->state_, target_state);
        }
       
        this->state_ = target_state;
    }
}
 
bool Node::CheckTimeout()
{
    if (this->state_ == STATE_NORM_OFFLINE)
    {
        return true;
    }
   
    if (this->code_.use_count() == 0 && this->information_.last_active_ + 10 < time(NULL))
    {
        this->state_ = STATE_NORM_OFFLINE;
        return false;
    }
   
    return true;
}
 
void Node::ResetTimeout()
{
    this->information_.last_active_ = time(NULL);
}
 
void Node::Reset()
{
    common::StringMap variables;
   
    this->Trigger(EVENT_RESET, variables);
}
 
void Node::ProgramApplication(Code::Pointer code)
{
    this->code_ = code;
   
    common::StringMap variables;
   
    this->Trigger(EVENT_PROGRAM_APP, variables);
}
 
void Node::ProgramBios(Code::Pointer code)
{
    this->code_ = code;
   
    common::StringMap variables;
   
    this->Trigger(EVENT_PROGRAM_BIOS, variables);
}
 
void Node::SendReset()
{
    if (!this->id_.empty())
    {
        LOG.Info("Sending node reset to " + this->id_);
        can::Message* payload = new can::Message("nmt", "", "", 0, "Reset");
        payload->SetVariable("HardwareId", boost::lexical_cast<std::string>(common::FromHex(this->id_)));
       
        broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
    }
}
 
void Node::SendListRequest()
{
    if (!this->id_.empty())
    {
        LOG.Debug("Sending module listing on node " + this->id_);
        can::Message* payload = new can::Message("mnmt", "To_Owner", "", 0, "List");
        payload->SetVariable("HardwareId", boost::lexical_cast<std::string>(common::FromHex(this->id_)));
       
        broker::Manager::Instance()->Post(broker::Message::Pointer(new broker::Message(broker::Message::CAN_MESSAGE, broker::Message::PayloadPointer(payload), Manager::Instance().get())));
    }
}
 
unsigned int Node::GetProgramProgress()
{
    //if ( this->state_ == STATE_APGM_START || this->state_ == STATE_BPGM_START || this->state_ == STATE_APGM_DATA || this->state_ == STATE_BPGM_DATA || this->state_ == STATE_APGM_END || this->state_ == STATE_BPGM_END || this->state_ == STATE_BPGM_COPY )
    //{
        return 100 * this->current_offset_ / this->code_->GetLength();
    //}
    //else
    //{
    //  return 0;
    //}
}
 
}; // namespace control
}; // namespace atom
 
Subversion Repositories HomeAutomation

(root)/trunk/PcSoftware/Atom1.5/src/control/Node.cpp – Rev 2164 → 2165
