/* ======================================================================== *\ ! ! * ! * This file is part of Stesy, the MAGIC Steering System ! * Software. It is distributed to you in the hope that it can be a useful ! * and timesaving tool in analysing Data of imaging Cerenkov telescopes. ! * It is distributed WITHOUT ANY WARRANTY. ! * ! * Permission to use, copy, modify and distribute this software and its ! * documentation for any purpose is hereby granted without fee, ! * provided that the above copyright notice appear in all copies and ! * that both that copyright notice and this permission notice appear ! * in supporting documentation. It is provided "as is" without express ! * or implied warranty. ! * ! ! ! Author(s): Thomas Bretz , 2001 ! ! Copyright: MAGIC Software Development, 2000-2001 ! ! \* ======================================================================== */ /////////////////////////////////////////////////////////////////////// // // CanOpen // // implements the canopen layer over the raw device driver // /////////////////////////////////////////////////////////////////////// #include "canopen.h" #include // cout #include // setw, setfill ClassImp(CanOpen); // -------------------------------------------------------------------------- // // Initializes a conditional and a mutex semaphore for all possible // PDO combinations // CanOpen::CanOpen(const char *dev, const int baud, MLog &out) : VmodIcan(dev, baud, out) { for (int i=0; i<32; i++) for (int j=0; j<4; j++) { pthread_cond_init(&fPdoCond[i][j], NULL); pthread_mutex_init(&fPdoMux[i][j], NULL); pthread_mutex_lock(&fPdoMux[i][j]); } lout << "- CanOpen initialized." << endl; } // -------------------------------------------------------------------------- // // Destroys all conditional and mutex semaphores // CanOpen::~CanOpen() { for (int i=0; i<32; i++) for (int j=0; j<4; j++) { pthread_cond_destroy(&fPdoCond[i][j]); pthread_mutex_destroy(&fPdoMux[i][j]); } } // -------------------------------------------------------------------------- // // This overloads VmodIcan::HandleCanMessage. It is called if a can // message was received with all message relevant data (COBId, data, time) // It distributes the data depending on its function code to several // functions (to be obverloaded) // In case of a PDO the conditional semaphore corresponding to this PDO // is raised (WaitForNextPDO) // HandleSDO: handles a SDO message // HandlePDO1/2/3/4:handles received PDOs // void CanOpen::HandleCanMessage(WORD_t cobid, BYTE_t *data, timeval_t *tv) { const WORD_t fcode = cobid >> 7; switch (fcode) { case kNMT: cout << "NMT: " << hex ; cout << "CobId: 0x" << cobid << " "; cout << "cmd=0x" << (int)data[0] << " "; cout << "node=" << dec << (int)data[1] << endl; return; case kSYNC: cout << "Sync" << endl; return; case kSDO_RX: { const BYTE_t node = cobid & 0x1f; const BYTE_t cmd = data[0]; const LWORD_t dat = data[4] | (data[5]<<8) | (data[6]<<16) | (data[7]<<24); const WORD_t idx = data[1] | (data[2]<<8); const WORD_t subidx = data[3]; HandleSDO(node, cmd, idx, subidx, dat, tv); fSdoList.Del(node, idx, subidx); } return; case kPDO1_TX: { const BYTE_t node = cobid & 0x1f; HandlePDO1(node, data, tv); pthread_cond_broadcast(&fPdoCond[node-1][0]); } return; case kPDO2_TX: { const BYTE_t node = cobid & 0x1f; HandlePDO2(node, data, tv); pthread_cond_broadcast(&fPdoCond[node-1][1]); } return; case kPDO3_TX: { const BYTE_t node = cobid & 0x1f; HandlePDO3(node, data, tv); pthread_cond_broadcast(&fPdoCond[node-1][2]); } return; case kPDO4_TX: { const BYTE_t node = cobid & 0x1f; HandlePDO4(node, data, tv); pthread_cond_broadcast(&fPdoCond[node-1][3]); } return; } const BYTE_t node = cobid & 0x1f; cout << "Function Code: 0x" << hex << fcode << " Node: " << dec << (int)node << endl; } // -------------------------------------------------------------------------- // // Enables can messaged for a given node ID and function code. // void CanOpen::EnableCanMsg(BYTE_t node, BYTE_t fcode, int flag) { if (node>=0x20) return; EnableCobId(CobId(node, fcode), flag); } // -------------------------------------------------------------------------- // // Enables Emergency messages for a given node // void CanOpen::EnableEmcy(BYTE_t node) { EnableCanMsg(node, kEMERGENCY); } // -------------------------------------------------------------------------- // // Enables SDO rx messages for a given node // void CanOpen::EnableSdoRx(BYTE_t node) { EnableCanMsg(node, kSDO_RX); } // -------------------------------------------------------------------------- // // Enables PDO1 tx messages for a given node // void CanOpen::EnablePdo1Rx(BYTE_t node) { EnableCanMsg(node, kPDO1_TX); } // -------------------------------------------------------------------------- // // Enables PDO2 tx messages for a given node // void CanOpen::EnablePdo2Rx(BYTE_t node) { EnableCanMsg(node, kPDO2_TX); } // -------------------------------------------------------------------------- // // Enables PDO3 rx messages for a given node // void CanOpen::EnablePdo3Rx(BYTE_t node) { EnableCanMsg(node, kPDO1_TX); } // -------------------------------------------------------------------------- // // Enables PDO4 rx messages for a given node // void CanOpen::EnablePdo4Rx(BYTE_t node) { EnableCanMsg(node, kPDO2_TX); } // -------------------------------------------------------------------------- // // Sends a PDO1 message with the given data to the given node // void CanOpen::SendPDO1(BYTE_t node, BYTE_t data[8]) { SendCanFrame(CobId(node, kPDO1_TX), data); } // -------------------------------------------------------------------------- // // Sends a PDO2 message with the given data to the given node // void CanOpen::SendPDO2(BYTE_t node, BYTE_t data[8]) { SendCanFrame(CobId(node, kPDO2_TX), data); } // -------------------------------------------------------------------------- // // Sends a PDO3 message with the given data to the given node // void CanOpen::SendPDO3(BYTE_t node, BYTE_t data[8]) { SendCanFrame(CobId(node, kPDO3_TX), data); } // -------------------------------------------------------------------------- // // Sends a PDO4 message with the given data to the given node // void CanOpen::SendPDO4(BYTE_t node, BYTE_t data[8]) { SendCanFrame(CobId(node, kPDO4_TX), data); } // -------------------------------------------------------------------------- // // Sends a PDO1 message with the given data to the given node // void CanOpen::SendPDO1(BYTE_t node, BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0, BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0) { BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 }; SendCanFrame(CobId(node, kPDO2_TX), msg); } // -------------------------------------------------------------------------- // // Sends a PDO2 message with the given data to the given node // void CanOpen::SendPDO2(BYTE_t node, BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0, BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0) { BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 }; SendCanFrame(CobId(node, kPDO2_TX), msg); } // -------------------------------------------------------------------------- // // Sends a PDO3 message with the given data to the given node // void CanOpen::SendPDO3(BYTE_t node, BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0, BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0) { BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 }; SendCanFrame(CobId(node, kPDO3_TX), msg); } // -------------------------------------------------------------------------- // // Sends a PDO4 message with the given data to the given node // void CanOpen::SendPDO4(BYTE_t node, BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0, BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0) { BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 }; SendCanFrame(CobId(node, kPDO4_TX), msg); } // -------------------------------------------------------------------------- // // Sends a SDO message with the given data to the given node: // - index describing the dictionary index to set // - subindex describing the dictionary subindex of theindex to set // - val describing the value to set. // - store describes whether the sdo should be stored in a list to // be able to wait for an answer // void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, BYTE_t val, bool store) { if (store) fSdoList.Add(node, idx, subidx); SendCanFrame(CobId(node, kSDO_TX), kSDO_RX1, word_to_lsb(idx), word_to_msb(idx), subidx, val); } // -------------------------------------------------------------------------- // // Sends a SDO message with the given data to the given node: // - index describing the dictionary index to set // - subindex describing the dictionary subindex of theindex to set // - val describing the value to set. // - store describes whether the sdo should be stored in a list to // be able to wait for an answer // void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, WORD_t val, bool store) { if (store) fSdoList.Add(node, idx, subidx); SendCanFrame(CobId(node, kSDO_TX), kSDO_RX2, word_to_lsb(idx), word_to_msb(idx), subidx, word_to_lsb(val), word_to_msb(val)); } // -------------------------------------------------------------------------- // // Sends a SDO message with the given data to the given node: // - index describing the dictionary index to set // - subindex describing the dictionary subindex of theindex to set // - val describing the value to set. // - store describes whether the sdo should be stored in a list to // be able to wait for an answer // void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, LWORD_t val, bool store) { if (store) fSdoList.Add(node, idx, subidx); SendCanFrame(CobId(node, kSDO_TX), kSDO_RX4, word_to_lsb(idx), word_to_msb(idx), subidx, word_to_lsb(val&0xffff), word_to_msb(val&0xffff), word_to_lsb(val>>16), word_to_msb(val>>16)); } // -------------------------------------------------------------------------- // // Request a SDO message from the given node: // - index describing the dictionary index to request // - subindex describing the dictionary subindex of the index to request // void CanOpen::RequestSDO(BYTE_t node, WORD_t idx, BYTE_t subidx) { fSdoList.Add(node, idx, subidx); SendCanFrame(CobId(node, kSDO_TX), kSDO_RX_DATA, word_to_lsb(idx), word_to_msb(idx), subidx); } // -------------------------------------------------------------------------- // // Send an NMT Message to the given node with command cmd // void CanOpen::SendNMT(BYTE_t node, BYTE_t cmd) { SendCanFrame(CobId(0, kNMT), cmd, node); } // -------------------------------------------------------------------------- // // Decodes node and function code into a CobId // WORD_t CanOpen::CobId(BYTE_t node, BYTE_t fcode) const { return (fcode<<7) | node&0x1f; }