git commit -m "first commit for v2"
This commit is contained in:
68
Devices/Libraries/Ros/ros_canopen/canopen_master/src/bcm_sync.cpp
Executable file
68
Devices/Libraries/Ros/ros_canopen/canopen_master/src/bcm_sync.cpp
Executable file
@@ -0,0 +1,68 @@
|
||||
#include <canopen_master/bcm_sync.h>
|
||||
#include <socketcan_interface/string.h>
|
||||
#include <iostream>
|
||||
|
||||
int main(int argc, char** argv){
|
||||
|
||||
if(argc < 4){
|
||||
std::cout << "Usage: " << argv[0] << " DEVICE PERIOD_MS HEADER [OVERFLOW] [+ID*] [-ID*] [--]" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::string can_device = argv[1];
|
||||
int sync_ms = atoi(argv[2]);
|
||||
can::Header header = can::toheader(argv[3]);
|
||||
|
||||
if(!header.isValid()){
|
||||
std::cout << "header is invalid" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
int sync_overflow = 0;
|
||||
|
||||
int start = 4;
|
||||
if(argc > start && argv[start][0] != '-' && argv[start][0] != '+'){
|
||||
sync_overflow = atoi(argv[4]);
|
||||
if(sync_overflow == 1 || sync_overflow < 0 || sync_overflow > 240){
|
||||
std::cout << "sync overflow is invalid" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
++start;
|
||||
}
|
||||
|
||||
std::set<int> monitored, ignored;
|
||||
|
||||
for(; argc > start; ++start){
|
||||
if(strncmp("--", argv[start], 2) == 0) break;
|
||||
int id = atoi(argv[start]);
|
||||
|
||||
if(id > 0 && id < 128) monitored.insert(id);
|
||||
else if (id < 0 && id > -128) ignored.insert(-id);
|
||||
else{
|
||||
std::cout << "ID is invalid: " << id << std::endl;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
can::SocketCANDriverSharedPtr driver = std::make_shared<can::SocketCANDriver>();
|
||||
if(!driver->init(can_device, false, can::NoSettings::create())){
|
||||
std::cout << "Could not initialize CAN" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
canopen::SyncProperties sync_properties(header, sync_ms, sync_overflow);
|
||||
canopen::BCMsync sync(can_device, driver, sync_properties);
|
||||
|
||||
sync.setMonitored(monitored);
|
||||
sync.setIgnored(ignored);
|
||||
|
||||
canopen::LayerStatus status;
|
||||
sync.init(status);
|
||||
if(!status.bounded<canopen::LayerStatus::Warn>()){
|
||||
std::cout << "Could not initialize sync" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
driver->run();
|
||||
|
||||
return 0;
|
||||
}
|
||||
148
Devices/Libraries/Ros/ros_canopen/canopen_master/src/emcy.cpp
Executable file
148
Devices/Libraries/Ros/ros_canopen/canopen_master/src/emcy.cpp
Executable file
@@ -0,0 +1,148 @@
|
||||
#include <canopen_master/canopen.h>
|
||||
#include <socketcan_interface/string.h>
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
#pragma pack(push) /* push current alignment to stack */
|
||||
#pragma pack(1) /* set alignment to 1 byte boundary */
|
||||
|
||||
struct EMCYid{
|
||||
uint32_t id:29;
|
||||
uint32_t extended:1;
|
||||
uint32_t :1;
|
||||
uint32_t invalid:1;
|
||||
EMCYid(uint32_t val){
|
||||
*(uint32_t*) this = val;
|
||||
}
|
||||
can::Header header() {
|
||||
return can::Header(id, extended, false, false);
|
||||
}
|
||||
const uint32_t get() const { return *(uint32_t*) this; }
|
||||
};
|
||||
|
||||
struct EMCYfield{
|
||||
uint32_t error_code:16;
|
||||
uint32_t addition_info:16;
|
||||
EMCYfield(uint32_t val){
|
||||
*(uint32_t*) this = val;
|
||||
}
|
||||
};
|
||||
|
||||
struct EMCYmsg{
|
||||
uint16_t error_code;
|
||||
uint8_t error_register;
|
||||
uint8_t manufacturer_specific_error_field[5];
|
||||
|
||||
struct Frame: public FrameOverlay<EMCYmsg>{
|
||||
Frame(const can::Frame &f) : FrameOverlay(f){ }
|
||||
};
|
||||
};
|
||||
|
||||
#pragma pack(pop) /* pop previous alignment from stack */
|
||||
|
||||
void EMCYHandler::handleEMCY(const can::Frame & msg){
|
||||
EMCYmsg::Frame em(msg);
|
||||
if (em.data.error_code == 0) {
|
||||
ROSCANOPEN_INFO("canopen_master", "EMCY reset: " << msg);
|
||||
} else {
|
||||
ROSCANOPEN_ERROR("canopen_master", "EMCY received: " << msg);
|
||||
}
|
||||
has_error_ = (em.data.error_register & ~32) != 0;
|
||||
}
|
||||
|
||||
EMCYHandler::EMCYHandler(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr storage): Layer("EMCY handler"), storage_(storage), has_error_(true){
|
||||
storage_->entry(error_register_, 0x1001);
|
||||
try{
|
||||
storage_->entry(num_errors_, 0x1003,0);
|
||||
}
|
||||
catch(...){
|
||||
// pass, 1003 is optional
|
||||
}
|
||||
try{
|
||||
EMCYid emcy_id(storage_->entry<uint32_t>(0x1014).get_cached());
|
||||
emcy_listener_ = interface->createMsgListenerM(emcy_id.header(), this, &EMCYHandler::handleEMCY);
|
||||
|
||||
|
||||
}
|
||||
catch(...){
|
||||
// pass, EMCY is optional
|
||||
}
|
||||
}
|
||||
|
||||
void EMCYHandler::handleRead(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state == Ready){
|
||||
if(has_error_){
|
||||
status.error("Node has emergency error");
|
||||
}
|
||||
}
|
||||
}
|
||||
void EMCYHandler::handleWrite(LayerStatus &status, const LayerState ¤t_state) {
|
||||
// noithing to do
|
||||
}
|
||||
|
||||
void EMCYHandler::handleDiag(LayerReport &report){
|
||||
uint8_t error_register = 0;
|
||||
if(!error_register_.get(error_register)){
|
||||
report.error("Could not read error error_register");
|
||||
return;
|
||||
}
|
||||
|
||||
if(error_register){
|
||||
if(error_register & 1){ // first bit should be set on all errors
|
||||
report.error("Node has emergency error");
|
||||
}else if(error_register & ~32){ // filter profile-specific bit
|
||||
report.warn("Error register is not zero");
|
||||
}
|
||||
report.add("error_register", (uint32_t) error_register);
|
||||
|
||||
uint8_t num = num_errors_.valid() ? num_errors_.get() : 0;
|
||||
std::stringstream buf;
|
||||
for(size_t i = 0; i < num; ++i) {
|
||||
if( i!= 0){
|
||||
buf << ", ";
|
||||
}
|
||||
try{
|
||||
ObjectStorage::Entry<uint32_t> error;
|
||||
storage_->entry(error, 0x1003,i+1);
|
||||
EMCYfield field(error.get());
|
||||
buf << std::hex << field.error_code << "#" << field.addition_info;
|
||||
}
|
||||
catch (const std::out_of_range & e){
|
||||
buf << "NOT_IN_DICT!";
|
||||
}
|
||||
catch (const TimeoutException & e){
|
||||
buf << "LIST_UNDERFLOW!";
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
report.add("errors", buf.str());
|
||||
|
||||
}
|
||||
}
|
||||
void EMCYHandler::handleInit(LayerStatus &status){
|
||||
uint8_t error_register = 0;
|
||||
if(!error_register_.get(error_register)){
|
||||
status.error("Could not read error error_register");
|
||||
return;
|
||||
}else if(error_register & 1){
|
||||
ROSCANOPEN_ERROR("canopen_master", "error register: " << int(error_register));
|
||||
status.error("Node has emergency error");
|
||||
return;
|
||||
}
|
||||
|
||||
resetErrors(status);
|
||||
}
|
||||
void EMCYHandler::resetErrors(LayerStatus &status){
|
||||
if(num_errors_.valid()) num_errors_.set(0);
|
||||
has_error_ = false;
|
||||
}
|
||||
|
||||
void EMCYHandler::handleRecover(LayerStatus &status){
|
||||
handleInit(status);
|
||||
}
|
||||
void EMCYHandler::handleShutdown(LayerStatus &status){
|
||||
}
|
||||
void EMCYHandler::handleHalt(LayerStatus &status){
|
||||
// do nothing
|
||||
}
|
||||
138
Devices/Libraries/Ros/ros_canopen/canopen_master/src/master_plugin.cpp
Executable file
138
Devices/Libraries/Ros/ros_canopen/canopen_master/src/master_plugin.cpp
Executable file
@@ -0,0 +1,138 @@
|
||||
#include <class_loader/class_loader.hpp>
|
||||
#include <socketcan_interface/reader.h>
|
||||
#include <canopen_master/canopen.h>
|
||||
|
||||
#include <set>
|
||||
|
||||
namespace canopen {
|
||||
|
||||
class ManagingSyncLayer: public SyncLayer {
|
||||
protected:
|
||||
can::CommInterfaceSharedPtr interface_;
|
||||
boost::chrono::milliseconds step_, half_step_;
|
||||
|
||||
std::set<void *> nodes_;
|
||||
boost::mutex nodes_mutex_;
|
||||
std::atomic<size_t> nodes_size_;
|
||||
|
||||
virtual void handleShutdown(LayerStatus &status) {
|
||||
}
|
||||
|
||||
virtual void handleHalt(LayerStatus &status) { /* nothing to do */ }
|
||||
virtual void handleDiag(LayerReport &report) { /* TODO */ }
|
||||
virtual void handleRecover(LayerStatus &status) { /* TODO */ }
|
||||
|
||||
public:
|
||||
ManagingSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface)
|
||||
: SyncLayer(p), interface_(interface), step_(p.period_ms_), half_step_(p.period_ms_/2), nodes_size_(0)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addNode(void * const ptr) {
|
||||
boost::mutex::scoped_lock lock(nodes_mutex_);
|
||||
nodes_.insert(ptr);
|
||||
nodes_size_ = nodes_.size();
|
||||
}
|
||||
virtual void removeNode(void * const ptr) {
|
||||
boost::mutex::scoped_lock lock(nodes_mutex_);
|
||||
nodes_.erase(ptr);
|
||||
nodes_size_ = nodes_.size();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
class SimpleSyncLayer: public ManagingSyncLayer {
|
||||
time_point read_time_, write_time_;
|
||||
can::Frame frame_;
|
||||
uint8_t overflow_;
|
||||
|
||||
void resetCounter(){
|
||||
frame_.data[0] = 1; // SYNC counter starts at 1
|
||||
}
|
||||
void tryUpdateCounter(){
|
||||
if (frame_.dlc > 0) { // sync counter is used
|
||||
if (frame_.data[0] >= overflow_) {
|
||||
resetCounter();
|
||||
}else{
|
||||
++frame_.data[0];
|
||||
}
|
||||
}
|
||||
}
|
||||
protected:
|
||||
virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Init){
|
||||
boost::this_thread::sleep_until(read_time_);
|
||||
write_time_ += step_;
|
||||
}
|
||||
}
|
||||
virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Init){
|
||||
boost::this_thread::sleep_until(write_time_);
|
||||
tryUpdateCounter();
|
||||
if(nodes_size_){ //)
|
||||
interface_->send(frame_);
|
||||
}
|
||||
read_time_ = get_abs_time(half_step_);
|
||||
}
|
||||
}
|
||||
|
||||
virtual void handleInit(LayerStatus &status){
|
||||
write_time_ = get_abs_time(step_);
|
||||
read_time_ = get_abs_time(half_step_);
|
||||
}
|
||||
public:
|
||||
SimpleSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface)
|
||||
: ManagingSyncLayer(p, interface), frame_(p.header_, 0), overflow_(p.overflow_) {
|
||||
if(overflow_ == 1 || overflow_ > 240){
|
||||
BOOST_THROW_EXCEPTION(Exception("SYNC counter overflow is invalid"));
|
||||
}else if(overflow_ > 1){
|
||||
frame_.dlc = 1;
|
||||
resetCounter();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
class ExternalSyncLayer: public ManagingSyncLayer {
|
||||
can::BufferedReader reader_;
|
||||
protected:
|
||||
virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) {
|
||||
can::Frame msg;
|
||||
if(current_state > Init){
|
||||
if(reader_.readUntil(&msg, get_abs_time(step_))){ // wait for sync
|
||||
boost::this_thread::sleep_until(get_abs_time(half_step_)); // shift readout to middle of period
|
||||
}
|
||||
}
|
||||
}
|
||||
virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) {
|
||||
// nothing to do here
|
||||
}
|
||||
virtual void handleInit(LayerStatus &status){
|
||||
reader_.listen(interface_, properties.header_);
|
||||
}
|
||||
public:
|
||||
ExternalSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface)
|
||||
: ManagingSyncLayer(p, interface), reader_(true,1) {}
|
||||
};
|
||||
|
||||
|
||||
template<typename SyncType> class WrapMaster: public Master{
|
||||
can::CommInterfaceSharedPtr interface_;
|
||||
public:
|
||||
virtual SyncLayerSharedPtr getSync(const SyncProperties &properties){
|
||||
return std::make_shared<SyncType>(properties, interface_);
|
||||
}
|
||||
WrapMaster(can::CommInterfaceSharedPtr interface) : interface_(interface) {}
|
||||
|
||||
class Allocator : public Master::Allocator{
|
||||
public:
|
||||
virtual MasterSharedPtr allocate(const std::string &name, can::CommInterfaceSharedPtr interface){
|
||||
return std::make_shared<WrapMaster>(interface);
|
||||
}
|
||||
};
|
||||
};
|
||||
|
||||
typedef WrapMaster<SimpleSyncLayer> SimpleMaster;
|
||||
typedef WrapMaster<ExternalSyncLayer> ExternalMaster;
|
||||
}
|
||||
CLASS_LOADER_REGISTER_CLASS(canopen::SimpleMaster::Allocator, canopen::Master::Allocator);
|
||||
CLASS_LOADER_REGISTER_CLASS(canopen::ExternalMaster::Allocator, canopen::Master::Allocator);
|
||||
218
Devices/Libraries/Ros/ros_canopen/canopen_master/src/node.cpp
Executable file
218
Devices/Libraries/Ros/ros_canopen/canopen_master/src/node.cpp
Executable file
@@ -0,0 +1,218 @@
|
||||
#include <canopen_master/canopen.h>
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
#pragma pack(push) /* push current alignment to stack */
|
||||
#pragma pack(1) /* set alignment to 1 byte boundary */
|
||||
|
||||
|
||||
struct NMTcommand{
|
||||
enum Command{
|
||||
Start = 1,
|
||||
Stop = 2,
|
||||
Prepare = 128,
|
||||
Reset = 129,
|
||||
Reset_Com = 130
|
||||
};
|
||||
uint8_t command;
|
||||
uint8_t node_id;
|
||||
|
||||
struct Frame: public FrameOverlay<NMTcommand>{
|
||||
Frame(uint8_t node_id, const Command &c) : FrameOverlay(can::Header()) {
|
||||
data.command = c;
|
||||
data.node_id = node_id;
|
||||
}
|
||||
};
|
||||
};
|
||||
|
||||
#pragma pack(pop) /* pop previous alignment from stack */
|
||||
|
||||
Node::Node(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id, const SyncCounterSharedPtr sync)
|
||||
: Layer("Node 301"), node_id_(node_id), interface_(interface), sync_(sync) , state_(Unknown), sdo_(interface, dict, node_id), pdo_(interface){
|
||||
try{
|
||||
getStorage()->entry(heartbeat_, 0x1017);
|
||||
}
|
||||
catch(const std::out_of_range){
|
||||
}
|
||||
}
|
||||
|
||||
const Node::State Node::getState(){
|
||||
boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
|
||||
return state_;
|
||||
}
|
||||
|
||||
bool Node::reset_com(){
|
||||
boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
|
||||
getStorage()->reset();
|
||||
interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Reset_Com));
|
||||
if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){
|
||||
return false;
|
||||
}
|
||||
state_ = PreOperational;
|
||||
setHeartbeatInterval();
|
||||
return true;
|
||||
}
|
||||
bool Node::reset(){
|
||||
boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
|
||||
getStorage()->reset();
|
||||
|
||||
interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Reset));
|
||||
if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){
|
||||
return false;
|
||||
}
|
||||
state_ = PreOperational;
|
||||
setHeartbeatInterval();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Node::prepare(){
|
||||
boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
|
||||
if(state_ == BootUp){
|
||||
// ERROR
|
||||
}
|
||||
interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Prepare));
|
||||
return 0 != wait_for(PreOperational, boost::chrono::seconds(2));
|
||||
}
|
||||
bool Node::start(){
|
||||
boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
|
||||
if(state_ == BootUp){
|
||||
// ERROR
|
||||
}
|
||||
interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Start));
|
||||
return 0 != wait_for(Operational, boost::chrono::seconds(2));
|
||||
}
|
||||
bool Node::stop(){
|
||||
boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
|
||||
if(sync_) sync_->removeNode(this);
|
||||
if(state_ == BootUp){
|
||||
// ERROR
|
||||
}
|
||||
interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Stop));
|
||||
return true;
|
||||
}
|
||||
|
||||
void Node::switchState(const uint8_t &s){
|
||||
bool changed = state_ != s;
|
||||
switch(s){
|
||||
case Operational:
|
||||
if(changed && sync_) sync_->addNode(this);
|
||||
break;
|
||||
case BootUp:
|
||||
case PreOperational:
|
||||
case Stopped:
|
||||
if(changed && sync_) sync_->removeNode(this);
|
||||
break;
|
||||
default:
|
||||
//error
|
||||
;
|
||||
}
|
||||
if(changed){
|
||||
state_ = (State) s;
|
||||
state_dispatcher_.dispatch(state_);
|
||||
cond.notify_one();
|
||||
}
|
||||
}
|
||||
void Node::handleNMT(const can::Frame & msg){
|
||||
boost::mutex::scoped_lock cond_lock(cond_mutex);
|
||||
uint16_t interval = getHeartbeatInterval();
|
||||
if(interval) heartbeat_timeout_ = get_abs_time(boost::chrono::milliseconds(3*interval));
|
||||
assert(msg.dlc == 1);
|
||||
switchState(msg.data[0]);
|
||||
}
|
||||
template<typename T> int Node::wait_for(const State &s, const T &timeout){
|
||||
boost::mutex::scoped_lock cond_lock(cond_mutex);
|
||||
time_point abs_time = get_abs_time(timeout);
|
||||
|
||||
while(s != state_) {
|
||||
if(cond.wait_until(cond_lock,abs_time) == boost::cv_status::timeout)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
if( s!= state_){
|
||||
if(getHeartbeatInterval() == 0){
|
||||
switchState(s);
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
bool Node::checkHeartbeat(){
|
||||
if(getHeartbeatInterval() == 0) return true; //disabled
|
||||
boost::mutex::scoped_lock cond_lock(cond_mutex);
|
||||
return heartbeat_timeout_ >= boost::chrono::high_resolution_clock::now();
|
||||
}
|
||||
|
||||
|
||||
void Node::handleRead(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Init){
|
||||
if(!checkHeartbeat()){
|
||||
status.error("heartbeat problem");
|
||||
} else if(getState() != Operational){
|
||||
status.error("not operational");
|
||||
} else{
|
||||
pdo_.read(status);
|
||||
}
|
||||
}
|
||||
}
|
||||
void Node::handleWrite(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Init){
|
||||
if(getState() != Operational) status.error("not operational");
|
||||
else if(! pdo_.write()) status.error("PDO write problem");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Node::handleDiag(LayerReport &report){
|
||||
State state = getState();
|
||||
if(state != Operational){
|
||||
report.error("Mode not operational");
|
||||
report.add("Node state", (int)state);
|
||||
}else if(!checkHeartbeat()){
|
||||
report.error("Heartbeat timeout");
|
||||
}
|
||||
}
|
||||
void Node::handleInit(LayerStatus &status){
|
||||
nmt_listener_ = interface_->createMsgListenerM(can::MsgHeader(0x700 + node_id_), this, &Node::handleNMT);
|
||||
|
||||
sdo_.init();
|
||||
try{
|
||||
if(!reset_com()) BOOST_THROW_EXCEPTION( TimeoutException("reset_timeout") );
|
||||
}
|
||||
catch(const TimeoutException&){
|
||||
status.error(boost::str(boost::format("could not reset node '%1%'") % (int)node_id_));
|
||||
return;
|
||||
}
|
||||
|
||||
if(!pdo_.init(getStorage(), status)){
|
||||
return;
|
||||
}
|
||||
getStorage()->init_all();
|
||||
sdo_.init(); // reread SDO paramters;
|
||||
// TODO: set SYNC data
|
||||
|
||||
try{
|
||||
if(!start()) BOOST_THROW_EXCEPTION( TimeoutException("start timeout") );
|
||||
}
|
||||
catch(const TimeoutException&){
|
||||
status.error(boost::str(boost::format("could not start node '%1%'") % (int)node_id_));
|
||||
}
|
||||
}
|
||||
void Node::handleRecover(LayerStatus &status){
|
||||
try{
|
||||
start();
|
||||
}
|
||||
catch(const TimeoutException&){
|
||||
status.error(boost::str(boost::format("could not start node '%1%'") % (int)node_id_));
|
||||
}
|
||||
}
|
||||
void Node::handleShutdown(LayerStatus &status){
|
||||
if(getHeartbeatInterval()> 0) heartbeat_.set(0);
|
||||
stop();
|
||||
nmt_listener_.reset();
|
||||
switchState(Unknown);
|
||||
}
|
||||
void Node::handleHalt(LayerStatus &status){
|
||||
// do nothing
|
||||
}
|
||||
480
Devices/Libraries/Ros/ros_canopen/canopen_master/src/objdict.cpp
Executable file
480
Devices/Libraries/Ros/ros_canopen/canopen_master/src/objdict.cpp
Executable file
@@ -0,0 +1,480 @@
|
||||
#include <canopen_master/objdict.h>
|
||||
#include <socketcan_interface/string.h>
|
||||
#include <boost/property_tree/ptree.hpp>
|
||||
#include <boost/property_tree/ini_parser.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
|
||||
namespace canopen{
|
||||
size_t hash_value(ObjectDict::Key const& k) { return k.hash; }
|
||||
std::ostream& operator<<(std::ostream& stream, const ObjectDict::Key &k) { return stream << std::string(k); }
|
||||
}
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
template<> const String & HoldAny::get() const{
|
||||
return buffer;
|
||||
}
|
||||
|
||||
template<> String & ObjectStorage::Data::access(){
|
||||
if(!valid){
|
||||
THROW_WITH_KEY(std::length_error("buffer not valid") , key);
|
||||
}
|
||||
return buffer;
|
||||
}
|
||||
template<> String & ObjectStorage::Data::allocate(){
|
||||
buffer.resize(0);
|
||||
valid = true;
|
||||
return buffer;
|
||||
}
|
||||
|
||||
size_t ObjectDict::Key::fromString(const std::string &str){
|
||||
uint16_t index = 0;
|
||||
uint8_t sub_index = 0;
|
||||
int num = sscanf(str.c_str(),"%hxsub%hhx", &index, &sub_index);
|
||||
return (index << 16) | (num==2?sub_index:0xFFFF);
|
||||
}
|
||||
ObjectDict::Key::operator std::string() const{
|
||||
std::stringstream sstr;
|
||||
sstr << std::hex << index();
|
||||
if(hasSub()) sstr << "sub" << (int) sub_index();
|
||||
return sstr.str();
|
||||
}
|
||||
void ObjectStorage::Data::init(){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
|
||||
if(entry->init_val.is_empty()) return;
|
||||
|
||||
if(valid && !entry->def_val.is_empty() && buffer != entry->def_val.data()) return; // buffer was changed
|
||||
|
||||
if(!valid || buffer != entry->init_val.data()){
|
||||
buffer = entry->init_val.data();
|
||||
valid = true;
|
||||
if(entry->writable && (entry->def_val.is_empty() || entry->init_val.data() != entry->def_val.data()))
|
||||
write_delegate(*entry, buffer);
|
||||
}
|
||||
}
|
||||
void ObjectStorage::Data::force_write(){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
|
||||
if(!valid && entry->readable){
|
||||
read_delegate(*entry, buffer);
|
||||
valid = true;
|
||||
}
|
||||
if(valid) write_delegate(*entry, buffer);
|
||||
}
|
||||
|
||||
void ObjectStorage::Data::reset(){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(!entry->def_val.is_empty() && entry->def_val.type() == type_guard){
|
||||
buffer = entry->def_val.data();
|
||||
valid = true;
|
||||
}else{
|
||||
valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool ObjectDict::iterate(ObjectDict::ObjectDictMap::const_iterator &it) const{
|
||||
if(it != ObjectDict::ObjectDictMap::const_iterator()){
|
||||
++it;
|
||||
}else it = dict_.begin();
|
||||
return it != dict_.end();
|
||||
}
|
||||
void set_access( ObjectDict::Entry &entry, std::string access){
|
||||
boost::algorithm::to_lower(access);
|
||||
entry.constant = false;
|
||||
if(access == "ro"){
|
||||
entry.readable = true;
|
||||
entry.writable = false;
|
||||
}else if (access == "wo"){
|
||||
entry.readable = false;
|
||||
entry.writable = true;
|
||||
}else if (access == "rw"){
|
||||
entry.readable = true;
|
||||
entry.writable = true;
|
||||
}else if (access == "rwr"){
|
||||
entry.readable = true;
|
||||
entry.writable = true;
|
||||
}else if (access == "rww"){
|
||||
entry.readable = true;
|
||||
entry.writable = true;
|
||||
}else if (access == "const"){
|
||||
entry.readable = true;
|
||||
entry.writable = false;
|
||||
entry.constant = true;
|
||||
}else{
|
||||
THROW_WITH_KEY(ParseException("Cannot determine access"), ObjectDict::Key(entry));
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T> T int_from_string(const std::string &s);
|
||||
|
||||
template<> int8_t int_from_string(const std::string &s){
|
||||
return strtol(s.c_str(), 0, 0);
|
||||
}
|
||||
template<> uint8_t int_from_string(const std::string &s){
|
||||
return strtoul(s.c_str(), 0, 0);
|
||||
}
|
||||
template<> int16_t int_from_string(const std::string &s){
|
||||
return strtol(s.c_str(), 0, 0);
|
||||
}
|
||||
template<> uint16_t int_from_string(const std::string &s){
|
||||
return strtoul(s.c_str(), 0, 0);
|
||||
}
|
||||
template<> int32_t int_from_string(const std::string &s){
|
||||
return strtol(s.c_str(), 0, 0);
|
||||
}
|
||||
template<> uint32_t int_from_string(const std::string &s){
|
||||
return strtoul(s.c_str(), 0, 0);
|
||||
}
|
||||
|
||||
template<> int64_t int_from_string(const std::string &s){
|
||||
return strtoll(s.c_str(), 0, 0);
|
||||
}
|
||||
template<> uint64_t int_from_string(const std::string &s){
|
||||
return strtoull(s.c_str(), 0, 0);
|
||||
}
|
||||
|
||||
template<typename T> HoldAny parse_int(boost::property_tree::iptree &pt, const std::string &key){
|
||||
if(pt.count(key) == 0) return HoldAny(TypeGuard::create<T>());
|
||||
|
||||
std::string str = boost::trim_copy(pt.get<std::string>(key));
|
||||
if(boost::istarts_with(str,"$NODEID")){
|
||||
return HoldAny(NodeIdOffset<T>(int_from_string<T>(boost::trim_copy(str.substr(str.find("+",7)+1)))));
|
||||
}else if (boost::iends_with(str,"$NODEID")){
|
||||
return HoldAny(NodeIdOffset<T>(int_from_string<T>(boost::trim_copy(str.substr(0, str.find("+")+1)))));
|
||||
}else return HoldAny(int_from_string<T>(str));
|
||||
}
|
||||
|
||||
template<typename T> HoldAny parse_octets(boost::property_tree::iptree &pt, const std::string &key){
|
||||
std::string out;
|
||||
if(pt.count(key) == 0 || can::hex2buffer(out,pt.get<std::string>(key), true)) return HoldAny(TypeGuard::create<T>());
|
||||
return HoldAny(T(out));
|
||||
}
|
||||
|
||||
template<typename T> HoldAny parse_typed_value(boost::property_tree::iptree &pt, const std::string &key){
|
||||
if(pt.count(key) == 0) return HoldAny(TypeGuard::create<T>());
|
||||
return HoldAny(pt.get<T>(key));
|
||||
}
|
||||
template<> HoldAny parse_typed_value<String>(boost::property_tree::iptree &pt, const std::string &key){
|
||||
if(pt.count(key) == 0) return HoldAny(TypeGuard::create<String>());
|
||||
return HoldAny(String(pt.get<std::string>(key)));
|
||||
}
|
||||
|
||||
struct ReadAnyValue{
|
||||
template<const ObjectDict::DataTypes dt> static HoldAny func(boost::property_tree::iptree &pt, const std::string &key);
|
||||
static HoldAny read_value(boost::property_tree::iptree &pt, uint16_t data_type, const std::string &key){
|
||||
return branch_type<ReadAnyValue, HoldAny (boost::property_tree::iptree &, const std::string &)>(data_type)(pt, key);
|
||||
}
|
||||
};
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_INTEGER8>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<int8_t>(pt,key); }
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_INTEGER16>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<int16_t>(pt,key); }
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_INTEGER32>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<int32_t>(pt,key); }
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_INTEGER64>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<int64_t>(pt,key); }
|
||||
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_UNSIGNED8>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<uint8_t>(pt,key); }
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_UNSIGNED16>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<uint16_t>(pt,key); }
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_UNSIGNED32>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<uint32_t>(pt,key); }
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_UNSIGNED64>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<uint64_t>(pt,key); }
|
||||
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_DOMAIN>(boost::property_tree::iptree &pt, const std::string &key)
|
||||
{ return parse_octets<ObjectStorage::DataType<ObjectDict::DEFTYPE_DOMAIN>::type>(pt,key); }
|
||||
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_OCTET_STRING>(boost::property_tree::iptree &pt, const std::string &key)
|
||||
{ return parse_octets<ObjectStorage::DataType<ObjectDict::DEFTYPE_OCTET_STRING>::type>(pt,key); }
|
||||
|
||||
template<const ObjectDict::DataTypes dt> HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key){
|
||||
return parse_typed_value<typename ObjectStorage::DataType<dt>::type>(pt, key);
|
||||
}
|
||||
|
||||
template<typename T> void read_optional(T& var, boost::property_tree::iptree &pt, const std::string &key){
|
||||
var = pt.get(key, T());
|
||||
}
|
||||
|
||||
template<typename T> void read_integer(T& var, boost::property_tree::iptree &pt, const std::string &key){
|
||||
var = int_from_string<T>(pt.get<std::string>(key));
|
||||
}
|
||||
|
||||
template<typename T> T read_integer(boost::property_tree::iptree &pt, const std::string &key){
|
||||
return int_from_string<T>(pt.get<std::string>(key, std::string()));
|
||||
}
|
||||
|
||||
|
||||
void read_var(ObjectDict::Entry &entry, boost::property_tree::iptree &object){
|
||||
read_integer<uint16_t>(entry.data_type, object, "DataType");
|
||||
entry.mappable = object.get<bool>("PDOMapping", false);
|
||||
try{
|
||||
set_access(entry, object.get<std::string>("AccessType"));
|
||||
}
|
||||
catch(...){
|
||||
THROW_WITH_KEY(ParseException("No AccessType") , ObjectDict::Key(entry));
|
||||
|
||||
}
|
||||
entry.def_val = ReadAnyValue::read_value(object, entry.data_type, "DefaultValue");
|
||||
entry.init_val = ReadAnyValue::read_value(object, entry.data_type, "ParameterValue");
|
||||
}
|
||||
|
||||
void parse_object(ObjectDictSharedPtr dict, boost::property_tree::iptree &pt, const std::string &name, const uint8_t* sub_index = 0){
|
||||
boost::optional<boost::property_tree::iptree&> object = pt.get_child_optional(name.substr(2));
|
||||
if(!object) return;
|
||||
|
||||
std::shared_ptr<ObjectDict::Entry> entry = std::make_shared<ObjectDict::Entry>();
|
||||
try{
|
||||
entry->index = int_from_string<uint16_t>(name);
|
||||
entry->obj_code = ObjectDict::Code(int_from_string<uint16_t>(object->get<std::string>("ObjectType", boost::lexical_cast<std::string>((uint16_t)ObjectDict::VAR))));
|
||||
entry->desc = object->get<std::string>("Denotation",object->get<std::string>("ParameterName"));
|
||||
|
||||
// std::cout << name << ": "<< entry->desc << std::endl;
|
||||
if(entry->obj_code == ObjectDict::VAR || entry->obj_code == ObjectDict::DOMAIN_DATA || sub_index){
|
||||
entry->sub_index = sub_index? *sub_index: 0;
|
||||
read_var(*entry, *object);
|
||||
dict->insert(sub_index != 0, entry);
|
||||
}else if(entry->obj_code == ObjectDict::ARRAY || entry->obj_code == ObjectDict::RECORD){
|
||||
uint8_t subs = read_integer<uint8_t>(*object, "CompactSubObj");
|
||||
if(subs){ // compact
|
||||
dict->insert(true, std::make_shared<const canopen::ObjectDict::Entry>(entry->index, 0, ObjectDict::DEFTYPE_UNSIGNED8, "NrOfObjects", true, false, false, HoldAny(subs)));
|
||||
|
||||
read_var(*entry, *object);
|
||||
|
||||
for(uint8_t i=1; i< subs; ++i){
|
||||
std::string subname = pt.get<std::string>(name.substr(2)+"Name." + boost::lexical_cast<std::string>((int)i),entry->desc + boost::lexical_cast<std::string>((int)i));
|
||||
subname = pt.get<std::string>(name.substr(2)+"Denotation." + boost::lexical_cast<std::string>((int)i), subname);
|
||||
|
||||
dict->insert(true, std::make_shared<const canopen::ObjectDict::Entry>(entry->index, i, entry->data_type, name, entry->readable, entry->writable, entry->mappable, entry->def_val,
|
||||
ReadAnyValue::read_value(pt, entry->data_type, name.substr(2)+"Value." + boost::lexical_cast<std::string>((int)i))));
|
||||
}
|
||||
}else{
|
||||
read_integer(subs, *object, "SubNumber");
|
||||
for(uint8_t i=0; i< subs; ++i){
|
||||
std::stringstream buf;
|
||||
buf << name << "sub" << std::hex << int(i);
|
||||
// std::cout << "added " << buf.str() << " " << int(i) << std::endl;
|
||||
parse_object(dict, pt, buf.str(), &i);
|
||||
}
|
||||
}
|
||||
}else{
|
||||
THROW_WITH_KEY(ParseException("Object type not supported") , ObjectDict::Key(*entry));
|
||||
}
|
||||
}
|
||||
catch(const std::bad_cast &e){
|
||||
throw ParseException(std::string("Type of ") + name + " does not match or is not supported");
|
||||
}
|
||||
catch(const std::exception&e){
|
||||
throw ParseException(std::string("Cannot process ") + name + ": " + e.what());
|
||||
}
|
||||
}
|
||||
void parse_objects(ObjectDictSharedPtr dict, boost::property_tree::iptree &pt, const std::string &key){
|
||||
if(!pt.count(key)) return;
|
||||
|
||||
boost::property_tree::iptree objects = pt.get_child(key);
|
||||
uint16_t count = read_integer<uint16_t>(objects, "SupportedObjects");
|
||||
for(uint16_t i=0; i < count; ++i){
|
||||
std::string name = objects.get<std::string>(boost::lexical_cast<std::string>(i+1));
|
||||
parse_object(dict, pt, name);
|
||||
}
|
||||
}
|
||||
ObjectDictSharedPtr ObjectDict::fromFile(const std::string &path, const ObjectDict::Overlay &overlay){
|
||||
DeviceInfo info;
|
||||
boost::property_tree::iptree pt;
|
||||
ObjectDictSharedPtr dict;
|
||||
|
||||
boost::property_tree::read_ini(path, pt);
|
||||
|
||||
boost::property_tree::iptree di = pt.get_child("DeviceInfo");
|
||||
|
||||
read_optional(info.vendor_name, di, "VendorName");
|
||||
read_optional(info.vendor_number, di, "VendorNumber");
|
||||
read_optional(info.product_name, di, "ProductName");
|
||||
read_optional(info.product_number, di, "ProductNumber");
|
||||
read_optional(info.revision_number, di, "RevisionNumber");
|
||||
read_optional(info.order_code, di, "OrderCode");
|
||||
read_optional(info.simple_boot_up_master, di, "SimpleBootUpMaster");
|
||||
read_optional(info.simple_boot_up_slave, di, "SimpleBootUpSlave");
|
||||
read_optional(info.granularity, di, "Granularity");
|
||||
read_optional(info.dynamic_channels_supported, di, "DynamicChannelsSupported");
|
||||
read_optional(info.group_messaging, di, "GroupMessaging");
|
||||
read_optional(info.nr_of_rx_pdo, di, "NrOfRXPDO");
|
||||
read_optional(info.nr_of_tx_pdo, di, "NrOfTXPDO");
|
||||
read_optional(info.lss_supported, di, "LSS_Supported");
|
||||
|
||||
std::unordered_set<uint32_t> baudrates;
|
||||
std::unordered_set<uint16_t> dummy_usage;
|
||||
|
||||
for(boost::property_tree::iptree::value_type &v: di){
|
||||
if(v.first.find("BaudRate_") == 0){
|
||||
uint16_t rate = int_from_string<uint16_t>(v.first.substr(9));
|
||||
if(v.second.get_value<bool>())
|
||||
info.baudrates.insert(rate * 1000);
|
||||
}
|
||||
}
|
||||
|
||||
if(pt.count("DummyUsage")){
|
||||
for(boost::property_tree::iptree::value_type &v: pt.get_child("DummyUsage")){
|
||||
if(v.first.find("Dummy") == 0){
|
||||
// std::cout << ("0x"+v.first.substr(5)) << std::endl;
|
||||
uint16_t dummy = int_from_string<uint16_t>("0x"+v.first.substr(5));
|
||||
if(v.second.get_value<bool>())
|
||||
info.dummy_usage.insert(dummy);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
dict = std::make_shared<ObjectDict>(info);
|
||||
|
||||
for(Overlay::const_iterator it= overlay.begin(); it != overlay.end(); ++it){
|
||||
pt.get_child(it->first).put("ParameterValue", it->second);
|
||||
}
|
||||
|
||||
parse_objects(dict, pt, "MandatoryObjects");
|
||||
parse_objects(dict, pt, "OptionalObjects");
|
||||
parse_objects(dict, pt, "ManufacturerObjects");
|
||||
|
||||
return dict;
|
||||
}
|
||||
|
||||
size_t ObjectStorage::map(const ObjectDict::EntryConstSharedPtr &e, const ObjectDict::Key &key, const ReadFunc & read_delegate, const WriteFunc & write_delegate){
|
||||
ObjectStorageMap::iterator it = storage_.find(key);
|
||||
|
||||
if(it == storage_.end()){
|
||||
|
||||
DataSharedPtr data;
|
||||
|
||||
|
||||
if(!e->def_val.type().valid()){
|
||||
THROW_WITH_KEY(std::bad_cast() , key);
|
||||
}
|
||||
|
||||
data = std::make_shared<Data>(key, e,e->def_val.type(),read_delegate_, write_delegate_);
|
||||
|
||||
std::pair<ObjectStorageMap::iterator, bool> ok = storage_.insert(std::make_pair(key, data));
|
||||
it = ok.first;
|
||||
it->second->reset();
|
||||
|
||||
}
|
||||
|
||||
if(read_delegate && write_delegate){
|
||||
it->second->set_delegates(read_delegate_, write_delegate);
|
||||
it->second->force_write(); // update buffer
|
||||
it->second->set_delegates(read_delegate, write_delegate_);
|
||||
}else if(write_delegate) {
|
||||
it->second->set_delegates(read_delegate_, write_delegate);
|
||||
it->second->force_write(); // update buffer
|
||||
}else if(read_delegate){
|
||||
it->second->set_delegates(read_delegate, write_delegate_);
|
||||
}
|
||||
return it->second->size();
|
||||
}
|
||||
|
||||
size_t ObjectStorage::map(uint16_t index, uint8_t sub_index, const ReadFunc & read_delegate, const WriteFunc & write_delegate){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
try{
|
||||
ObjectDict::Key key(index,sub_index);
|
||||
const ObjectDict::EntryConstSharedPtr e = dict_->get(key);
|
||||
return map(e, key,read_delegate, write_delegate);
|
||||
}
|
||||
catch(std::out_of_range) {
|
||||
if(sub_index != 0) throw;
|
||||
|
||||
ObjectDict::Key key(index);
|
||||
const ObjectDict::EntryConstSharedPtr e = dict_->get(key);
|
||||
return map(e, key, read_delegate, write_delegate);
|
||||
}
|
||||
}
|
||||
|
||||
ObjectStorage::ObjectStorage(ObjectDictConstSharedPtr dict, uint8_t node_id, ReadFunc read_delegate, WriteFunc write_delegate)
|
||||
:read_delegate_(read_delegate), write_delegate_(write_delegate), dict_(dict), node_id_(node_id){
|
||||
assert(dict_);
|
||||
assert(read_delegate_);
|
||||
assert(write_delegate_);
|
||||
}
|
||||
|
||||
void ObjectStorage::init_nolock(const ObjectDict::Key &key, const ObjectDict::EntryConstSharedPtr &entry){
|
||||
|
||||
if(!entry->init_val.is_empty()){
|
||||
ObjectStorageMap::iterator it = storage_.find(key);
|
||||
|
||||
if(it == storage_.end()){
|
||||
DataSharedPtr data = std::make_shared<Data>(key,entry, entry->init_val.type(), read_delegate_, write_delegate_);
|
||||
std::pair<ObjectStorageMap::iterator, bool> ok = storage_.insert(std::make_pair(key, data));
|
||||
it = ok.first;
|
||||
if(!ok.second){
|
||||
THROW_WITH_KEY(std::bad_alloc() , key);
|
||||
}
|
||||
}
|
||||
it->second->init();
|
||||
}
|
||||
}
|
||||
void ObjectStorage::init(const ObjectDict::Key &key){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
init_nolock(key, dict_->get(key));
|
||||
}
|
||||
void ObjectStorage::init_all(){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
ObjectDict::ObjectDictMap::const_iterator entry_it;
|
||||
while(dict_->iterate(entry_it)){
|
||||
init_nolock(entry_it->first, entry_it->second);
|
||||
}
|
||||
}
|
||||
|
||||
void ObjectStorage::reset(){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
for(ObjectStorageMap::iterator it = storage_.begin(); it != storage_.end(); ++it){
|
||||
it->second->reset();
|
||||
}
|
||||
}
|
||||
|
||||
template<const ObjectDict::DataTypes dt, typename T> std::string formatValue(const T &value){
|
||||
std::stringstream sstr;
|
||||
sstr << value;
|
||||
return sstr.str();
|
||||
}
|
||||
template<> std::string formatValue<ObjectDict::DEFTYPE_DOMAIN>(const std::string &value){
|
||||
return can::buffer2hex(value, false);
|
||||
}
|
||||
template<> std::string formatValue<ObjectDict::DEFTYPE_OCTET_STRING>(const std::string &value){
|
||||
return can::buffer2hex(value, false);
|
||||
}
|
||||
|
||||
struct PrintValue {
|
||||
template<const ObjectDict::DataTypes dt> static std::string func(ObjectStorage& storage, const ObjectDict::Key &key, bool cached){
|
||||
ObjectStorage::Entry<typename ObjectStorage::DataType<dt>::type> entry = storage.entry<typename ObjectStorage::DataType<dt>::type>(key);
|
||||
return formatValue<dt>(cached? entry.get_cached() : entry.get() );
|
||||
}
|
||||
static std::function<std::string()> getReader(ObjectStorage& storage, const ObjectDict::Key &key, bool cached){
|
||||
ObjectDict::DataTypes data_type = (ObjectDict::DataTypes) storage.dict_->get(key)->data_type;
|
||||
return std::bind(branch_type<PrintValue, std::string (ObjectStorage&, const ObjectDict::Key &, bool)>(data_type),std::ref(storage), key, cached);
|
||||
}
|
||||
};
|
||||
|
||||
ObjectStorage::ReadStringFuncType ObjectStorage::getStringReader(const ObjectDict::Key &key, bool cached){
|
||||
return PrintValue::getReader(*this, key, cached);
|
||||
}
|
||||
|
||||
struct WriteStringValue {
|
||||
typedef HoldAny (*reader_type)(boost::property_tree::iptree &, const std::string &);
|
||||
template<typename T> static void write(ObjectStorage::Entry<T> entry, bool cached, reader_type reader, const std::string &value){
|
||||
boost::property_tree::iptree pt;
|
||||
pt.put("value", value);
|
||||
HoldAny any = reader(pt, "value");
|
||||
if(cached){
|
||||
entry.set_cached(any.get<T>());
|
||||
} else {
|
||||
entry.set(any.get<T>());
|
||||
}
|
||||
}
|
||||
template<const ObjectDict::DataTypes dt> static std::function<void (const std::string&)> func(ObjectStorage& storage, const ObjectDict::Key &key, bool cached){
|
||||
ObjectStorage::Entry<typename ObjectStorage::DataType<dt>::type> entry = storage.entry<typename ObjectStorage::DataType<dt>::type>(key);
|
||||
reader_type reader = branch_type<ReadAnyValue, HoldAny (boost::property_tree::iptree &, const std::string &)>(dt);
|
||||
return std::bind(&WriteStringValue::write<typename ObjectStorage::DataType<dt>::type >, entry, cached, reader, std::placeholders::_1);
|
||||
}
|
||||
static std::function<void (const std::string&)> getWriter(ObjectStorage& storage, const ObjectDict::Key &key, bool cached){
|
||||
ObjectDict::DataTypes data_type = (ObjectDict::DataTypes) storage.dict_->get(key)->data_type;
|
||||
return branch_type<WriteStringValue, std::function<void (const std::string&)> (ObjectStorage&, const ObjectDict::Key &, bool)>(data_type)(storage, key, cached);
|
||||
}
|
||||
};
|
||||
|
||||
ObjectStorage::WriteStringFuncType ObjectStorage::getStringWriter(const ObjectDict::Key &key, bool cached){
|
||||
return WriteStringValue::getWriter(*this, key, cached);
|
||||
}
|
||||
383
Devices/Libraries/Ros/ros_canopen/canopen_master/src/pdo.cpp
Executable file
383
Devices/Libraries/Ros/ros_canopen/canopen_master/src/pdo.cpp
Executable file
@@ -0,0 +1,383 @@
|
||||
#include <canopen_master/canopen.h>
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
#pragma pack(push) /* push current alignment to stack */
|
||||
#pragma pack(1) /* set alignment to 1 byte boundary */
|
||||
|
||||
class PDOid{
|
||||
const uint32_t value_;
|
||||
public:
|
||||
static const unsigned int ID_MASK = (1u << 29)-1;
|
||||
static const unsigned int EXTENDED_MASK = (1u << 29);
|
||||
static const unsigned int NO_RTR_MASK = (1u << 30);
|
||||
static const unsigned int INVALID_MASK = (1u << 31);
|
||||
|
||||
PDOid(const uint32_t &val)
|
||||
: value_(val)
|
||||
{}
|
||||
can::Header header(bool fill_rtr = false) const {
|
||||
return can::Header(value_ & ID_MASK, value_ & EXTENDED_MASK, fill_rtr && !(value_ & NO_RTR_MASK), false);
|
||||
}
|
||||
bool isInvalid() const { return value_ & INVALID_MASK; }
|
||||
};
|
||||
|
||||
struct PDOmap{
|
||||
uint8_t length;
|
||||
uint8_t sub_index;
|
||||
uint16_t index;
|
||||
PDOmap(uint32_t val)
|
||||
: length(val & 0xFF),
|
||||
sub_index((val>>8) & 0xFF),
|
||||
index(val>>16)
|
||||
{}
|
||||
};
|
||||
|
||||
#pragma pack(pop) /* pop previous alignment from stack */
|
||||
|
||||
|
||||
const uint8_t SUB_COM_NUM = 0;
|
||||
const uint8_t SUB_COM_COB_ID = 1;
|
||||
const uint8_t SUB_COM_TRANSMISSION_TYPE = 2;
|
||||
const uint8_t SUB_COM_RESERVED = 4;
|
||||
|
||||
const uint8_t SUB_MAP_NUM = 0;
|
||||
|
||||
const uint16_t RPDO_COM_BASE =0x1400;
|
||||
const uint16_t RPDO_MAP_BASE =0x1600;
|
||||
const uint16_t TPDO_COM_BASE =0x1800;
|
||||
const uint16_t TPDO_MAP_BASE =0x1A00;
|
||||
|
||||
bool check_com_changed(const ObjectDict &dict, const uint16_t com_id){
|
||||
bool com_changed = false;
|
||||
|
||||
// check if com parameter has to be set
|
||||
for(uint8_t sub = 0; sub <=6 ; ++sub){
|
||||
try{
|
||||
if(!dict(com_id,sub).init_val.is_empty()){
|
||||
com_changed = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
catch (std::out_of_range) {}
|
||||
}
|
||||
return com_changed;
|
||||
}
|
||||
|
||||
bool check_map_changed(const uint8_t &num, const ObjectDict &dict, const uint16_t &map_index){
|
||||
bool map_changed = false;
|
||||
|
||||
// check if mapping has to be set
|
||||
if(num <= 0x40){
|
||||
for(uint8_t sub = 1; sub <=num ; ++sub){
|
||||
try{
|
||||
if(!dict(map_index,sub).init_val.is_empty()){
|
||||
map_changed = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
catch (std::out_of_range) {}
|
||||
}
|
||||
}else{
|
||||
map_changed = dict( map_index ,0 ).init_val.is_empty();
|
||||
}
|
||||
return map_changed;
|
||||
}
|
||||
void PDOMapper::PDO::parse_and_set_mapping(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index, const bool &read, const bool &write){
|
||||
|
||||
const canopen::ObjectDict & dict = *storage->dict_;
|
||||
|
||||
ObjectStorage::Entry<uint8_t> num_entry;
|
||||
storage->entry(num_entry, map_index, SUB_MAP_NUM);
|
||||
|
||||
uint8_t map_num;
|
||||
|
||||
try{
|
||||
map_num = num_entry.desc().value().get<uint8_t>();
|
||||
}catch(...){
|
||||
map_num = 0;
|
||||
}
|
||||
|
||||
bool map_changed = check_map_changed(map_num, dict, map_index);
|
||||
|
||||
// disable PDO if needed
|
||||
ObjectStorage::Entry<uint32_t> cob_id;
|
||||
storage->entry(cob_id, com_index, SUB_COM_COB_ID);
|
||||
|
||||
bool com_changed = check_com_changed(dict, map_index);
|
||||
if((map_changed || com_changed) && cob_id.desc().writable){
|
||||
cob_id.set(cob_id.get() | PDOid::INVALID_MASK);
|
||||
}
|
||||
if(map_num > 0 && map_num <= 0x40){ // actual mapping
|
||||
if(map_changed){
|
||||
num_entry.set(0);
|
||||
}
|
||||
|
||||
frame.dlc = 0;
|
||||
for(uint8_t sub = 1; sub <=map_num; ++sub){
|
||||
ObjectStorage::Entry<uint32_t> mapentry;
|
||||
storage->entry(mapentry, map_index, sub);
|
||||
const HoldAny init = dict(map_index ,sub).init_val;
|
||||
if(!init.is_empty()) mapentry.set(init.get<uint32_t>());
|
||||
|
||||
PDOmap param(mapentry.get_cached());
|
||||
BufferSharedPtr b = std::make_shared<Buffer>(param.length/8);
|
||||
if(param.index < 0x1000){
|
||||
// TODO: check DummyUsage
|
||||
}else{
|
||||
ObjectStorage::ReadFunc rd;
|
||||
ObjectStorage::WriteFunc wd;
|
||||
|
||||
if(read){
|
||||
rd = std::bind<void(Buffer::*)(const canopen::ObjectDict::Entry&, String&)>(&Buffer::read, b.get(), std::placeholders::_1, std::placeholders::_2);
|
||||
}
|
||||
if(read || write)
|
||||
{
|
||||
wd = std::bind<void(Buffer::*)(const canopen::ObjectDict::Entry&, const String&)>(&Buffer::write, b.get(), std::placeholders::_1, std::placeholders::_2);
|
||||
size_t l = storage->map(param.index, param.sub_index, rd, wd);
|
||||
assert(l == param.length/8);
|
||||
}
|
||||
}
|
||||
|
||||
frame.dlc += b->size;
|
||||
assert( frame.dlc <= 8 );
|
||||
b->clean();
|
||||
buffers.push_back(b);
|
||||
}
|
||||
}
|
||||
if(com_changed){
|
||||
uint8_t subs = dict(com_index, SUB_COM_NUM).value().get<uint8_t>();
|
||||
for(uint8_t i = SUB_COM_NUM+1; i <= subs; ++i){
|
||||
if(i == SUB_COM_COB_ID || i == SUB_COM_RESERVED) continue;
|
||||
try{
|
||||
storage->init(ObjectDict::Key(com_index, i));
|
||||
}
|
||||
catch (const std::out_of_range &){
|
||||
// entry was not provided, so skip it
|
||||
}
|
||||
}
|
||||
}
|
||||
if(map_changed){
|
||||
num_entry.set(map_num);
|
||||
}
|
||||
if((com_changed || map_changed) && cob_id.desc().writable){
|
||||
storage->init(ObjectDict::Key(com_index, SUB_COM_COB_ID));
|
||||
|
||||
cob_id.set(NodeIdOffset<uint32_t>::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_));
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
PDOMapper::PDOMapper(const can::CommInterfaceSharedPtr interface)
|
||||
:interface_(interface)
|
||||
{
|
||||
}
|
||||
bool PDOMapper::init(const ObjectStorageSharedPtr storage, LayerStatus &status){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
try{
|
||||
rpdos_.clear();
|
||||
|
||||
const canopen::ObjectDict & dict = *storage->dict_;
|
||||
for(uint16_t i=0; i < 512 && rpdos_.size() < dict.device_info.nr_of_tx_pdo;++i){ // TPDOs of device
|
||||
if(!dict.has(TPDO_COM_BASE + i,0) && !dict.has(TPDO_MAP_BASE + i,0)) continue;
|
||||
|
||||
RPDO::RPDOSharedPtr rpdo = RPDO::create(interface_,storage, TPDO_COM_BASE + i, TPDO_MAP_BASE + i);
|
||||
if(rpdo){
|
||||
rpdos_.insert(rpdo);
|
||||
}
|
||||
}
|
||||
// ROSCANOPEN_DEBUG("canopen_master", "RPDOs: " << rpdos_.size());
|
||||
|
||||
tpdos_.clear();
|
||||
for(uint16_t i=0; i < 512 && tpdos_.size() < dict.device_info.nr_of_rx_pdo;++i){ // RPDOs of device
|
||||
if(!dict.has(RPDO_COM_BASE + i,0) && !dict.has(RPDO_MAP_BASE + i,0)) continue;
|
||||
|
||||
TPDO::TPDOSharedPtr tpdo = TPDO::create(interface_,storage, RPDO_COM_BASE + i, RPDO_MAP_BASE + i);
|
||||
if(tpdo){
|
||||
tpdos_.insert(tpdo);
|
||||
}
|
||||
}
|
||||
// ROSCANOPEN_DEBUG("canopen_master", "TPDOs: " << tpdos_.size());
|
||||
|
||||
return true;
|
||||
}
|
||||
catch(const std::out_of_range &e){
|
||||
status.error(std::string("PDO error: ") + e.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool PDOMapper::RPDO::init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
listener_.reset();
|
||||
const canopen::ObjectDict & dict = *storage->dict_;
|
||||
parse_and_set_mapping(storage, com_index, map_index, true, false);
|
||||
|
||||
PDOid pdoid( NodeIdOffset<uint32_t>::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_) );
|
||||
|
||||
if(buffers.empty() || pdoid.isInvalid()){
|
||||
return false;
|
||||
}
|
||||
|
||||
frame = pdoid.header(true);
|
||||
|
||||
transmission_type = dict(com_index, SUB_COM_TRANSMISSION_TYPE).value().get<uint8_t>();
|
||||
|
||||
listener_ = interface_->createMsgListenerM(pdoid.header(), this, &RPDO::handleFrame);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PDOMapper::TPDO::init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
const canopen::ObjectDict & dict = *storage->dict_;
|
||||
|
||||
|
||||
PDOid pdoid( NodeIdOffset<uint32_t>::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_) );
|
||||
frame = pdoid.header();
|
||||
|
||||
parse_and_set_mapping(storage, com_index, map_index, false, true);
|
||||
if(buffers.empty() || pdoid.isInvalid()){
|
||||
return false;
|
||||
}
|
||||
ObjectStorage::Entry<uint8_t> tt;
|
||||
storage->entry(tt, com_index, SUB_COM_TRANSMISSION_TYPE);
|
||||
transmission_type = tt.desc().value().get<uint8_t>();
|
||||
|
||||
if(transmission_type != 1 && transmission_type <=240){
|
||||
tt.set(1); // enforce 1 for compatibility
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void PDOMapper::TPDO::sync(){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
|
||||
bool updated = false;
|
||||
size_t len = frame.dlc;
|
||||
can::Frame::value_type * dest = frame.c_array();
|
||||
for(std::vector< BufferSharedPtr >::iterator b_it = buffers.begin(); b_it != buffers.end(); ++b_it){
|
||||
Buffer &b = **b_it;
|
||||
if(len >= b.size){
|
||||
updated = b.read(dest, len) || updated;
|
||||
len -= b.size;
|
||||
dest += b.size;
|
||||
}else{
|
||||
// ERROR
|
||||
}
|
||||
}
|
||||
|
||||
if( len != 0){
|
||||
// ERROR
|
||||
}
|
||||
if(updated){
|
||||
interface_->send( frame );
|
||||
}else{
|
||||
// TODO: Notify
|
||||
}
|
||||
}
|
||||
|
||||
void PDOMapper::RPDO::sync(LayerStatus &status){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if((transmission_type >= 1 && transmission_type <= 240) || transmission_type == 0xFC){ // cyclic
|
||||
if(timeout > 0){
|
||||
--timeout;
|
||||
}else if(timeout == 0) {
|
||||
status.warn("RPDO timeout");
|
||||
}
|
||||
}
|
||||
if(transmission_type == 0xFC || transmission_type == 0xFD){
|
||||
if(frame.is_rtr){
|
||||
interface_->send(frame);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PDOMapper::RPDO::handleFrame(const can::Frame & msg){
|
||||
size_t offset = 0;
|
||||
const uint8_t * src = msg.data.data();
|
||||
for(std::vector<BufferSharedPtr >::iterator it = buffers.begin(); it != buffers.end(); ++it){
|
||||
Buffer &b = **it;
|
||||
|
||||
if( offset + b.size <= msg.dlc ){
|
||||
b.write(src+offset, b.size);
|
||||
offset += b.size;
|
||||
}else{
|
||||
// ERROR
|
||||
}
|
||||
}
|
||||
if( offset != msg.dlc ){
|
||||
// ERROR
|
||||
}
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(transmission_type >= 1 && transmission_type <= 240){
|
||||
timeout = transmission_type + 2;
|
||||
}else if(transmission_type == 0xFC || transmission_type == 0xFD){
|
||||
if(frame.is_rtr){
|
||||
timeout = 1+2;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PDOMapper::read(LayerStatus &status){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
for(std::unordered_set<RPDO::RPDOSharedPtr >::iterator it = rpdos_.begin(); it != rpdos_.end(); ++it){
|
||||
(*it)->sync(status);
|
||||
}
|
||||
}
|
||||
bool PDOMapper::write(){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
for(std::unordered_set<TPDO::TPDOSharedPtr >::iterator it = tpdos_.begin(); it != tpdos_.end(); ++it){
|
||||
(*it)->sync();
|
||||
}
|
||||
return true; // TODO: check for errors
|
||||
}
|
||||
|
||||
bool PDOMapper::Buffer::read(uint8_t* b, const size_t len){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(size > len){
|
||||
BOOST_THROW_EXCEPTION( std::bad_cast() );
|
||||
}
|
||||
if(empty) return false;
|
||||
|
||||
memcpy(b,&buffer[0], size);
|
||||
bool was_dirty = dirty;
|
||||
dirty = false;
|
||||
return was_dirty;
|
||||
}
|
||||
void PDOMapper::Buffer::write(const uint8_t* b, const size_t len){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(size > len){
|
||||
BOOST_THROW_EXCEPTION( std::bad_cast() );
|
||||
}
|
||||
empty = false;
|
||||
dirty = true;
|
||||
memcpy(&buffer[0], b, size);
|
||||
}
|
||||
void PDOMapper::Buffer::read(const canopen::ObjectDict::Entry &entry, String &data){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
time_point abs_time = get_abs_time(boost::chrono::seconds(1));
|
||||
if(size != data.size()){
|
||||
THROW_WITH_KEY(std::bad_cast(), ObjectDict::Key(entry));
|
||||
}
|
||||
if(empty){
|
||||
THROW_WITH_KEY(TimeoutException("PDO data empty"), ObjectDict::Key(entry));
|
||||
}
|
||||
if(dirty){
|
||||
data.assign(buffer.begin(), buffer.end());
|
||||
dirty = false;
|
||||
}
|
||||
}
|
||||
void PDOMapper::Buffer::write(const canopen::ObjectDict::Entry &entry, const String &data){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(size != data.size()){
|
||||
THROW_WITH_KEY(std::bad_cast(), ObjectDict::Key(entry));
|
||||
}
|
||||
empty = false;
|
||||
dirty = true;
|
||||
buffer.assign(data.begin(),data.end());
|
||||
}
|
||||
451
Devices/Libraries/Ros/ros_canopen/canopen_master/src/sdo.cpp
Executable file
451
Devices/Libraries/Ros/ros_canopen/canopen_master/src/sdo.cpp
Executable file
@@ -0,0 +1,451 @@
|
||||
#include <canopen_master/canopen.h>
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
const uint8_t COMMAND_MASK = (1<<7) | (1<<6) | (1<<5);
|
||||
const uint8_t INITIATE_DOWNLOAD_REQUEST = (0 << 5);
|
||||
const uint8_t INITIATE_DOWNLOAD_RESPONSE = (1 << 5);
|
||||
const uint8_t DOWNLOAD_SEGMENT_REQUEST = (1 << 5);
|
||||
const uint8_t DOWNLOAD_SEGMENT_RESPONSE = (3 << 5);
|
||||
const uint8_t INITIATE_UPLOAD_REQUEST = (2 << 5);
|
||||
const uint8_t INITIATE_UPLOAD_RESPONSE = (2 << 5);
|
||||
const uint8_t UPLOAD_SEGMENT_REQUEST = (3 << 5);
|
||||
const uint8_t UPLOAD_SEGMENT_RESPONSE = (0 << 5);
|
||||
const uint8_t ABORT_TRANSFER_REQUEST = (4 << 5);
|
||||
|
||||
|
||||
#pragma pack(push) /* push current alignment to stack */
|
||||
#pragma pack(1) /* set alignment to 1 byte boundary */
|
||||
|
||||
struct SDOid{
|
||||
uint32_t id:29;
|
||||
uint32_t extended:1;
|
||||
uint32_t dynamic:1;
|
||||
uint32_t invalid:1;
|
||||
SDOid(uint32_t val){
|
||||
*(uint32_t*) this = val;
|
||||
}
|
||||
can::Header header() {
|
||||
return can::Header(id, extended, false, false);
|
||||
}
|
||||
};
|
||||
|
||||
struct InitiateShort{
|
||||
uint8_t :5;
|
||||
uint8_t command:3;
|
||||
uint16_t index;
|
||||
uint8_t sub_index;
|
||||
uint8_t reserved[4];
|
||||
};
|
||||
|
||||
struct InitiateLong{
|
||||
uint8_t size_indicated:1;
|
||||
uint8_t expedited:1;
|
||||
uint8_t num:2;
|
||||
uint8_t :1;
|
||||
uint8_t command:3;
|
||||
uint16_t index;
|
||||
uint8_t sub_index;
|
||||
uint8_t payload[4];
|
||||
|
||||
size_t data_size(){
|
||||
if(expedited && size_indicated) return 4-num;
|
||||
else if(!expedited && size_indicated) return payload[0] | (payload[3]<<8);
|
||||
else return 0;
|
||||
}
|
||||
size_t apply_buffer(const String &buffer){
|
||||
size_t size = buffer.size();
|
||||
size_indicated = 1;
|
||||
if(size > 4){
|
||||
expedited = 0;
|
||||
payload[0] = size & 0xFF;
|
||||
payload[3] = (size >> 8) & 0xFF;
|
||||
return 0;
|
||||
}else{
|
||||
expedited = 1;
|
||||
size_indicated = 1;
|
||||
num = 4-size;
|
||||
memcpy(payload, buffer.data(), size);
|
||||
return size;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
struct SegmentShort{
|
||||
uint8_t :4;
|
||||
uint8_t toggle:1;
|
||||
uint8_t command:3;
|
||||
uint8_t reserved[7];
|
||||
};
|
||||
|
||||
struct SegmentLong{
|
||||
uint8_t done:1;
|
||||
uint8_t num:3;
|
||||
uint8_t toggle:1;
|
||||
uint8_t command:3;
|
||||
uint8_t payload[7];
|
||||
size_t data_size(){
|
||||
return 7-num;
|
||||
}
|
||||
size_t apply_buffer(const String &buffer, const size_t offset){
|
||||
size_t size = buffer.size() - offset;
|
||||
if(size > 7) size = 7;
|
||||
else done = 1;
|
||||
num = 7 - size;
|
||||
memcpy(payload, buffer.data() + offset, size);
|
||||
return offset + size;
|
||||
}
|
||||
};
|
||||
|
||||
struct DownloadInitiateRequest: public FrameOverlay<InitiateLong>{
|
||||
static const uint8_t command = 1;
|
||||
|
||||
DownloadInitiateRequest(const Header &h, const canopen::ObjectDict::Entry &entry, const String &buffer, size_t &offset) : FrameOverlay(h) {
|
||||
data.command = command;
|
||||
data.index = entry.index;
|
||||
data.sub_index = entry.sub_index;
|
||||
offset = data.apply_buffer(buffer);
|
||||
}
|
||||
DownloadInitiateRequest(const can::Frame &f) : FrameOverlay(f){ }
|
||||
};
|
||||
|
||||
struct DownloadInitiateResponse: public FrameOverlay<InitiateShort>{
|
||||
static const uint8_t command = 3;
|
||||
|
||||
DownloadInitiateResponse(const can::Frame &f) : FrameOverlay(f){ }
|
||||
|
||||
bool test(const can::Frame &msg, uint32_t &reason){
|
||||
DownloadInitiateRequest req(msg);
|
||||
if(req.data.command == DownloadInitiateRequest::command && data.index == req.data.index && data.sub_index == req.data.sub_index){
|
||||
return true;
|
||||
}
|
||||
reason = 0x08000000; // General error
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
struct DownloadSegmentRequest: public FrameOverlay<SegmentLong>{
|
||||
static const uint8_t command = 0;
|
||||
|
||||
DownloadSegmentRequest(const can::Frame &f) : FrameOverlay(f){ }
|
||||
|
||||
DownloadSegmentRequest(const Header &h, bool toggle, const String &buffer, size_t& offset) : FrameOverlay(h) {
|
||||
data.command = command;
|
||||
data.toggle = toggle?1:0;
|
||||
offset = data.apply_buffer(buffer, offset);
|
||||
}
|
||||
};
|
||||
|
||||
struct DownloadSegmentResponse : public FrameOverlay<SegmentShort>{
|
||||
static const uint8_t command = 1;
|
||||
DownloadSegmentResponse(const can::Frame &f) : FrameOverlay(f) {
|
||||
}
|
||||
bool test(const can::Frame &msg, uint32_t &reason){
|
||||
DownloadSegmentRequest req(msg);
|
||||
if (req.data.command != DownloadSegmentRequest::command){
|
||||
reason = 0x08000000; // General error
|
||||
return false;
|
||||
}else if( data.toggle != req.data.toggle){
|
||||
reason = 0x05030000; // Toggle bit not alternated
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
struct UploadInitiateRequest: public FrameOverlay<InitiateShort>{
|
||||
static const uint8_t command = 2;
|
||||
UploadInitiateRequest(const Header &h, const canopen::ObjectDict::Entry &entry) : FrameOverlay(h) {
|
||||
data.command = command;
|
||||
data.index = entry.index;
|
||||
data.sub_index = entry.sub_index;
|
||||
}
|
||||
UploadInitiateRequest(const can::Frame &f) : FrameOverlay(f){ }
|
||||
};
|
||||
|
||||
struct UploadInitiateResponse: public FrameOverlay<InitiateLong>{
|
||||
static const uint8_t command = 2;
|
||||
UploadInitiateResponse(const can::Frame &f) : FrameOverlay(f) { }
|
||||
bool test(const can::Frame &msg, size_t size, uint32_t &reason){
|
||||
UploadInitiateRequest req(msg);
|
||||
if(req.data.command == UploadInitiateRequest::command && data.index == req.data.index && data.sub_index == req.data.sub_index){
|
||||
size_t ds = data.data_size();
|
||||
if(ds == 0 || size == 0 || ds >= size) { // should be ==, but >= is needed for Elmo, it responses with more byte than requested
|
||||
if(!data.expedited || (ds <= 4 && size <= 4)) return true;
|
||||
}else{
|
||||
reason = 0x06070010; // Data type does not match, length of service parameter does not match
|
||||
return false;
|
||||
}
|
||||
}
|
||||
reason = 0x08000000; // General error
|
||||
return false;
|
||||
}
|
||||
bool read_data(String & buffer, size_t & offset, size_t & total){
|
||||
if(data.size_indicated && total == 0){
|
||||
total = data.data_size();
|
||||
buffer.resize(total);
|
||||
}
|
||||
if(data.expedited){
|
||||
memcpy(&buffer.front(), data.payload, buffer.size());
|
||||
offset = buffer.size();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
struct UploadSegmentRequest: public FrameOverlay<SegmentShort>{
|
||||
static const uint8_t command = 3;
|
||||
UploadSegmentRequest(const Header &h, bool toggle) : FrameOverlay(h) {
|
||||
data.command = command;
|
||||
data.toggle = toggle?1:0;
|
||||
}
|
||||
UploadSegmentRequest(const can::Frame &f) : FrameOverlay(f) { }
|
||||
};
|
||||
|
||||
struct UploadSegmentResponse : public FrameOverlay<SegmentLong>{
|
||||
static const uint8_t command = 0;
|
||||
UploadSegmentResponse(const can::Frame &f) : FrameOverlay(f) {
|
||||
}
|
||||
bool test(const can::Frame &msg, uint32_t &reason){
|
||||
UploadSegmentRequest req(msg);
|
||||
if(req.data.command != UploadSegmentRequest::command){
|
||||
reason = 0x08000000; // General error
|
||||
return false;
|
||||
}else if( data.toggle != req.data.toggle){
|
||||
reason = 0x05030000; // Toggle bit not alternated
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
bool read_data(String & buffer, size_t & offset, const size_t & total){
|
||||
uint32_t n = data.data_size();
|
||||
if(total == 0){
|
||||
buffer.resize(offset + n);
|
||||
}
|
||||
if(offset + n <= buffer.size()){
|
||||
memcpy(&buffer[offset], data.payload, n);
|
||||
offset += n;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
struct AbortData{
|
||||
uint8_t :5;
|
||||
uint8_t command:3;
|
||||
uint16_t index;
|
||||
uint8_t sub_index;
|
||||
uint32_t reason;
|
||||
|
||||
const char * text(){
|
||||
switch(reason){
|
||||
case 0x05030000: return "Toggle bit not alternated.";
|
||||
case 0x05040000: return "SDO protocol timed out.";
|
||||
case 0x05040001: return "Client/server command specifier not valid or unknown.";
|
||||
case 0x05040002: return "Invalid block size (block mode only).";
|
||||
case 0x05040003: return "Invalid sequence number (block mode only).";
|
||||
case 0x05040004: return "CRC error (block mode only).";
|
||||
case 0x05040005: return "Out of memory.";
|
||||
case 0x06010000: return "Unsupported access to an object.";
|
||||
case 0x06010001: return "Attempt to read a write only object.";
|
||||
case 0x06010002: return "Attempt to write a read only object.";
|
||||
case 0x06020000: return "Object does not exist in the object dictionary.";
|
||||
case 0x06040041: return "Object cannot be mapped to the PDO.";
|
||||
case 0x06040042: return "The number and length of the objects to be mapped would exceed PDO length.";
|
||||
case 0x06040043: return "General parameter incompatibility reason.";
|
||||
case 0x06040047: return "General internal incompatibility in the device.";
|
||||
case 0x06060000: return "Access failed due to an hardware error.";
|
||||
case 0x06070010: return "Data type does not match, length of service parameter does not match";
|
||||
case 0x06070012: return "Data type does not match, length of service parameter too high";
|
||||
case 0x06070013: return "Data type does not match, length of service parameter too low";
|
||||
case 0x06090011: return "Sub-index does not exist.";
|
||||
case 0x06090030: return "Invalid value for parameter (download only).";
|
||||
case 0x06090031: return "Value of parameter written too high (download only).";
|
||||
case 0x06090032: return "Value of parameter written too low (download only).";
|
||||
case 0x06090036: return "Maximum value is less than minimum value.";
|
||||
case 0x060A0023: return "Resource not available: SDO connection";
|
||||
case 0x08000000: return "General error";
|
||||
case 0x08000020: return "Data cannot be transferred or stored to the application.";
|
||||
case 0x08000021: return "Data cannot be transferred or stored to the application because of local control.";
|
||||
case 0x08000022: return "Data cannot be transferred or stored to the application because of the present device state.";
|
||||
case 0x08000023: return "Object dictionary dynamic generation fails or no object dictionary is present (e.g.object dictionary is generated from file and generation fails because of an file error).";
|
||||
case 0x08000024: return "No data available";
|
||||
default: return "Abort code is reserved";
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
struct AbortTranserRequest: public FrameOverlay<AbortData>{
|
||||
static const uint8_t command = 4;
|
||||
AbortTranserRequest(const can::Frame &f) : FrameOverlay(f) {}
|
||||
AbortTranserRequest(const Header &h, uint16_t index, uint8_t sub_index, uint32_t reason) : FrameOverlay(h) {
|
||||
data.command = command;
|
||||
data.index = index;
|
||||
data.sub_index = sub_index;
|
||||
data.reason = reason;
|
||||
}
|
||||
};
|
||||
|
||||
#pragma pack(pop) /* pop previous alignment from stack */
|
||||
|
||||
void SDOClient::abort(uint32_t reason){
|
||||
if(current_entry){
|
||||
interface_->send(last_msg = AbortTranserRequest(client_id, current_entry->index, current_entry->sub_index, reason));
|
||||
}
|
||||
}
|
||||
|
||||
bool SDOClient::processFrame(const can::Frame & msg){
|
||||
if(msg.dlc != 8) return false;
|
||||
|
||||
uint32_t reason = 0;
|
||||
switch(msg.data[0] >> 5){
|
||||
case DownloadInitiateResponse::command:
|
||||
{
|
||||
DownloadInitiateResponse resp(msg);
|
||||
if(resp.test(last_msg, reason) ){
|
||||
if(offset < total){
|
||||
interface_->send(last_msg = DownloadSegmentRequest(client_id, false, buffer, offset));
|
||||
}else{
|
||||
done = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case DownloadSegmentResponse::command:
|
||||
{
|
||||
DownloadSegmentResponse resp(msg);
|
||||
if( resp.test(last_msg, reason) ){
|
||||
if(offset < total){
|
||||
interface_->send(last_msg = DownloadSegmentRequest(client_id, !resp.data.toggle, buffer, offset));
|
||||
}else{
|
||||
done = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case UploadInitiateResponse::command:
|
||||
{
|
||||
UploadInitiateResponse resp(msg);
|
||||
if( resp.test(last_msg, total, reason) ){
|
||||
if(resp.read_data(buffer, offset, total)){
|
||||
done = true;
|
||||
}else{
|
||||
interface_->send(last_msg = UploadSegmentRequest(client_id, false));
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case UploadSegmentResponse::command:
|
||||
{
|
||||
UploadSegmentResponse resp(msg);
|
||||
if( resp.test(last_msg, reason) ){
|
||||
if(resp.read_data(buffer, offset, total)){
|
||||
if(resp.data.done || offset == total){
|
||||
done = true;
|
||||
}else{
|
||||
interface_->send(last_msg = UploadSegmentRequest(client_id, !resp.data.toggle));
|
||||
}
|
||||
}else{
|
||||
// abort, size mismatch
|
||||
ROSCANOPEN_ERROR("canopen_master", "abort, size mismatch" << buffer.size() << " " << resp.data.data_size());
|
||||
reason = 0x06070010; // Data type does not match, length of service parameter does not match
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case AbortTranserRequest::command:
|
||||
ROSCANOPEN_ERROR("canopen_master", "abort" << std::hex << (uint32_t) AbortTranserRequest(msg).data.index << "#"<< std::dec << (uint32_t) AbortTranserRequest(msg).data.sub_index << ", reason: " << AbortTranserRequest(msg).data.text());
|
||||
offset = 0;
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
if(reason){
|
||||
abort(reason);
|
||||
offset = 0;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
void SDOClient::init(){
|
||||
assert(storage_);
|
||||
assert(interface_);
|
||||
const canopen::ObjectDict & dict = *storage_->dict_;
|
||||
|
||||
try{
|
||||
client_id = SDOid(NodeIdOffset<uint32_t>::apply(dict(0x1200, 1).value(), storage_->node_id_)).header();
|
||||
}
|
||||
catch(...){
|
||||
client_id = can::MsgHeader(0x600+ storage_->node_id_);
|
||||
}
|
||||
|
||||
last_msg = AbortTranserRequest(client_id, 0,0,0);
|
||||
current_entry = 0;
|
||||
|
||||
can::Header server_id;
|
||||
try{
|
||||
server_id = SDOid(NodeIdOffset<uint32_t>::apply(dict(0x1200, 2).value(), storage_->node_id_)).header();
|
||||
}
|
||||
catch(...){
|
||||
server_id = can::MsgHeader(0x580+ storage_->node_id_);
|
||||
}
|
||||
reader_.listen(interface_, server_id);
|
||||
}
|
||||
|
||||
void SDOClient::transmitAndWait(const canopen::ObjectDict::Entry &entry, const String &data, String *result){
|
||||
buffer = data;
|
||||
offset = 0;
|
||||
total = buffer.size();
|
||||
current_entry = &entry;
|
||||
done = false;
|
||||
|
||||
can::BufferedReader::ScopedEnabler enabler(reader_);
|
||||
|
||||
if(result){
|
||||
interface_->send(last_msg = UploadInitiateRequest(client_id, entry));
|
||||
}else{
|
||||
interface_->send(last_msg = DownloadInitiateRequest(client_id, entry, buffer, offset));
|
||||
}
|
||||
|
||||
boost::this_thread::disable_interruption di;
|
||||
can::Frame msg;
|
||||
|
||||
while(!done){
|
||||
if(!reader_.read(&msg,boost::chrono::seconds(1)))
|
||||
{
|
||||
abort(0x05040000); // SDO protocol timed out.
|
||||
ROSCANOPEN_ERROR("canopen_master", "Did not receive a response message");
|
||||
break;
|
||||
}
|
||||
if(!processFrame(msg)){
|
||||
ROSCANOPEN_ERROR("canopen_master", "Could not process message");
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(offset == 0 || offset != total){
|
||||
THROW_WITH_KEY(TimeoutException("SDO"), ObjectDict::Key(*current_entry));
|
||||
}
|
||||
|
||||
if(result) *result=buffer;
|
||||
|
||||
}
|
||||
|
||||
void SDOClient::read(const canopen::ObjectDict::Entry &entry, String &data){
|
||||
boost::timed_mutex::scoped_lock lock(mutex, boost::chrono::seconds(2));
|
||||
if(lock){
|
||||
transmitAndWait(entry, data, &data);
|
||||
}else{
|
||||
THROW_WITH_KEY(TimeoutException("SDO read"), ObjectDict::Key(entry));
|
||||
}
|
||||
}
|
||||
void SDOClient::write(const canopen::ObjectDict::Entry &entry, const String &data){
|
||||
boost::timed_mutex::scoped_lock lock(mutex, boost::chrono::seconds(2));
|
||||
if(lock){
|
||||
transmitAndWait(entry, data, 0);
|
||||
}else{
|
||||
THROW_WITH_KEY(TimeoutException("SDO write"), ObjectDict::Key(entry));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user