Index: trunk/MagicSoft/Cosy/base/File.cc =================================================================== --- trunk/MagicSoft/Cosy/base/File.cc (revision 732) +++ trunk/MagicSoft/Cosy/base/File.cc (revision 732) @@ -0,0 +1,89 @@ +#include "File.h" + +#include + +File::File(const char *name, const char *flags) +{ + f = fopen(name, flags); +} + +File::~File() +{ + fclose(f); +} + +char File::Getc() const +{ + return fgetc(f); +} + +int File::Eof() const +{ + return feof(f); +} + +void File::Seek(long pos) const +{ + fseek(f, pos, SEEK_SET); +} + +void File::Reset() const +{ + fseek(f, 0, SEEK_SET); +} + +long File::Tell() const +{ + return ftell(f); +} + +long File::Size() const +{ + long l = ftell(f); + fseek(f, 0, SEEK_END); + long s = ftell(f); + fseek(f, l, SEEK_SET); + return s; +} + +char *File::Gets(char *c, int cnt) const +{ + for (int i=0; i + +class File +{ +private: + FILE *f; + +public: + File(const char *name, const char *flags); + ~File(); + + void Reset() const; + int Eof() const; + + void Seek(long pos) const; + long Tell() const; + long Size() const; + + char Getc() const; + char *Gets(char *c, int cnt) const; + int Geti(int cnt) const; + float Getf(int cnt) const; + + void Newline() const; + void Skip(int cnt) const; +}; + +#endif Index: trunk/MagicSoft/Cosy/base/MGList.h =================================================================== --- trunk/MagicSoft/Cosy/base/MGList.h (revision 732) +++ trunk/MagicSoft/Cosy/base/MGList.h (revision 732) @@ -0,0 +1,25 @@ +#ifndef MGLIST_H +#define MGLIST_H + +#ifndef ROOT_TOrdCollection +#include +#endif + +class MGList : public TOrdCollection +{ +public: + ~MGList() + { + TIter Next(this); + + TObject *obj; + while ((obj=Next())) + delete (TGObject*)obj; + } + void Add(TGObject *obj) + { + TOrdCollection::Add(obj); + } +}; + +#endif Index: trunk/MagicSoft/Cosy/base/MStopwatch.cc =================================================================== --- trunk/MagicSoft/Cosy/base/MStopwatch.cc (revision 732) +++ trunk/MagicSoft/Cosy/base/MStopwatch.cc (revision 732) @@ -0,0 +1,14 @@ +#include + +#include "MStopwatch.h" + +void MStopwatch::Print(Float_t evts) +{ + if (evts <= 1 ) + Printf("Real time %.3fs, CP time %.3f", RealTime(), CpuTime()); + else + { + Printf("Real time %.3fs, CP time %.3fs, %.0f frames, Speed %.3ffps", + RealTime(), CpuTime(), evts, evts/RealTime()); + } +} Index: trunk/MagicSoft/Cosy/base/MStopwatch.h =================================================================== --- trunk/MagicSoft/Cosy/base/MStopwatch.h (revision 732) +++ trunk/MagicSoft/Cosy/base/MStopwatch.h (revision 732) @@ -0,0 +1,12 @@ +#ifndef MSTOPWATCH_H +#define MSTOPWATCH_H + +#include + +class MStopwatch : public TStopwatch +{ +public: + void Print(Float_t i); +}; + +#endif Index: trunk/MagicSoft/Cosy/base/coord.h =================================================================== --- trunk/MagicSoft/Cosy/base/coord.h (revision 732) +++ trunk/MagicSoft/Cosy/base/coord.h (revision 732) @@ -0,0 +1,122 @@ +#ifndef COORD_H +#define COORD_H + +#include // floor + +#include "slalib/slamac.h" // D2PI + +class Deg +{ +protected: + double fDeg; + +public: + Deg(const double d) : fDeg(d) {} + Deg(const Deg &c) { fDeg = c.fDeg; } + + void Set(double d) { fDeg=d; } + + double operator()() { return fDeg; } + + operator double() const { return fDeg*D2PI/360.0; } +}; + +class XY +{ +protected: + double fX; + double fY; + +public: + XY(double x=0, double y=0) : fX(x), fY(y) {} + XY(const XY &c) { fX = c.fX; fY = c.fY; } + + void Set(double x, double y) { fX=x; fY=y; } + + double X() const { return fX; } + double Y() const { return fY; } + + void operator/=(double c) { fX/=c; fY/=c; } + void operator*=(double c) { fX*=c; fY*=c; } + + XY operator/(double c) const { return XY(fX/c, fY/c); } + XY operator*(double c) const { return XY(fX*c, fY*c); } + XY operator*(const XY &c) const { return XY(fX*c.fX, fY*c.fY); } + XY operator/(const XY &c) const { return XY(fX/c.fX, fY/c.fY); } + XY operator+(const XY &c) const { return XY(fX+c.fX, fY+c.fY); } + XY operator-(const XY &c) const { return XY(fX-c.fX, fY-c.fY); } + + double Sqr() const { return fX*fX + fY*fY; } + double Sqrt() const { return sqrt(Sqr()); } + double Ratio() const { return fX/fY; } + void Round() { fX=(int)(floor(fX+.5)); fY=(int)(floor(fY+.5)); } +}; + +class AltAz : public XY +{ +public: + AltAz(double alt=0, double az=0) : XY(alt, az) {} + + double Alt() const { return fX; } + double Az() const { return fY; } + + void Alt(double d) { fX=d; } + void Az(double d) { fY=d; } + void operator*=(const XY &c) { fX*=c.X(); fY*=c.Y(); } + void operator-=(const AltAz &c) { fX-=c.fX; fY-=c.fY; } + + AltAz operator/(double c) const { return AltAz(fX/c, fY/c); } + AltAz operator*(double c) const { return AltAz(fX*c, fY*c); } + AltAz operator*(const XY &c) const { return AltAz(fX*c.X(), fY*c.Y()); } + AltAz operator/(const XY &c) const { return AltAz(fX/c.X(), fY/c.Y()); } + AltAz operator+(const AltAz &c) const { return AltAz(fX+c.fX, fY+c.fY); } + AltAz operator-(const AltAz &c) const { return AltAz(fX-c.fX, fY-c.fY); } +}; + +class ZdAz : public XY +{ +public: + ZdAz(double zd=0, double az=0) : XY(zd, az) {} + + double Zd() const { return fX; } + double Az() const { return fY; } + + void Zd(double d) { fX=d; } + void Az(double d) { fY=d; } + void operator*=(const XY &c) { fX*=c.X(); fY*=c.Y(); } + void operator-=(const ZdAz &c) { fX-=c.fX; fY-=c.fY; } + void operator+=(const ZdAz &c) { fX+=c.fX; fY+=c.fY; } + + ZdAz operator/(double c) const { return ZdAz(fX/c, fY/c); } + ZdAz operator*(double c) const { return ZdAz(fX*c, fY*c); } + ZdAz operator*(const XY &c) const { return ZdAz(fX*c.X(), fY*c.Y()); } + ZdAz operator/(const XY &c) const { return ZdAz(fX/c.X(), fY/c.Y()); } + ZdAz operator+(const ZdAz &c) const { return ZdAz(fX+c.fX, fY+c.fY); } + ZdAz operator-(const ZdAz &c) const { return ZdAz(fX-c.fX, fY-c.fY); } +}; + +class RaDec : public XY +{ +public: + RaDec(double ra=0, double dec=0) : XY(ra, dec) {} + + double Ra() const { return fX; } + double Dec() const { return fY; } + + RaDec operator/(double c) const { return RaDec(fX/c, fY/c); } + RaDec operator*(double c) const { return RaDec(fX*c, fY*c); } + RaDec operator*(const XY &c) const { return RaDec(fX*c.X(), fY*c.Y()); } + RaDec operator+(const RaDec &c) const { return RaDec(fX+c.fX, fY+c.fY); } + RaDec operator-(const RaDec &c) const { return RaDec(fX-c.fX, fY-c.fY); } +}; + +inline double Rad2Deg(double rad) +{ + return 360.0/D2PI*rad; +} + +inline double Deg2Rad(double rad) +{ + return D2PI/360.0*rad; +} +#endif Index: trunk/MagicSoft/Cosy/base/log.h =================================================================== --- trunk/MagicSoft/Cosy/base/log.h (revision 732) +++ trunk/MagicSoft/Cosy/base/log.h (revision 732) @@ -0,0 +1,15 @@ +#ifndef LOG_H +#define LOG_H + +#include + +class Log +{ +protected: + ostream &lout; + +public: + Log(ostream &out=cout) : lout(out) {} +}; + +#endif Index: trunk/MagicSoft/Cosy/base/msgqueue.cc =================================================================== --- trunk/MagicSoft/Cosy/base/msgqueue.cc (revision 732) +++ trunk/MagicSoft/Cosy/base/msgqueue.cc (revision 732) @@ -0,0 +1,99 @@ +#include "msgqueue.h" + +#include + +#include // PRIO_PROCESS + +MsgQueue::MsgQueue() : fBreak(0) +{ + fMp = new unsigned char; + pthread_create(&fThread, NULL, MapThread, this); +} + +MsgQueue::~MsgQueue() +{ + pthread_cancel(fThread); + delete fMp; +} + +void *MsgQueue::Proc(int msg, void *mp) +{ + return NULL; +} + +void *MsgQueue::MapThread(void *arg) +{ + pthread_detach(pthread_self()); + + setpriority(PRIO_PROCESS, 0, -5); + + ((MsgQueue*)arg)->Thread(); + + return NULL; +} + +void MsgQueue::Thread() +{ + // + // Tell the poster that processing is done + // + fStart = 0; + while (!fBreak) + usleep(1); + + while(1) + { + while (!fStart) usleep(1); + fStart = 0; + + // + // This makes sure that also a very fast Break() after + // a PostMsg is processed correctly + // + pthread_mutex_lock(&fMuxMsg); + fBreak = 0; + pthread_mutex_unlock(&fMuxMsg); + + fRc=Proc(fMsg, fMp); + } +} + +void *MsgQueue::PostMsg(int msg, void *mp, int size) +{ + // + // Lock Mutex, put msg on stack and tell thread to process message + // + + // + // Make sure that only one Proc() is running and can be stopped + // stopped and the messages are processed serialized + // + pthread_mutex_lock(&fMuxMsg); + + // + // Set break state and wait until Proc() returned (break state deleted) + // + fBreak = 1; + + // + // copy return code from Proc() and set new message + // + void *rc = fRc; + + fMsg = msg; + + delete fMp; + fMp = new unsigned char[size]; + + memcpy(fMp, mp, size); + + // + // Start Proc() + // + fStart = 1; + pthread_mutex_unlock(&fMuxMsg); + while (fStart) usleep(1); + + return rc; +} + Index: trunk/MagicSoft/Cosy/base/msgqueue.h =================================================================== --- trunk/MagicSoft/Cosy/base/msgqueue.h (revision 732) +++ trunk/MagicSoft/Cosy/base/msgqueue.h (revision 732) @@ -0,0 +1,41 @@ +#ifndef MSGQUEUE_H +#define MSGQUEUE_H + +#include + +#define WM_NULL 0x0000 + +class MsgQueue +{ +private: + int fBreak; + int fStart; + int fStop; + + int fMsg; // Message identifier + void *fMp; // Message Parameter + void *fSize; // Message Parameter Size + void *fRc; // Proc return code + + pthread_t fThread; + pthread_mutex_t fMuxMsg; + pthread_cond_t fCondMsg; + pthread_mutex_t fMuxRc; + pthread_cond_t fCondRc; + + static void *MapThread(void *arg); + + void Thread(); + +public: + MsgQueue(); + virtual ~MsgQueue(); + + int Break() { return fBreak; } + + virtual void *Proc(int msg, void *mp1); + + void *PostMsg(int msg, void *mp1, int size); +}; + +#endif Index: trunk/MagicSoft/Cosy/base/timer.cc =================================================================== --- trunk/MagicSoft/Cosy/base/timer.cc (revision 732) +++ trunk/MagicSoft/Cosy/base/timer.cc (revision 732) @@ -0,0 +1,108 @@ +#include "timer.h" + +#include + +#include "math.h" // modf +#include "sys/time.h" // struct timeval +#include "slalib/slalib.h" // slacldj + +void Timer::SetTimer(int tv_sec, double tv_usec) +{ + fSecs = tv_sec; + fMs = tv_usec; + + fSec = tv_sec%60; + tv_sec /= 60; + + fMin = tv_sec%60; + tv_sec /= 60; + + fHor = tv_sec%24; + tv_sec /= 24; + + fDay = tv_sec%365; + tv_sec /= 365; + + fYea = tv_sec+1970; + + fDay -= (fYea-1972)/4; + + int days = fDay; + + fMon=0; + do fDay -= fDays[fMon++]; + while (fDay>0); + + int i=1; + while (itv_sec, (double)tv->tv_usec/1000000.0); +} + +Timer::Timer(double t) +{ + double inte; + double frac = modf(t, &inte); + SetTimer(inte, frac); +} + +Timer::Timer(struct timeval *tv) +{ + SetTimer(tv); +} + +Timer::Timer(Timer &t) +{ + fMs = t.fMs; + fDiv = t.fDiv; + fSec = t.fSec; + fSecs = t.fSecs; + fMin = t.fMin; + fHor = t.fHor; + fDay = t.fDay; + fMon = t.fMon; + fYea = t.fYea; +} + +double Timer::GetTime() +{ + struct timeval tv; + gettimeofday(&tv, NULL); + + SetTimer(tv.tv_sec, (double)tv.tv_usec/1000000.0); + + return fMs+fSecs; +} + +double Timer::GetMjd() +{ + int status; + double mjd=0; + slaCldj(fYea, fMon, fDay, &mjd, &status); + + if (status) + cout << "Warning: slaCldj returned " << status << endl; + + return mjd + fDiv; +} + +Timer::operator double() +{ + return fMs+fSecs; +} + +void Timer::Print() +{ + cout << fSec << "s +" << fMs << "s" << endl; +} + +const int Timer::fDays[12] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; Index: trunk/MagicSoft/Cosy/base/timer.h =================================================================== --- trunk/MagicSoft/Cosy/base/timer.h (revision 732) +++ trunk/MagicSoft/Cosy/base/timer.h (revision 732) @@ -0,0 +1,39 @@ +#ifndef TIMER_H +#define TIMER_H + +#include // gettimeofday + +class Timer +{ +private: + const static int fDays[12]; + double fMs; + double fDiv; + int fSec; + int fSecs; + int fMin; + int fHor; + int fDay; + int fMon; + int fYea; + +public: + Timer() : fMs(0), fSec(0), fSecs(0), fMin(0), fHor(0), fDay(0), fMon(0), fYea(0) {} + Timer(double t); + Timer(struct timeval *tv); + Timer(Timer &t); + + void SetTimer(int tv_sec, double tv_usec); + void SetTimer(struct timeval *tv); + + + int GetSecs() { return fSecs; } + double GetTime(); + double GetMjd(); + + void Print(); + + operator double(); +}; + +#endif Index: trunk/MagicSoft/Cosy/candrv/canopen.cc =================================================================== --- trunk/MagicSoft/Cosy/candrv/canopen.cc (revision 732) +++ trunk/MagicSoft/Cosy/candrv/canopen.cc (revision 732) @@ -0,0 +1,228 @@ +#include "canopen.h" + +#include // cout +#include // setw, setfill + +CanOpen::CanOpen(const char *dev, const int baud, ostream &out=cout) : 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]); + } +} + +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]); + } +} + +void CanOpen::HandleCanMessage(WORD_t cobid, BYTE_t *data, struct timeval *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; +} + +void CanOpen::EnableCanMsg(BYTE_t node, BYTE_t fcode, int flag) +{ + if (node>=0x20) + return; + + EnableCobId(CobId(node, fcode), flag); +} + +void CanOpen::EnableEmcy(BYTE_t node) +{ + EnableCanMsg(node, kEMERGENCY); +} + + +void CanOpen::EnableSdoRx(BYTE_t node) +{ + EnableCanMsg(node, kSDO_RX); +} + +void CanOpen::EnablePdo1Rx(BYTE_t node) +{ + EnableCanMsg(node, kPDO1_TX); +} + +void CanOpen::EnablePdo2Rx(BYTE_t node) +{ + EnableCanMsg(node, kPDO2_TX); +} + +void CanOpen::EnablePdo3Rx(BYTE_t node) +{ + EnableCanMsg(node, kPDO1_TX); +} + +void CanOpen::EnablePdo4Rx(BYTE_t node) +{ + EnableCanMsg(node, kPDO2_TX); +} + +void CanOpen::SendPDO1(BYTE_t node, BYTE_t data[8]) +{ + SendCanFrame(CobId(node, kPDO1_TX), data); +} + +void CanOpen::SendPDO2(BYTE_t node, BYTE_t data[8]) +{ + SendCanFrame(CobId(node, kPDO2_TX), data); +} + +void CanOpen::SendPDO3(BYTE_t node, BYTE_t data[8]) +{ + SendCanFrame(CobId(node, kPDO3_TX), data); +} + +void CanOpen::SendPDO4(BYTE_t node, BYTE_t data[8]) +{ + SendCanFrame(CobId(node, kPDO4_TX), data); +} + +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); +} + +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); +} + +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); +} + +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); +} + +void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, BYTE_t val) +{ + fSdoList.Add(node, idx, subidx); + + SendCanFrame(CobId(node, kSDO_TX), kSDO_RX1, + word_to_lsb(idx), word_to_msb(idx), subidx, val); +} + +void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, WORD_t val) +{ + 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)); +} + +void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, LWORD_t val) +{ + 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)); +} + +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); +} + +void CanOpen::SendNMT(BYTE_t node, BYTE_t cmd) +{ + SendCanFrame(CobId(0, kNMT), cmd, node); +} + +WORD_t CanOpen::CobId(BYTE_t node, BYTE_t fcode) +{ + return (fcode<<7) | node&0x1f; +} Index: trunk/MagicSoft/Cosy/candrv/network.cc =================================================================== --- trunk/MagicSoft/Cosy/candrv/network.cc (revision 732) +++ trunk/MagicSoft/Cosy/candrv/network.cc (revision 732) @@ -0,0 +1,167 @@ +#include "network.h" + +#include // cout +#include // setw, setfill + +void Network::Start() +{ + StartReceiver(); + InitNodes(); +} + +void Network::Stop() +{ + StopNodes(); + StopReceiver(); + + lout << "- Network stopped." << endl; +} + +Network::Network(const char *dev, const int baud, ostream &out) : CanOpen(dev, baud, out) +{ + for (int i=0; i<32; i++) + fNodes[i] = NULL; +} + +void Network::HandleSDO(BYTE_t node, BYTE_t cmd, WORD_t idx, BYTE_t subidx, LWORD_t data, struct timeval *tv) +{ + switch (cmd) + { + case kSDO_TX4: // answer to 0x40 with 4 bytes of data + if (fNodes[node]) + { + fNodes[node]->HandleSDO(idx, subidx, data, tv); + return; + } + break; + + case kSDO_TX3: // answer to 0x40 with 2 bytes of data + if (fNodes[node]) + { + fNodes[node]->HandleSDO(idx, subidx, data>>16, tv); + return; + } + break; + + case kSDO_TX1: // answer to 0x40 with 1 byte of data + if (fNodes[node]) + { + fNodes[node]->HandleSDO(idx, subidx, data>>24, tv); + return; + } + break; + + case kSDO_TX_OK: // answer to a SDO_TX message + if (fNodes[node]) + { + fNodes[node]->HandleSDOOK(idx, subidx); + return; + } + break; + + case kSDO_TX_ERROR: // error message (instead of 0x60) + if (fNodes[node]) + { + fNodes[node]->HandleSDOError(data); + return; + } + break; + } + cout << dec << setfill('0'); + cout << "Node=" << (int)node << " Cmd=0x" << hex << (int)cmd << ": "; + cout << "Sdo=" << idx << "/" << (int)subidx << ": 0x" << setw(8) << data; + cout << endl; +} + +void Network::HandlePDO1(BYTE_t node, BYTE_t *data, struct timeval *tv) +{ + if (!fNodes[node]) + { + cout << "Node " << dec << (int)node << ", PDO1: " << hex; + for (int i=0; i<8; i++) + cout << " 0x" << (int)data[i]; + cout << endl; + return; + } + + fNodes[node]->HandlePDO1(data, tv); +} + +void Network::HandlePDO2(BYTE_t node, BYTE_t *data, struct timeval *tv) +{ + if (!fNodes[node]) + { + cout << "Node " << dec << (int)node << ", PDO2: " << hex; + for (int i=0; i<8; i++) + cout << " 0x" << (int)data[i]; + cout << endl; + return; + } + + fNodes[node]->HandlePDO2(data, tv); +} + +void Network::HandlePDO3(BYTE_t node, BYTE_t *data, struct timeval *tv) +{ + if (!fNodes[node]) + { + cout << "Node " << dec << (int)node << ", PDO3: " << hex; + for (int i=0; i<8; i++) + cout << " 0x" << (int)data[i]; + cout << endl; + return; + } + + fNodes[node]->HandlePDO3(data, tv); +} + +void Network::HandlePDO4(BYTE_t node, BYTE_t *data, struct timeval *tv) +{ + if (!fNodes[node]) + { + cout << "Node " << dec << (int)node << ", PDO4: " << hex; + for (int i=0; i<8; i++) + cout << " 0x" << (int)data[i]; + cout << endl; + return; + } + + fNodes[node]->HandlePDO4(data, tv); +} + +void Network::SetNode(NodeDrv *drv) +{ + const BYTE_t nodeid = drv->GetId(); + + if (nodeid>31) + { + cout << "SetNode - Error: Only node Numbers < 32 are allowed"<< endl; + return; + } + + fNodes[nodeid] = drv; +} + +void Network::InitNodes() +{ + for (int i=0; i<32; i++) + if (fNodes[i]) + { + lout << "- Initializing Node #" << dec << i << endl; + fNodes[i]->InitDevice(this); + fNodeInitialized[i] = TRUE; + } + lout << "- All Nodes initialized." << endl; +} + +void Network::StopNodes() +{ + for (int i=0; i<32; i++) + if (fNodes[i] && fNodeInitialized[i]) + { + lout << "- Stopping Node #" << dec << i << endl; + fNodes[i]->StopDevice(); + } + lout << "- All Nodes stopped." << endl; +} + Index: trunk/MagicSoft/Cosy/candrv/network.h =================================================================== --- trunk/MagicSoft/Cosy/candrv/network.h (revision 732) +++ trunk/MagicSoft/Cosy/candrv/network.h (revision 732) @@ -0,0 +1,34 @@ +#ifndef NETWORK_H +#define NETWORK_H + +#include "canopen.h" +#include "nodedrv.h" + +class Network : public CanOpen +{ +private: + NodeDrv *fNodes[32]; + int fNodeInitialized[32]; + + void HandleSDO(BYTE_t node, BYTE_t cmd, WORD_t idx, BYTE_t subidx, LWORD_t data, struct timeval *tv); + void HandlePDO1(BYTE_t node, BYTE_t *data, struct timeval *tv); + void HandlePDO2(BYTE_t node, BYTE_t *data, struct timeval *tv); + void HandlePDO3(BYTE_t node, BYTE_t *data, struct timeval *tv); + void HandlePDO4(BYTE_t node, BYTE_t *data, struct timeval *tv); + + void InitNodes(); + void StopNodes(); + +public: + Network(const char *dev, const int baud, ostream &out=cout); + + void SetNode(NodeDrv *drv); + + NodeDrv *operator[](int i) { return fNodes[i]; } + NodeDrv *GetNode(int i) { return fNodes[i]; } + + virtual void Start(); + virtual void Stop(); +}; + +#endif Index: trunk/MagicSoft/Cosy/candrv/nodedrv.cc =================================================================== --- trunk/MagicSoft/Cosy/candrv/nodedrv.cc (revision 732) +++ trunk/MagicSoft/Cosy/candrv/nodedrv.cc (revision 732) @@ -0,0 +1,150 @@ +#include "nodedrv.h" + +#include +#include + +#include "network.h" + +NodeDrv::NodeDrv(BYTE_t nodeid, ostream &out) : Log(out), fNetwork(NULL), fId(32) +{ + if (nodeid>31) + { + cout << "SetNode - Error: Only node Numbers < 32 are allowed"<< endl; + return; + } + + fId = nodeid; +} + +void NodeDrv::InitDevice(Network *net) +{ + fNetwork = net; + + EnableCanMsg(kPDO1_TX); + EnableCanMsg(kPDO2_TX); + EnableCanMsg(kPDO3_TX); + EnableCanMsg(kPDO4_TX); + EnableCanMsg(kSDO_RX); + EnableCanMsg(kSDO_TX); +} + +void NodeDrv::HandleSDOOK(WORD_t idx, BYTE_t subidx) +{ + lout << hex << setfill('0'); + lout << "Sdo=" << idx << "/" << (int)subidx << " set."; + lout << endl; +} + +void NodeDrv::HandleSDOError(LWORD_t data) +{ + lout << "Err: 0x"; + lout << hex << setfill('0') << setw(4) << data; + lout << endl;; +} + +void NodeDrv::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, struct timeval *tv) +{ + cout << "SdoRx: Idx=0x"<< hex << idx << "/" << (int)subidx; + cout << ", val=0x" << val << endl; +} + +void NodeDrv::SendPDO1(BYTE_t data[8]) +{ + fNetwork->SendPDO1(fId, data); +} + +void NodeDrv::SendPDO2(BYTE_t data[8]) +{ + fNetwork->SendPDO2(fId, data); +} + +void NodeDrv::SendPDO1(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) +{ + fNetwork->SendPDO1(fId, m0, m1, m2, m3, m4, m5, m6, m7); +} + +void NodeDrv::SendPDO2(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) +{ + fNetwork->SendPDO2(fId, m0, m1, m2, m3, m4, m5, m6, m7); +} + +void NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, BYTE_t val) +{ + fNetwork->SendSDO(fId, idx, subidx, val); +} + +void NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, WORD_t val) +{ + fNetwork->SendSDO(fId, idx, subidx, val); +} + +void NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, LWORD_t val) +{ + fNetwork->SendSDO(fId, idx, subidx, val); +} + +void NodeDrv::SendSDO(WORD_t idx, BYTE_t val) +{ + fNetwork->SendSDO(fId, idx, val); +} + +void NodeDrv::SendSDO(WORD_t idx, WORD_t val) +{ + fNetwork->SendSDO(fId, idx, val); +} + +void NodeDrv::SendSDO(WORD_t idx, LWORD_t val) +{ + fNetwork->SendSDO(fId, idx, val); +} + +void NodeDrv::RequestSDO(WORD_t idx, BYTE_t subidx=0) +{ + fNetwork->RequestSDO(fId, idx, subidx); +} + +void NodeDrv::SendNMT(BYTE_t cmd) +{ + fNetwork->SendNMT(fId, cmd); +} + +void NodeDrv::EnableCanMsg(BYTE_t fcode) +{ + fNetwork->EnableCanMsg(fId, fcode, TRUE); +} + +void NodeDrv::WaitForSdo(WORD_t idx, BYTE_t subidx) +{ + while (fNetwork->WaitingForSdo(fId, idx, subidx)) + usleep(1); +} + +void NodeDrv::WaitForSdos() +{ + while (fNetwork->WaitingForSdo(fId)) + usleep(1); +} + +void NodeDrv::WaitForNextPdo1() +{ + fNetwork->WaitForNextPdo1(fId); +} + +void NodeDrv::WaitForNextPdo2() +{ + cout << "WAIT: " << (int)fId << " " << fNetwork << endl; + fNetwork->WaitForNextPdo2(fId); +} + +void NodeDrv::WaitForNextPdo3() +{ + fNetwork->WaitForNextPdo3(fId); +} + +void NodeDrv::WaitForNextPdo4() +{ + fNetwork->WaitForNextPdo4(fId); +} + Index: trunk/MagicSoft/Cosy/candrv/nodedrv.h =================================================================== --- trunk/MagicSoft/Cosy/candrv/nodedrv.h (revision 732) +++ trunk/MagicSoft/Cosy/candrv/nodedrv.h (revision 732) @@ -0,0 +1,63 @@ +#ifndef NODEDRV_H +#define NODEDRV_H + +#include "log.h" +#include "gendef.h" + +class Network; + +class NodeDrv : public Log +{ +private: + Network *fNetwork; + BYTE_t fId; + +public: + NodeDrv(BYTE_t nodeid, ostream &out=cout); + + BYTE_t GetId() { return fId; } + Network *GetNetwork() { return fNetwork; } + + virtual void InitDevice(Network *net); + virtual void StopDevice() = 0; + + virtual void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, struct timeval *tv); + virtual void HandleSDOOK(WORD_t idx, BYTE_t subidx); + virtual void HandleSDOError(LWORD_t data); + + virtual void HandlePDO1(BYTE_t *data, struct timeval *tv) {}; + virtual void HandlePDO2(BYTE_t *data, struct timeval *tv) {}; + virtual void HandlePDO3(BYTE_t *data, struct timeval *tv) {}; + virtual void HandlePDO4(BYTE_t *data, struct timeval *tv) {}; + + void SendPDO1(BYTE_t data[8]); + void SendPDO2(BYTE_t data[8]); + void SendPDO1(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); + void SendPDO2(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); + + void SendSDO(WORD_t idx, BYTE_t subidx, BYTE_t val); + void SendSDO(WORD_t idx, BYTE_t subidx, WORD_t val); + void SendSDO(WORD_t idx, BYTE_t subidx, LWORD_t val); + + void SendSDO(WORD_t idx, BYTE_t val); + void SendSDO(WORD_t idx, WORD_t val); + void SendSDO(WORD_t idx, LWORD_t val=0); + + void SendNMT(BYTE_t cmd); + + void RequestSDO(WORD_t idx, BYTE_t subidx=0); + + void WaitForNextPdo1(); + void WaitForNextPdo2(); + void WaitForNextPdo3(); + void WaitForNextPdo4(); + + void WaitForSdos(); + void WaitForSdo(WORD_t idx, BYTE_t subidx=0); + + void EnableCanMsg(BYTE_t fcode); +}; + +#endif Index: trunk/MagicSoft/Cosy/candrv/sdolist.cc =================================================================== --- trunk/MagicSoft/Cosy/candrv/sdolist.cc (revision 732) +++ trunk/MagicSoft/Cosy/candrv/sdolist.cc (revision 732) @@ -0,0 +1,125 @@ +#include "sdolist.h" + +#include + +PendingSDOList::PendingSDOList() +{ + fFirst = new PendingSDO; + fLast = fFirst; + + pthread_mutex_init(&fMux, NULL); +} + +PendingSDOList::~PendingSDOList() +{ + // pthread_mutex_lock(&fMux); + + DelAll(); + delete fFirst; + + pthread_mutex_destroy(&fMux); +} + +void PendingSDOList::DelAll() +{ + pthread_mutex_lock(&fMux); + + PendingSDO *prev = fFirst; + PendingSDO *sdo; + + do + { + sdo = prev->Next; + delete prev; + + } while((prev=sdo)); + + fFirst = new PendingSDO; + fLast = fFirst; + + pthread_mutex_unlock(&fMux); +} + +void PendingSDOList::Add(BYTE_t node, WORD_t idx, BYTE_t subidx) +{ + PendingSDO *sdo = fFirst; + + pthread_mutex_lock(&fMux); + while ((sdo=sdo->Next)) + if (sdo->Node==node && sdo->Idx==idx && sdo->Subidx==subidx) + { + cout << "Warning: SDO Node #" << dec << (int)node << ", 0x"; + cout << hex << idx << "/" << (int)subidx; + cout << " still pending." << endl; + break; + } + + fLast->Next = new PendingSDO(node, idx, subidx); + fLast = fLast->Next; + pthread_mutex_unlock(&fMux); +} + +void PendingSDOList::Del(BYTE_t node, WORD_t idx, BYTE_t subidx) +{ + PendingSDO *prev = fFirst; + PendingSDO *sdo; + + pthread_mutex_lock(&fMux); + while ((sdo=prev->Next)) + { + if (sdo->Node!=node || sdo->Idx!=idx || sdo->Subidx!=subidx) + { + prev = sdo; + continue; + } + + prev->Next = sdo->Next; + + delete sdo; + + if (!prev->Next) + fLast = prev; + + break; + } + pthread_mutex_unlock(&fMux); +} + +int PendingSDOList::IsPending() +{ + return (int)fFirst->Next; +} + +int PendingSDOList::IsPending(BYTE_t node) +{ + int rc = FALSE; + PendingSDO *sdo = fFirst; + + pthread_mutex_lock(&fMux); + while ((sdo=sdo->Next)) + if (sdo->Node==node) + { + rc = TRUE; + break; + } + pthread_mutex_unlock(&fMux); + + return rc; +} + +int PendingSDOList::IsPending(BYTE_t node, WORD_t idx, BYTE_t subidx) +{ + int rc = FALSE; + PendingSDO *sdo = fFirst; + + pthread_mutex_lock(&fMux); + while ((sdo=sdo->Next)) + if (sdo->Node==node && sdo->Idx==idx && sdo->Subidx==subidx) + { + rc = TRUE; + break; + } + pthread_mutex_unlock(&fMux); + + return rc; +} Index: trunk/MagicSoft/Cosy/candrv/sdolist.h =================================================================== --- trunk/MagicSoft/Cosy/candrv/sdolist.h (revision 732) +++ trunk/MagicSoft/Cosy/candrv/sdolist.h (revision 732) @@ -0,0 +1,41 @@ +#ifndef SDOLIST_H +#define SDOLIST_H + +#include + +#include "gendef.h" + +class PendingSDO +{ +public: + BYTE_t Node; + WORD_t Idx; + BYTE_t Subidx; + PendingSDO *Next; + + PendingSDO(BYTE_t n=0, WORD_t i=0, BYTE_t s=0) + : Node(n), Idx(i), Subidx(s), Next(NULL) {} +}; + +class PendingSDOList +{ +private: + PendingSDO *fFirst; + PendingSDO *fLast; + + pthread_mutex_t fMux; + +public: + PendingSDOList(); + ~PendingSDOList(); + + void Add(BYTE_t node, WORD_t idx, BYTE_t subidx); + void Del(BYTE_t node, WORD_t idx, BYTE_t subidx); + void DelAll(); + + int IsPending(); + int IsPending(BYTE_t node); + int IsPending(BYTE_t node, WORD_t idx, BYTE_t subidx); +}; + +#endif Index: trunk/MagicSoft/Cosy/candrv/vmodican.cc =================================================================== --- trunk/MagicSoft/Cosy/candrv/vmodican.cc (revision 732) +++ trunk/MagicSoft/Cosy/candrv/vmodican.cc (revision 732) @@ -0,0 +1,825 @@ +#include "vmodican.h" + +#include // cout +#include // setw, setfill + +#include // O_RDONLY +#include // errno +#include // read +#include // pthread_create +#include // ioctl +#include // PRIO_PROCESS + +void VmodIcan::PrintMsg(Message *m) +{ + cout << "Cmd=0x" << hex << (int)m->cmd << dec << " " << flush; + cout << "len=" << (int)m->len << ":" << flush; + + cout << hex << flush; + for (int i=0; ilen; i++) + cout << " " << (int)m->data[i] << flush; + cout << dec << endl; +} + +int VmodIcan::Ioctl(int msg, void *arg) +{ + return ioctl(fd, msg, (int)arg) >= 0; +} + +void VmodIcan::SetTermination(int state) /* 0 = off, 1 = on */ +{ + /* -*-Func-*- + * + * SwitchCanTermination - Switch the CANbus termination resistor + * + * The VMOD-ICAN3 module allows the user to change the state of the CANbus + * termination via software. This is done with this service. + * + * SERVICE: HwConf + */ + + Message msg; + + msg.cmd = M_HW_CONF; + + msg.len = 2; + msg.data[0] = 0x00; + msg.data[1] = (BYTE_t)state; + + while (!Send(&msg)); + + lout << "- CAN Bus Termination set to " << (state?"on":"off") << endl; +} + +void *VmodIcan::ReceiveThread(void *data) +{ + VmodIcan *cal = (VmodIcan*)data; + + cal->lout << "- Starting Receiver Loop." << endl; + + // + // set higher priority to make sure that no messages are lost. + // + setpriority(PRIO_PROCESS, 0, -10); + + while (1) + { + unsigned char c; + struct timeval tv; + + //cout << "waiting..." << endl; + + const int n = read(cal->fd, &c, 1); // sleep until message arrive + + gettimeofday(&tv, NULL); + + //cout << "read n=" << n << " c=" << (int)c << endl; + + if (n == 0) + { + cerr << "panic read (errno=" << errno << ") ..." << endl; + return (void *)1; + } + + switch(c) + { + case FAST_QUEUE: + cout << "--> Fast Queue: " << flush; + + FastMessage fmsg; + + if (cal->ReceiveFast(&fmsg) < 0) + return (void *)1; + + cout << "Fast msg ID " << + (fmsg.data[0] << 3) + ((fmsg.data[1] >> 5) & 7) << ": " << flush; + + for(int i=0; i<16; i++) + cout << (int)*(((unsigned char *)(&fmsg))+i) << " " << flush; + + cout << endl; + break; + + case PLAIN_QUEUE: + + Message msg; + + if (cal->Receive(&msg) < 0) + return (void *)1; + + //cal->PrintMsg(&msg); + cal->HandleMessage(&msg, &tv); + + break; + } + + } +} + +void VmodIcan::HandleMessage(Message *msg, struct timeval *tv) +{ + const WORD_t desc = msg->data[2]<<8 | msg->data[3]; + const BYTE_t rtr = (desc>>4)&1; + const BYTE_t len = desc&0xf; + const WORD_t cobid = desc>>5; + + switch (msg->cmd) + { + case M_MSG_LOST: + cout << "VmodIcan reports: " << dec << (int)msg->data[0] << " msg(s) lost!" << endl; + return; + + case M_BCAN_EVENT_ind: + cout << "VmodIcan reports: CEVTind=0x37, " << hex; + switch (msg->data[0]) // error indicator + { + case 0x01: + cout << "Error interrup occured" << endl; + cout << "This means noisy network normally. Please check the bus termination." << endl; + switch (msg->data[1]) // msg type (board depending) + { + case 2: // SJA1000 + cout << dec; + cout << "ModeReg=" << (int)msg->data[2] << ", "; + cout << "StatReg=" << (int)msg->data[3] << ", "; + cout << "RxErrCnt=" << (int)msg->data[4] << ", "; + cout << "TxErrCnt=" << (int)msg->data[5] << endl; + } + TerminateApp(); + return; + case 0x02: + cout << "Overrun interrup occured" << endl; + return; + case 0x04: + cout << "Interrupts lost" << endl; + return; + case 0x08: + cout << "Send queue full" << endl; + return; + case 0x10: + cout << "CANbus bus-error" << endl; + return; + } + return; + case M_BCAN_RX_ind: + HandleCanMessage(cobid, &msg->data[4], tv); + return; + } + + cout << hex; + cout << "Cmd=0x" << (int)msg->cmd << ": "; + cout << "Descr: 0x" << cobid << dec; + cout << " Rtr: " << (rtr?"Yes":"No"); + cout << " Len: " << (int)len << endl; + + cout << "As Raw Data:" << hex << setfill('0'); + for (int i=0; ilen; i++) + cout << " " << setw(2) << (int)(msg->data[i]) << flush; + cout << endl; +} + +int VmodIcan::Receive(Message *pm) /* receive buffer */ +{ + /* can_recv - receive a message from standard interface + * + * This function reads a whole message from the standard host interface of + * a VMOD-ICAN. + * The module is selected by the module number . + * + * The structure is filled with the received message. + * + * RETURNS: + * The function returns the number of message received, or -1 when the + * system call failed. + * The return value therefore 0 determines, that no message was available to + * be read: can_recv() does not block in such a case and therefore + * can be used to poll a module for incoming messages. + */ + + struct dpm_rw_can_desc arg; + + arg.pm = pm; + + if (!Ioctl(DPM_READ_MBOX, &arg)) + return -1; + + return arg.rval; +} + +int VmodIcan::ReceiveFast(FastMessage *pm) +{ + + /* can_recv_fast - receive a message from layer2 interface + * + * This function reads a FastMessage from the layer2 fast message + * interface of a VMOD-ICAN. + * The module is selected by the file descriptor . + * + * The structure is filled with the received message. + * + * RETURNS: + * The function returns -1 when the * system call failed. + * The return value therefore 0 determines, that no message was available to + * be read: can_recv_fast() does not block in such a case and therefore + * can be used to poll a module for incoming messages. + */ + + struct dpm_write_fast_can_desc arg; + + arg.pm = pm; + + if (!Ioctl(DPM_READ_FAST_MBOX, &arg)) + return -1; + + return arg.rval; +} + + +void VmodIcan::SetBaudRate(int rate) +{ + /* IcWriteBtrBCAN - Set bit timing parameters + * + * Set bit timing parameters in CAN controller. May only be used if + * CAN controller is in bus-off state. stores the bus-timing + * parameters as required by the 82C200 controller. See the description + * of the CBTRreq-service for possible values. + * + * BTR1 is stored in the upper byte of and BTR0 in the lower. Examples + * are: + * .CS + * Baudrate btr Macro + * 1Mbit 0x2300 BTR_1MB + * 500kBit 0x1c00 BTR_500KB + * 250kBit 0x1c01 BTR_250KB + * 125kBit 0x1c03 BTR_125KB + * 100kBit 0x34c7 BTR_100KB + * 50kBit 0x34cf BTR_50KB + * 20kBit 0x7fcf BTR_20KB + * .CE + * + * SERVICE: CBTRreq + * + * NOTE: + * Raw ICANOS version of the firmware only. + */ + + /* create message for vmod-ican */ + Message msg; /* buffer for module messages */ + + int rateid; + + switch (rate) + { + case 1000: + rateid=BTR_1MB; + break; + case 500: + rateid=BTR_500KB; + break; + case 250: + rateid=BTR_250KB; + break; + case 125: + rateid=BTR_125KB; + break; + case 100: + rateid=BTR_100KB; + break; + case 50: + rateid=BTR_50KB; + break; + case 20: + rateid=BTR_20KB; + break; + + default: + cout << "Error: Wrong bit rate specified" << endl; + return; + } + + msg.cmd = M_BCAN_SET_BTR_req; + + msg.len = 4; + + msg.data[2] = word_to_lsb(rateid); + msg.data[3] = word_to_msb(rateid); + + while (!Send(&msg)); /* transmitt to module */ + + lout << "- Baudrate set to " << rate << "bps" << endl; +} + + +void VmodIcan::EnableCanBusConnection() +{ + /* IcBusOnBCAN - switch CANbus controller to bus-on state + * + * Switch CAN controller bus-on. You will need to use this + * function explicitly after you have connected yourself + * to the module with can_open() (or ican_open() under DOS/WINDOWS). + * This is because the module comes up in the bus-off state. + * + * SERVICE: CONreq + * + * NOTE: + * Raw ICANOS version of the firmware only. + */ + Message msg; /* buffer for module messages */ + + msg.cmd = M_BCAN_BUSON_req; + msg.len = 0; + + while (!Send(&msg)); + + lout << "- Controller connected to bus" << endl; +} + +int VmodIcan::EnableFastCan(int rbuffers, int wbuffers) +{ + /* ican2_init_fast_can - initialize fast can access for VMOD-ICAN + * + * By this function, the user may initialize and enable the fast + * host interface (layer2 access) for a VMOD-ICAN module. + * + * The calling application can request buffer elements in the queue + * that sends data to the host and buffer elements for the queue + * that transports data to the module. + * + * NOTE: + * Notice that the message filtering on the VMOD-ICAN has to be + * set correctly, so that messages can be received through the fast + * interface. + * + * CAVE AT: + * The and wbuffers> have no special limit, but the general + * resources of the DPM must not be exceeded. + * For the calculation you need to assume, that 16 buffers in one of the fast + * interface queues take the same DPM space as 1 buffer in the standard + * host interface. + * + * The user must use only one of the functions, either + * ican2_init_fast_can or ican2_init_fast_can_prio + * + * RETURNS: + * Zero if the operation performed successfully, or less than zero on error. + */ + struct dpm_fast_can_desc hdp; + + hdp.tohost_len = rbuffers; + hdp.fromhost_len = wbuffers; + + if (!Ioctl(DPM_INIT_FAST_CAN, &hdp)) + return -1; + + lout << "- Fast Host Interface Enabled" << endl; + + return 0; +} + +void VmodIcan::DisableCanBusConnection() +{ + /* IcWriteEwlBCAN - Set error warning limit + * + * Set error warning limit in CAN controller. If this limit is passed, the + * user will get a CEVTind message stating an error interrupt. This type + * of message will also occur if the both error counter again fall below + * this limit. + * + * RESTRICTIONS: + * Will only take effect if CAN controller is in bus-off state. Requires + * an SJA1000 CANbus controller, and will be no-op for 82C200. + * + * SERVICE: CBCONFreq + * + * NOTE: + * Raw ICANOS version of the firmware only. + */ + lout << "- Disconnect from Bus!" << endl; + + Message msg; /* buffer for module messages */ + + msg.cmd = M_BCAN_BUSOFF_req; + msg.len = 0; + + while (!Send(&msg)); +} + +void VmodIcan::Close() +{ + /* can_close - close connection to a VMOD-ICAN module + * + * The function can be used to close a connection to a VMOD-ICAN + * that has been established by a can_open() call. + * The module has to be selected by the file descriptor which was + * obtained when you did the can_open() call. + * + * When you call can_close, all the resources that were used by the driver + * for communication are freed. + * + * The VMOD-ICAN module under question will be reseted, to make sure that + * the communication with the host will stop. That means especially that + * no further interrupt will occur and that the module will not longer be + * active on the CANbus. + * + * RETURNS: N/A + */ + lout << "- Close Device!" << endl; + + Message msg; /* disconnect message */ + + msg.cmd = M_DISCONNECT; + msg.len = 0; + + while (!Send(&msg)); + + close(fd); +} + +int VmodIcan::EnableFifo() +{ + // + // Allow VMOD to send messages through the fifo + // + Message msg; /* connect message */ + + msg.cmd = M_CONNECT_INTR; + msg.len = 0; + + while (!Send(&msg)); + + lout << "- Fifo enabled" << endl; + + return TRUE; +} + +int VmodIcan::Reset() +{ + const int rc = Ioctl(DPM_RESET, 0); + + lout << "- Reset done." << endl; + + return rc; +} + +int VmodIcan::Open(const char *devname) /* pathname of device */ +{ + /* can_open - open VMOD-ICAN device + * + * With this function call you open a VMOD-ICAN plugged + * into a MODULbus carrier board for use. The module is + * reseted and then initialized for communication to the host. + * + * A specific module is selected by it's device name (e.g. "/dev/dpm_01"). + */ + + fd = open (devname, O_RDONLY, 0); + + if (fd < 0) + { + lout << "Error: Opening device '" << devname << "' (rc=" << fd << ")" << endl; + return FALSE; + } + + lout << "- Device " << devname << " open." << endl; + + return TRUE; +} + +int VmodIcan::StdHost2NewStyle(int rbuffers, int wbuffers, + int wbuffers_hi, int wbuffers_low) +/* buffers in to, from, from_hi, from_low host queue */ +/* ican2_select_hostif - switch standard host interface to new style mode + * + * The routine ican2_select_hostif() can be used to switch a module from + * the standard host interface to the new style mode. The module is selected + * by the module number . + * + * The calling application can request buffer for the communication + * queue that sends data to the host and buffer for the reverse + * communication direction (normal priority queue). By this function the hi- and + * low-prioritized message-queues which sends data to the module are initialized + * to a length of 1. + * + * NOTE: + * To notify the module of the new situation, the driver sends + * a M_NEWHOSTIF message to the module. This is the last message to be + * transfered through the old style host interface. Immediately after + * sending this message, the library is switched to the new style mode. + * Any messages that are sent by the module in this time gap, may be lost. + * It is therefore not recommended to use this function when you wait + * for messages from the module. + * + * The selection of the new host interface is not reversible. It will stay + * until the next reset for the module occurs. This will probably occur + * when you use can_close(). + * + * HINTS: + * When the new style mode is active, no more internal message buffering + * on the module exists. That is whenever the module tries to send something + * and cannot because the queue is full, this message will be dropped. + * Thereby, when enabling the new style host interface you should create + * enough buffers for the queue that sends to the host, to prevent the + * loss of messages. If you loose messages, however you will be indicated + * of that event by a MSGLOST messages (which will not be lost!). + * + * CAVE AT: + * The parameters , , and + * must be greater than 0, less than 128, and the total sum must not + * exceed 236. These parameters aren't checked by the driver! + */ +{ + struct dpm_new_hostif_desc_prio hdp; + + hdp.tohost_len = rbuffers; + hdp.fromhost_len = wbuffers; + hdp.fromhost_hi_len = wbuffers_hi; + hdp.fromhost_low_len = wbuffers_low; + + Ioctl(DPM_INIT_NEW_HOSTIF_PRIO, &hdp); + + lout << "- New style host interface enabled" << endl; + + return 0; +} + +int VmodIcan::SendHi(Message *pm) +{ + /* can_send_hi - send message to standard host interface (high priority) + * + * This function performs the same action as can_send(), except it will + * append message to the highest priority queue of the standard + * host interface. + * + * NOTE: + * Notice that the prioritized issue of the message take effect on the new style + * mode of the standard host interface only. + * + * RETURNS: + * The function returns the number of message send, or -1 when the system + * call failed. + * The return value 0 determines that no message could be send, + * probably because there was no space in the targeted queue. can_send_hi() + * does not block or retry in such a case, so you need to loop explicitly + * until the message is send. + */ + struct dpm_rw_can_desc arg; + + arg.pm = pm; + + if (!Ioctl(DPM_WRITE_MBOX_HI, &arg)) + return FALSE; + + return arg.rval; +} + +int VmodIcan::SendLo(Message *pm) +{ + /* can_send_low - send message to standard host interface (low priority) + * + * This function performs the same action as can_send(), except it will + * append message to the lowest priority queue of the standard + * host interface. + * + * NOTE: + * Notice that the prioritized issue of the message take effect on the new + * style mode of the standard host interface only. + * + * RETURNS: + * The function returns the number of message send, or -1 when the system + * call failed. + * The return value 0 determines that no message could be send, + * probably because there was no space in the targeted queue. can_send_low() + * does not block or retry in such a case, so you need to loop explicitly + * until the message is send. + * + */ + struct dpm_rw_can_desc arg; + + arg.pm = pm; + + if (!Ioctl(DPM_WRITE_MBOX_LOW, &arg)) + return FALSE; + + return arg.rval; +} + +int VmodIcan::Send(Message *pm) /* file descriptor, message to send */ +{ + /* can_send - send message to standard host interface (mid priority) + * + * This function sends a complete message to the standard host interface of + * a VMOD-ICAN. + * + * The message will be queued to the middle prioritized of the three + * queues. + * + * RETURNS: + * The function returns the number of message send, or -1 when the system + * call failed. + * The return value 0 determines that no message could be send, + * probably because there was no space in the targeted queue. can_send() + * does not block or retry in such a case, so you need to loop explicitly + * until the message is send. + */ + struct dpm_rw_can_desc arg; + + arg.pm = pm; + + if (!Ioctl(DPM_WRITE_MBOX, &arg)) + return FALSE; + + return arg.rval; +} + +int VmodIcan::Send(FastMessage *pm) /* file descriptor, message to send */ +{ + /* can_fast_send - send message to fast interface + * + * This function sends a message to the fast host interface (layer-2 + * interface) of a VMOD-ICAN. The module is selected by the module number + * . + * The message to be send will be given in the structure . + * + * The fast host interface needs to be established before can_fast_send() + * can be used successfully. + * + * RETURNS: + * The function returns 1 if can_fast_send() completed successful. + * Otherwise the return value 0 determines that the message could not be send, + * probably because there was no space in the DPM. The function + * does not block or retry in such a case, so you need to loop explicitly + * until the message is send. + * The function returns -1 when the system-call itself failed. + */ + struct dpm_write_fast_can_desc arg; + + arg.pm = pm; + + if (!Ioctl(DPM_WRITE_FAST_CAN, &arg)) + return FALSE; + + lout << "done." << endl; + + return arg.rval; +} + +void VmodIcan::DisableAllCobIds() +{ + /* -*-Func-*- + * + * IcSetAfil - Set software acceptance filter mask + * + * Set software acceptance filtering. + * + * SERVICE: SetAfilMask + */ + Message msg; + + msg.cmd = M_SET_AFIL; + msg.len = 5; + + msg.data[0] = word_to_lsb(0); + msg.data[1] = word_to_msb(0); + msg.data[2] = word_to_lsb(0x7ff); // 11 bit Cob-Ids + msg.data[3] = word_to_msb(0x7ff); // 11 bit Cob-Ids + msg.data[4] = 0; + + while (!Send(&msg)); + + lout << "- All CobIds disabled." << endl; +} + + +void VmodIcan::EnableCobId(WORD_t cobid, int flag) +{ + /* -*-Func-*- + * + * IcSetAfil - Set software acceptance filter mask + * + * Set software acceptance filtering. + * + * SERVICE: SetAfilMask + * + * NOTE: This service is available in both version of the firmware: Raw-CAN + * and CAL. + */ + Message msg; + + msg.cmd = M_SET_AFIL; + msg.len = 3; + + msg.data[0] = word_to_lsb(cobid); + msg.data[1] = word_to_msb(cobid); + msg.data[2] = (BYTE_t)(flag?3:0); + + while (!Send(&msg)); + + lout << "- CobId 0x" << hex << setfill('0') << setw(3) << cobid << " enabled." << endl; +} + +void VmodIcan::SendCanFrame(WORD_t cobid, BYTE_t m[8]) +{ + /* -*-Func-*- + * + * IcSendReqBCAN - Send a CANbus message + * + * Issue request to send a CAN message. controls whether to send with + * or without spec/confirmation. + * .CS + * spec action + * 0 send only + * 1 send with confirmation to the host. + * 2 send and echo message to the host. + * 3 send and generate both echo and confirmation. + * .CE + * + * SERVICE: CTXreq, CTXCreq, CTXEreq, CTXCEreq + * + * NOTE: + * Raw ICANOS version of the firmware only. + */ + const WORD_t desc = MsgDescr(cobid, 8); + + Message msg; + + msg.cmd = M_BCAN_TX_req; + + msg.len = 12; + msg.data[0] = 0; + msg.data[1] = 0; + msg.data[2] = word_to_msb(desc); + msg.data[3] = word_to_lsb(desc); + + memcpy(&msg.data[4], m, 8); + + while (!Send(&msg)); + + /* + cout << "0x" << hex << (int)cobid << ": "; + for(int i=0; i<10; i++) + cout << hex << (int)msg.data[i+2] << " " << flush; + cout << endl; + cout << "- Message sent." << endl; + */ + +} + +VmodIcan::VmodIcan(const char *dev, const int baud, ostream &out) : Log(out), fRxThrd(NULL)//: CanDriver(dev, baud) +{ + Open(dev); // open module + Reset(); + EnableFifo(); // connect to host (in interrupt mode) + SetBaudRate(baud); // set baud rate + DisableAllCobIds(); + EnableCanBusConnection(); // connect to bus + /* + StdHost2NewStyle(50, 50, 50, 50); // set host style + EnableFastCan(50, 50); + SetTermination(0); + */ +} + +VmodIcan::~VmodIcan() +{ + StopReceiver(); + DisableCanBusConnection(); + Close(); +} + +void VmodIcan::SendCanFrame(WORD_t cobid, + BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3, + BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7) +{ + BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 }; + SendCanFrame(cobid, msg); +} + +void VmodIcan::StopReceiver() +{ + if (!fRxThrd) + return; + + pthread_cancel(*fRxThrd); + + delete fRxThrd; + fRxThrd = NULL; + + lout << "- Receiver Thread stopped." << endl; +} +void VmodIcan::StartReceiver() +{ + //************************************** + //* create thread waiting for messages * + //************************************** + if (fRxThrd) + { + cout << "Error: rx thread already started." << endl; + return; + } + + lout << "- Starting receiving Thread." << endl; + + fRxThrd = new pthread_t; + pthread_create(fRxThrd, NULL, ReceiveThread, this); +} + Index: trunk/MagicSoft/Cosy/candrv/vmodican.h =================================================================== --- trunk/MagicSoft/Cosy/candrv/vmodican.h (revision 732) +++ trunk/MagicSoft/Cosy/candrv/vmodican.h (revision 732) @@ -0,0 +1,70 @@ +#ifndef VMODICAN_H +#define VMODICAN_H + +#include + +#include "unistd.h" // gettimeofday + +#include "log.h" +#include "gendef.h" + +class VmodIcan : public Log +{ +private: + int fd; // file descriptor for can module + pthread_t *fRxThrd; + + int Ioctl(int msg, void *arg); + + static void *ReceiveThread(void *data); + + void HandleMessage(Message *msg, struct timeval *tv); + virtual void HandleCanMessage(WORD_t cobid, BYTE_t *data, struct timeval *tv) {} + + int Receive(Message *pm); + int ReceiveFast(FastMessage *pm); + + void SetBaudRate(int rate); + void EnableCanBusConnection(); + int EnableFastCan(int rbuffers, int wbuffers); + void DisableCanBusConnection(); + int EnableFifo(); + int Reset(); + void SetTermination(int strate=1); + int StdHost2NewStyle(int rbuffers, int wbuffers, int wbuffers_hi, int wbuffers_low); + void DisableAllCobIds(); + + void PrintMsg(Message *m); + + int Send(Message *pm); + int SendHi(Message *pm); + int SendLo(Message *pm); + int Send(FastMessage *pm); + + void Close(); + int Open(const char *devname); /* pathname of device */ + + WORD_t MsgDescr(const WORD_t cobid, const BYTE_t dlc, const BYTE_t rtr=0) + { + return (cobid<<5) | ((rtr&0x1)<<4) | (dlc&0xf); + } + + virtual void TerminateApp() { exit(-1); } + +protected: + void StartReceiver(); + void StopReceiver(); + +public: + VmodIcan(const char *dev, const int baud, ostream &out=cout); + virtual ~VmodIcan(); + + void EnableCobId(WORD_t cobid, int flag=TRUE); + + void SendCanFrame(WORD_t cobid, BYTE_t m[8]); + void SendCanFrame(WORD_t cobid, + 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); +}; + +#endif Index: trunk/MagicSoft/Cosy/devdrv/macs.cc =================================================================== --- trunk/MagicSoft/Cosy/devdrv/macs.cc (revision 732) +++ trunk/MagicSoft/Cosy/devdrv/macs.cc (revision 732) @@ -0,0 +1,351 @@ +#include "macs.h" + +#include +#include // timeval->tv_sec + +#include "timer.h" +#include "network.h" + +Macs::Macs(BYTE_t nodeid, ostream &out=cout) + : NodeDrv(nodeid, out), fMacId(2*nodeid+1), + fPos(0), fPosTime(0.0), fPdoPos(0), fPdoTime(0.0), + fPosActive(0), fRpmActive(0), fError(FALSE) +{ +} + +Macs::~Macs() +{ +} + +void Macs::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, struct timeval *tv) +{ + switch (idx) + { + case 0x100a: + lout << "- Mac using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl; + return; + + case 0x2002: + cout << "Actual velocity: " << dec << val << endl; + fVel = val; + return; + + case 0x6004: + if (subidx) + return; + +// lout << "Actual Position: " << dec << (signed long)val << endl; + fPos = (LWORDS_t)val; + fPosTime.SetTimer(tv); + return; +/* + case 0x2001: + cout << "Axe Status: 0x" << hex << val << endl; + cout << " - Motor " << (val&0x01?"standing":"moving") << endl; + cout << " - Positioning " << (val&0x02?"active":"inactive") << endl; + cout << " - Rotary mode " << (val&0x04?"active":"inactive") << endl; + cout << " - Attitude control: " << (val&0x40?"off":"on") << endl; + cout << " - Axe resetted: " << (val&0x80?"yes":"no") << endl; + fPosActive = val&0x02; + fRpmActive = val&0x04; + return; + case 0x2003: + if (!subidx) + { + cout << "Input State: "; + for (int i=0; i<8; i++) + cout << (int)(val&(1<>8)&0xff) << endl; + cout << " - Attitude control: " << (val&0x04?"off":"on") << endl; + cout << " - Startswitch active: " << (val&0x20?"yes":"no") << endl; + cout << " - Referenceswitch active: " << (val&0x40?"yes":"no") << endl; + cout << " - Endswitch active: " << (val&0x80?"yes":"no") << endl; + return; +*/ + + case 0x6002: + lout << "- Velocity resolution #" << (int)GetId() << ": " << dec << val << " ticks/min" << endl; + fVelRes = val; + return; + } + cout << "Macs: SDO, idx=0x"<< hex << idx << "/" << (int)subidx; + cout << ", val=0x"<SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]); +} + +void Macs::SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0, + BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0) +{ + GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5); +} + +void Macs::HandlePDO1(BYTE_t *data, struct timeval *tv) +{ + fPdoPos = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7]; + + fPosActive = data[3]&0x02; + fRpmActive = data[3]&0x04; + + fPdoTime.SetTimer(tv); +} + +void Macs::HandlePDO2(BYTE_t *data, struct timeval *tv) +{ + LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3]; + LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7]; + + if (!errnum) + { + cout << "Mac #" << (int)GetId() << " reports Error occursion." << endl; + fError = TRUE; + return; + } + + cout << "Mac #" << (int)GetId() << " reports: "; + switch (errnum) + { + case 6: + cout << "Home position not the first positioning command." << endl; + return; + + case 8: + cout << "Control deviation overflow." << endl; + return; + + case 9: + cout << "Zero index not found." << endl; + return; + + case 25: + switch (errinf) + { + case 1: + cout << "Positive"; + break; + case 2: + cout << "Negative"; + break; + default: + cout << "-unknown-"; + } + cout << " endswitch activated." << endl; + fError = FALSE; + return; + + case 84: + cout << "Too many (>12) ON TIME calls." << endl; + return; + + default: + cout << "Error Nr. " << errnum << ", " << errinf << endl; + } +} + +double Macs::GetTime() +{ + return fPosTime.GetTime(); +} + +double Macs::GetMjd() +{ + return fPosTime.GetMjd(); +} + +double Macs::GetPdoTime() +{ + return fPdoTime.GetTime(); +} + +double Macs::GetPdoMjd() +{ + return fPdoTime.GetMjd(); +} + Index: trunk/MagicSoft/Cosy/devdrv/macs.h =================================================================== --- trunk/MagicSoft/Cosy/devdrv/macs.h (revision 732) +++ trunk/MagicSoft/Cosy/devdrv/macs.h (revision 732) @@ -0,0 +1,86 @@ +#ifndef MACS_H +#define MACS_H + +#include "timer.h" +#include "nodedrv.h" + +class Macs : public NodeDrv +{ +private: + BYTE_t fMacId; + + LWORD_t fVelRes; + LWORDS_t fVel; + + LWORDS_t fPos; + Timer fPosTime; + + LWORDS_t fPdoPos; + Timer fPdoTime; + + BYTE_t fPosActive; + BYTE_t fRpmActive; + + int fError; // Indicater for error state + + LWORD_t string(BYTE_t b0=0, BYTE_t b1=0, BYTE_t b2=0, BYTE_t b3=0) + { + return (LWORD_t)(b0<<24 | b1<<16 | b2<<8 | b3); + } + +public: + Macs(BYTE_t nodeid, ostream &out=cout); + virtual ~Macs(); + + void InitDevice(Network *); + + void StartDevice(); + void StopDevice(); + + void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, struct timeval *tv); + void HandleSDOOK(WORD_t idx, BYTE_t subidx) { NodeDrv::HandleSDOOK(idx, subidx); } + void HandleSDOError(LWORD_t data) { NodeDrv::HandleSDOError(data); } + + void HandlePDO1(BYTE_t *data, struct timeval *tv); + void HandlePDO2(BYTE_t *data, struct timeval *tv); + + void SendMsg(BYTE_t data[6]); + void SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0, + BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0); + + void ReqPos(); + void ReqVel(); + void ReqAxEnd(); + void ReqVelRes(); + void SetHome(LWORDS_t pos=0); + void SetAcceleration(LWORD_t acc); + void SetDeceleration(LWORD_t dec); + void SetVelocity(LWORD_t vel); + void SetNoWait(BYTE_t flag=TRUE); + void SetRpmMode(BYTE_t mode=TRUE); + void SetRpmVelocity(LWORDS_t cvel); + void SetPDO1On(BYTE_t flag=TRUE); + + void StartRelPos(LWORDS_t pos); + void StartAbsPos(LWORDS_t pos); + + void StopMotor(); + + int IsPositioning() { return fPosActive; } + + double GetTime(); + double GetMjd(); + + double GetPdoTime(); + double GetPdoMjd(); + + LWORDS_t GetPdoPos() { return fPdoPos; } + + LWORDS_t GetPos() { return fPos; } + LWORDS_t GetVel() { return fVel; } + LWORD_t GetVelRes() { return fVelRes; } + + int HasError() { return fError; } +}; + +#endif Index: trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc =================================================================== --- trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc (revision 732) +++ trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc (revision 732) @@ -0,0 +1,293 @@ +#include "shaftencoder.h" + +#include "timer.h" +#include "network.h" + +#include // cout +#include // setw, setfill + +#include // gSystem +#include // TGLabel->SetText + +#include +#include // PRIO_PROCESS + +ShaftEncoder::ShaftEncoder(BYTE_t nodeid, ostream &out=cout) : NodeDrv(nodeid, out), fLabel(NULL) +{ + // + // Show information + // + pthread_create(&fThread, NULL, MapUpdateThread, this); + +} + +ShaftEncoder::~ShaftEncoder() +{ + pthread_cancel(fThread); +} + +void ShaftEncoder::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, struct timeval *tv) +{ + switch (idx) + { + case 0x1000: + cout << "Model: "; + switch (val&0xffff) + { + case 0x0196: + cout << "Shaft Encoder Type: "; + switch ((val>>16)&0xff) + { + case 0x01: + cout << "Singleturn" << endl; + return; + case 0x02: + cout << "Multiturn" << endl; + return; + default: + cout << "?" << endl; + return; + } + default: + cout << "???" << endl; + return; + } + case 0x100b: + cout << "Node ID: " << dec << val << endl; + return; + + case 0x6000: + case 0x6500: + cout << "Counting: " << (val&1 ?"anti-clockwise":"clockwise") << " "; + cout << "HwTest: " << (val&2 ?"on":"off") << " "; + cout << "Scaling: " << (val&4 ?"on":"off") << " "; + cout << "Modulo: " << (val&4096?"on":"off") << endl; + + case 0x6001: + cout << "Logical Ticks/Turn: " << dec << val << endl; + return; + + case 0x6004: + cout << "Position: " << dec << val << endl; + fPos = val; + fTurn = 0; + return; + + + case 0x6501: + cout << "Phys. Ticks/Turn: " << dec << val << endl; + fTicks = val; + return; + + case 0x6502: + cout << "Possible Turns: " << dec << val << endl; + fTurns = val ? val : 1; // Single Turn = Multiturn with one turn + return; + + + } + cout << hex << setfill('0'); + cout << "Sdo=" << idx << "/" << (int)subidx << ": 0x" << setw(8) << val; + cout << endl; +} + +void *ShaftEncoder::MapUpdateThread(void *data) +{ + ShaftEncoder *se = (ShaftEncoder*) data; + + // + // Detach thread (make sure that it is really removed from memory) + // + pthread_detach(pthread_self()); + setpriority(PRIO_PROCESS, 0, 5); + + se->UpdateThread(); + + return NULL; +} + +void ShaftEncoder::UpdateThread() +{ + // + // check for a running thread and lock mutex + // + char text[21]; + + // + // Wait until the network and the output widget is initialized + // + while (!GetNetwork() || !fLabel) + usleep(1); + + while (1) + { + WaitForNextPdo1(); + + // + // Update information + // + sprintf(text, "%ld", fPos); + fLabel[0]->SetText(new TGString(text)); + + sprintf(text, "%d", fVel); + fLabel[1]->SetText(new TGString(text)); + + sprintf(text, "%d", fAcc); + fLabel[2]->SetText(new TGString(text)); + + // + // make updated information visible + // + gSystem->ProcessEvents(); + } +} + +void ShaftEncoder::HandlePDOType0(BYTE_t *data) +{ + // + // Decode information, we have a 14bit only + // + fPos = data[0] | (data[1]<<8) | (data[2]<<16); // | (data[3]<<24); +} + +void ShaftEncoder::HandlePDOType1(BYTE_t *data) +{ + // + // Decode information, we have a 14bit only + // + LWORDS_t pos = data[0] | (data[1]<<8) | (data[2]<<16); // | (data[3]<<24); + BYTE_t flag = data[4]; + pos=pos; + flag=flag; +} + +void ShaftEncoder::HandlePDOType2(BYTE_t *data, struct timeval *tv) +{ + // + // Decode information, we have a 14bit only + // + LWORDS_t pos = data[0] | (data[1]<<8) | (data[2]<<16); // | (data[3]<<24); + + fVel = data[4] | (data[5]<<8); + fAcc = data[6] | (data[7]<<8); + + fTime.SetTimer(tv); + + const int uplim = 9*fTicks/10; + const int dnlim = 1*fTicks/10; + + if (fPos > uplim && pos < dnlim) + fTurn++; + + if (fPos < dnlim && pos > uplim) + fTurn--; + + fPos = pos; +} + +double ShaftEncoder::GetTime() +{ + return fTime.GetTime(); +} + +double ShaftEncoder::GetMjd() +{ + return fTime.GetMjd(); +} + +void ShaftEncoder::InitDevice(Network *net) +{ + NodeDrv::InitDevice(net); + + //----------------------------------------------------------------------- + // Start Setup of the Shaft Encoder + //----------------------------------------------------------------------- + + // + // Requesting and checking (FIXME) type of encoder + // + lout << "- Requesting SDO 0x1000 of " << (int)GetId() << endl; + RequestSDO(0x1000); + WaitForSdo(0x1000); + + // + // Read physical ticks per turn + // + lout << "- Requesting SDO 0x6501 of " << (int)GetId() << endl; + RequestSDO(0x6501); + WaitForSdo(0x6501); + + // + // Read number of possible ticks per turn + // + lout << "- Requesting SDO 0x6502 of " << (int)GetId() << endl; + RequestSDO(0x6502); + WaitForSdo(0x6502); + + // + // Set logic ticks per turn = physical ticks per turn => scale factor = 1 + // + lout << "- Configuring SDO 0x6001 of " << (int)GetId() << endl; + SendSDO(0x6001, fTicks); + WaitForSdo(0x6001); + + // + // Set maximum number of ticks (ticks * turns) + // + lout << "- Configuring SDO 0x6002 of " << (int)GetId() << endl; + SendSDO(0x6002, (LWORD_t)(fTicks*fTurns)); + WaitForSdo(0x6002); + + // + // Configure PDOs + // + lout << "- Configuring SDO 0x1802 of " << (int)GetId() << endl; + SendSDO(0x1802, 1, (LWORD_t)0x281); + WaitForSdo(0x1802, 1); + + // + // Delete preset Value + // + lout << "- Configuring SDO 0x6003 of " << (int)GetId() << endl; + SendSDO(0x6003, (LWORD_t)0xffffffff); + WaitForSdo(0x6003); + + // + // Request Parameter + // + lout << "- Requesting SDO 0x6000 of " << (int)GetId() << endl; + RequestSDO(0x6000); + WaitForSdo(0x6000); + + ReqPos(); + + lout << "- Start Node " << (int)GetId() << endl; + SendNMT(kNMT_START); +} + +void ShaftEncoder::ReqPos() +{ + // + // Request Position + // + lout << "- Requesting Position of #" << (int)GetId() << endl; + RequestSDO(0x6004); + WaitForSdo(0x6004); +} + +void ShaftEncoder::SetPreset(LWORD_t pre) +{ + fPos = pre%16384; + fTurn = pre/16384; + + lout << " - Setting Preset #" << (int)GetId() << endl; + SendSDO(0x6003, (LWORD_t)fPos); + WaitForSdo(0x6003); +} + +void ShaftEncoder::StopDevice() +{ + lout << "- Start Node " << (int)GetId() << endl; + SendNMT(kNMT_STOP); +} + Index: trunk/MagicSoft/Cosy/devdrv/shaftencoder.h =================================================================== --- trunk/MagicSoft/Cosy/devdrv/shaftencoder.h (revision 732) +++ trunk/MagicSoft/Cosy/devdrv/shaftencoder.h (revision 732) @@ -0,0 +1,62 @@ +#ifndef SHAFTENCODER_H +#define SHAFTENCODER_H + +#include "timer.h" +#include "nodedrv.h" + +#include + +class TGLabel; + +class ShaftEncoder : public NodeDrv +{ +private: + LWORDS_t fPos; // ticks + WORDS_t fVel; // ticks per 5ms + WORDS_t fAcc; // ticks per 25ms^2 + WORDS_t fTurn; // Number of turn + LWORD_t fTicks; // Number of ticks per turn + WORD_t fTurns; // Number of possible turns + + TGLabel **fLabel; + + Timer fTime; + + pthread_t fThread; + + static void *MapUpdateThread(void *se); + void UpdateThread(); + + void HandlePDOType0(BYTE_t *data); + void HandlePDOType1(BYTE_t *data); + void HandlePDOType2(BYTE_t *data, struct timeval *tv); + + void ReqPos(); + +public: + ShaftEncoder(BYTE_t nodeid, ostream &out=cout); + virtual ~ShaftEncoder(); + + void InitDevice(Network *); + + void StartDevice(); + void StopDevice(); + + void SetDisplay(TGLabel **label) { fLabel = label; } + + void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, struct timeval *tv); + void HandleSDOOK(WORD_t idx, BYTE_t subidx) { NodeDrv::HandleSDOOK(idx, subidx); } + void HandleSDOError(LWORD_t data) { NodeDrv::HandleSDOError(data); } + + void HandlePDO1(BYTE_t *data, struct timeval *tv) { HandlePDOType2(data, tv); } + void HandlePDO2(BYTE_t *data, struct timeval *tv) { HandlePDOType2(data, tv); } + + LWORDS_t GetPos() { return fPos+fTurn*fTicks; } + + double GetTime(); + double GetMjd(); + + void SetPreset(LWORD_t pre=0); +}; + +#endif Index: trunk/MagicSoft/Cosy/gui/MGCoordinate.cc =================================================================== --- trunk/MagicSoft/Cosy/gui/MGCoordinate.cc (revision 732) +++ trunk/MagicSoft/Cosy/gui/MGCoordinate.cc (revision 732) @@ -0,0 +1,247 @@ +// +// This File contains the definition of the MGCoordinate-class +// +// Author: Thomas Bretz +// Version: V1.0 (1-8-2000) + +#include "MGCoordinate.h" + +#include // atoi +#include // cout + +#include +#include +#include + +#include "slalib.h" +#include "slamac.h" + +enum { + IDM_kDeg, + IDM_kMin, + IDM_kSec +}; + +MGCoordinate::MGCoordinate(const TGWindow* p, + const Bool_t flag, const char *txt, + const UInt_t deg, const UInt_t min, const UInt_t sec) +: TGFrame(p, 114, flag?76:46, kSunkenFrame|kFixedSize), fDeg(deg), fMin(min), fSec(sec) +{ + // p = pointer to MainFrame (not owner) + if (flag) + { + fTextEntryDeg = new TGTextEntry(this, "****", IDM_kDeg); + fTextEntryMin = new TGTextEntry(this, "***", IDM_kMin); + fTextEntrySec = new TGTextEntry(this, "***", IDM_kSec); + // fTextEntryDeg->SetAlignment(kTextCenterX); + // fTextEntryMin->SetAlignment(kTextCenterX); + // fTextEntrySec->SetAlignment(kTextCenterX); + fTextEntryDeg->Move( 4, 26); + fTextEntryMin->Move(44, 26); + fTextEntrySec->Move(78, 26); + fTextEntryDeg->MapWindow(); + fTextEntryMin->MapWindow(); + fTextEntrySec->MapWindow(); + fList.Add(fTextEntrySec); + fList.Add(fTextEntryMin); + fList.Add(fTextEntryDeg); + + Set(fTextEntryDeg, fDeg); + Set(fTextEntryMin, fMin); + Set(fTextEntrySec, fSec); + } + + const int ypos = flag?56:26; + + fLabelDeg = new TGLabel(this, "****"); + fLabelMin = new TGLabel(this, "***"); + fLabelSec = new TGLabel(this, "***"); + fLabelDeg->SetTextJustify(kTextRight); + fLabelMin->SetTextJustify(kTextRight); + fLabelSec->SetTextJustify(kTextRight); + fLabelDeg->Move( 4+6, ypos); + fLabelMin->Move(44+6, ypos); + fLabelSec->Move(78+6, ypos); + fLabelDeg->MapWindow(); + fLabelMin->MapWindow(); + fLabelSec->MapWindow(); + fList.Add(fLabelSec); + fList.Add(fLabelDeg); + fList.Add(fLabelMin); + + Set(fLabelDeg, fDeg); + Set(fLabelMin, fMin); + Set(fLabelSec, fSec); + + fLabel = new TGLabel(this, txt); + fLabel->SetTextJustify(kTextLeft); + fLabel->Move(4, 4); + fLabel->MapWindow(); + fList.Add(fLabel); + + TGLabel *label; + + if (flag) + { + label = new TGLabel(this, "\xb0"); + label->SetTextJustify(kTextLeft); + label->Move(36, 26); + label->MapWindow(); + fList.Add(label); + + label = new TGLabel(this, "'"); + label->SetTextJustify(kTextLeft); + label->Move(70, 26); + label->MapWindow(); + fList.Add(label); + + label = new TGLabel(this, "\""); + label->SetTextJustify(kTextLeft); + label->Move(104, 26); + label->MapWindow(); + fList.Add(label); + } + + label = new TGLabel(this, "\xb0"); + label->SetTextJustify(kTextLeft); + label->Move(36, ypos); + label->MapWindow(); + fList.Add(label); + + label = new TGLabel(this, "'"); + label->SetTextJustify(kTextLeft); + label->Move(70, ypos); + label->MapWindow(); + fList.Add(label); + + label = new TGLabel(this, "\""); + label->SetTextJustify(kTextLeft); + label->Move(104, ypos); + label->MapWindow(); + fList.Add(label); + + MapWindow(); +} + +MGCoordinate::~MGCoordinate() +{ + cout << "MGCoordinate destroyed." << endl; +} + +Double_t MGCoordinate::GetVal() const +{ + return (Double_t)(60*(60*fDeg+fMin)+fSec)/3600; +} + +void MGCoordinate::Print() +{ + cout << fLabel->GetText()->GetString() << " " << fDeg << "\xb0 " << fMin << "' " << fSec << "\"" << endl; +} + +void MGCoordinate::Set(TGLabel *label, UInt_t val) +{ + char txt[20]; + + sprintf(txt, "%d", val); + + label->SetText(new TGString(txt)); +} + +void MGCoordinate::Set(TGTextEntry *entry, UInt_t val) +{ + char txt[20]; + + sprintf(txt, "%d", val); + + entry->SetText(txt); +} + +void MGCoordinate::SetVal(const Double_t d) +{ + int dms[4]; + char sign; + + //** ndp int number of decimal places of arcseconds + //** angle double angle in radians + //** sign char* '+' or '-' + //** idmsf int[4] degrees, arcminutes, arcseconds, fraction + + slaDr2af(0, d*D2PI/360.0, &sign, dms); + + + fDeg = sign=='-'?-dms[0]:dms[0]; + fMin = dms[1]; + fSec = dms[2]; + + Set(fLabelDeg, fDeg); + Set(fLabelMin, fMin); + Set(fLabelSec, fSec); + + // Set(fTextEntryDeg, fDeg); + // Set(fTextEntryMin, fMin); + // Set(fTextEntrySec, fSec); +} + + +Bool_t MGCoordinate::Set(TGLabel *label, UInt_t &val, TGTextEntry *entry) +{ + UInt_t newval = atoi(entry->GetText()); + + Bool_t ok = (entry == fTextEntryDeg || newval<60); + + if (ok) + { + val = newval; + Set(label, val); + } + + Set(entry, val); + + return ok; +} + +Bool_t MGCoordinate::ProcessMessage(Long_t msg, Long_t mp1, Long_t mp2) +{ + if (GET_MSG(msg)!=kC_TEXTENTRY || GET_SUBMSG(msg)!=kTE_ENTER) + return kTRUE; + + // kC_TEXTENTRY = 4, + // kTE_TEXTCHANGED = 1, + // kTE_ENTER = 2, + + switch (mp1) + { + case IDM_kDeg: + if (!Set(fLabelDeg, fDeg, fTextEntryDeg)) + fTextEntryDeg->SelectAll(); + else + { + fTextEntryMin->SetFocus(); + fTextEntryMin->SelectAll(); + } + return kTRUE; + + case IDM_kMin: + if(!Set(fLabelMin, fMin, fTextEntryMin)) + fTextEntryMin->SelectAll(); + else + { + fTextEntrySec->SetFocus(); + fTextEntrySec->SelectAll(); + } + return kTRUE; + + case IDM_kSec: + if (!Set(fLabelSec, fSec, fTextEntrySec)) + fTextEntrySec->SelectAll(); + else + { + fTextEntryDeg->SetFocus(); + fTextEntryDeg->SelectAll(); + } + return kTRUE; + } + + return kTRUE; +} + Index: trunk/MagicSoft/Cosy/gui/MGCoordinate.h =================================================================== --- trunk/MagicSoft/Cosy/gui/MGCoordinate.h (revision 732) +++ trunk/MagicSoft/Cosy/gui/MGCoordinate.h (revision 732) @@ -0,0 +1,56 @@ +#ifndef MGCOORDINATE_H +#define MGCOORDINATE_H + +// +// This File contains the declaration of the MGCoordinates-class +// +// Author: Thomas Bretz +// Version: V1.0 (1-8-2000) + +#ifndef ROOT_TGFrame +#include +#endif +#ifndef MGLIST_H +#include "MGList.h" +#endif + +class TGLabel; +class TGTextEntry; + +class MGCoordinate : public TGFrame +{ + UInt_t fDeg; + UInt_t fMin; + UInt_t fSec; + + MGList fList; + + TGTextEntry *fTextEntryDeg; + TGTextEntry *fTextEntryMin; + TGTextEntry *fTextEntrySec; + + TGLabel *fLabelDeg; + TGLabel *fLabelMin; + TGLabel *fLabelSec; + + TGLabel *fLabel; + + void Set(TGLabel *label, const UInt_t val); + void Set(TGTextEntry *entry, const UInt_t val); + Bool_t Set(TGLabel *label, UInt_t &val, TGTextEntry *label); + +public: + MGCoordinate(const TGWindow* p, + const Bool_t flag=kTRUE, const char *txt="Coordinates:", + const UInt_t deg=0, const UInt_t min=0, const UInt_t sec=0); + ~MGCoordinate(); + + Double_t GetVal() const; + void SetVal(Double_t d); + + Bool_t ProcessMessage(Long_t msg, Long_t mp1, Long_t mp2); + + void Print(); +}; + +#endif // MGIMAGE_H Index: trunk/MagicSoft/Cosy/gui/MGCoordinates.cc =================================================================== --- trunk/MagicSoft/Cosy/gui/MGCoordinates.cc (revision 732) +++ trunk/MagicSoft/Cosy/gui/MGCoordinates.cc (revision 732) @@ -0,0 +1,53 @@ +// +// This File contains the definition of the MGCoordinates-class +// +// Author: Thomas Bretz +// Version: V1.0 (1-8-2000) + +#include "MGCoordinates.h" + +#include // cout + +#include "MGCoordinate.h" + +MGCoordinates::MGCoordinates(const TGWindow* p, + const Bool_t flag, + const char *txt1, const char *txt2, + const UInt_t deg1, const UInt_t min1, const UInt_t sec1, + const UInt_t deg2, const UInt_t min2, const UInt_t sec2) +: TGFrame(p, 234, 76, kFixedSize) +{ + // p = pointer to MainFrame (not owner) + fX = new MGCoordinate(this, flag, txt1, deg1, min1, sec1); + fX->Move(0, 0); + + fY = new MGCoordinate(this, flag, txt2, deg2, min2, sec2); + fY->Move(120, 0); + + MapWindow(); +} + +MGCoordinates::~MGCoordinates() +{ + delete fY; + delete fX; + + cout << "MGCoordinates destroyed." << endl; +} + +XY MGCoordinates::GetCoordinates() const +{ + return XY(fX->GetVal(), fY->GetVal()); +} + +void MGCoordinates::SetCoordinates(const XY &xy) +{ + fX->SetVal(xy.X()); + fY->SetVal(xy.Y()); +} + +void MGCoordinates::Print() +{ + fX->Print(); + fY->Print(); +} Index: trunk/MagicSoft/Cosy/gui/MGCoordinates.h =================================================================== --- trunk/MagicSoft/Cosy/gui/MGCoordinates.h (revision 732) +++ trunk/MagicSoft/Cosy/gui/MGCoordinates.h (revision 732) @@ -0,0 +1,39 @@ +#ifndef MGCOORDINATES_H +#define MGCOORDINATES_H + +// +// This File contains the declaration of the MGCoordinates-class +// +// Author: Thomas Bretz +// Version: V1.0 (1-8-2000) + +#ifndef ROOT_TGFrame +#include +#endif +#ifndef COORD_H +#include "coord.h" +#endif + +class MGCoordinate; + +class MGCoordinates : public TGFrame +{ + MGCoordinate *fX; + MGCoordinate *fY; + +public: + MGCoordinates(const TGWindow* p, + const Bool_t flag=kTRUE, + const char *txt1="Coordinate1:", + const char *txt2="Coordinate2:", + const UInt_t deg1=0, const UInt_t min1=0, const UInt_t sec1=0, + const UInt_t deg2=0, const UInt_t min2=0, const UInt_t sec2=0); + ~MGCoordinates(); + + XY GetCoordinates() const; + void SetCoordinates(const XY &xy); + + void Print(); +}; + +#endif // MGIMAGE_H Index: trunk/MagicSoft/Cosy/gui/MGImage.cc =================================================================== --- trunk/MagicSoft/Cosy/gui/MGImage.cc (revision 732) +++ trunk/MagicSoft/Cosy/gui/MGImage.cc (revision 732) @@ -0,0 +1,351 @@ +// +// This File contains the definition of the MGImage-class +// +// Author: Thomas Bretz +// Version: V1.0 (1-8-2000) +// +// x11/src/GX11Gui.cxx +// +#include "MGImage.h" + +#include +#include + +MGImage::MGImage(const TGWindow* p, UInt_t w, UInt_t h, UInt_t options, ULong_t back) + : TGFrame(p, w, h, options, back), fWidth(w), fHeight(h) +{ + // p = pointer to MainFrame (not owner) + // w = width of frame + // h = width of frame + + // + // Set Color Table (Gray scale) + // + const int cols = 0x100+4*4*4; + + for (int c=0; c<0x100; c++) + sprintf(fColors[c], "%02x", c); + + // + // create space for the pixmap buffer, initialize pointer to data area + // + fBuffer = new char*[1+cols+fHeight+1]; + fBody = &fBuffer[1+cols]; + + // + // fill buffer with header informations + // + fBuffer[0] = new char[14]; + sprintf(fBuffer[0], "%3d %3d %3d %1d", fWidth, fHeight, cols, 2); + + for (int k=0; k<0x100; k++) + { + const int l = k+1; + fBuffer[l] = new char[13]; + fBuffer[l][0] = fColors[k][0]; + fBuffer[l][1] = fColors[k][1]; + fBuffer[l][2] = '\t'; + fBuffer[l][3] = 'c'; + fBuffer[l][4] = ' '; + fBuffer[l][5] = '#'; + fBuffer[l][6] = fColors[k][0]; + fBuffer[l][7] = fColors[k][1]; + fBuffer[l][8] = fColors[k][0]; + fBuffer[l][9] = fColors[k][1]; + fBuffer[l][10] = fColors[k][0]; + fBuffer[l][11] = fColors[k][1]; + fBuffer[l][12] = '\0'; + } + + for (int b=0; b<4; b++) + for (int g=0; g<4; g++) + for (int r=0; r<4; r++) + { + const int nr = r+(g<<2)+(b<<4); + const int l = 0x100+nr+1; + + fBuffer[l] = new char[13]; + fBuffer[l][0] = 'f'+nr/8+1; + fBuffer[l][1] = 'f'+nr%8+1; + fBuffer[l][2] = '\t'; + fBuffer[l][3] = 'c'; + fBuffer[l][4] = ' '; + fBuffer[l][5] = '#'; + fBuffer[l][6] = fColors[r*85][0]; + fBuffer[l][7] = fColors[r*85][1]; + fBuffer[l][8] = fColors[g*85][0]; + fBuffer[l][9] = fColors[g*85][1]; + fBuffer[l][10] = fColors[b*85][0]; + fBuffer[l][11] = fColors[b*85][1]; + fBuffer[l][12] = '\0'; + } + + // + // mark end of lines of the data area + // + for (UInt_t y=0; yCreateGC(fId, 0); // GetBckgndGC(); // + + // + // Creat drawing semaphore + // + fMuxPixmap = new pthread_mutex_t; + pthread_mutex_init((pthread_mutex_t*)fMuxPixmap, NULL); + + // + // create empty (black) pixmap + // return (Pixmap_t) XCreatePixmap(fDisplay, (Drawable) id, w, h, + // DefaultDepth(fDisplay, DefaultScreen(fDisplay))); + // + fPixmap = kNone; //@@@gVirtualX->CreatePixmap(fId, fWidth, fHeight); + + Resize(w, h); +} + +void MGImage::Resize(UInt_t w, UInt_t h) +{ + TGFrame::Resize(w+2*GetBorderWidth(), h+2*GetBorderWidth()); + // FIXME: RESIZE THE PIXMAP +} + +void MGImage::Resize(TGDimension size) +{ + TGFrame::Resize(size.fWidth+2*GetBorderWidth(), size.fHeight+2*GetBorderWidth()); + // FIXME: RESIZE THE PIXMAP +} + +void MGImage::MoveResize(Int_t x, Int_t y, UInt_t w, UInt_t h) +{ + TGFrame::MoveResize(x, y, w+2*GetBorderWidth(), h+2*GetBorderWidth()); + // FIXME: RESIZE THE PIXMAP +} + +MGImage::~MGImage() +{ + pthread_mutex_lock((pthread_mutex_t*)fMuxPixmap); + + char *b = *fBuffer; + while (*b) + delete[] b++; + delete[] fBuffer; + + if (fPixmap!=kNone) // @@@ + gVirtualX->DeletePixmap(fPixmap); // XFreePixmap(fDisplay, (Pixmap) pmap); + gVirtualX->DeleteGC(fDefGC); // XFreeGC(fDisplay, (GC) gc); + + pthread_mutex_destroy((pthread_mutex_t*)fMuxPixmap); + + cout << "MGImage destroyed." << endl; +} + +/* +#include +#include +#include +XImage *fPix=NULL; +*/ + +void MGImage::DoRedraw() +{ + // Pixmap_t pm = gVirtualX->CreatePixmap(this->GetId(), buffer, + // 768, 576, 0, 0xff, 8); + TGFrame::DrawBorder(); + pthread_mutex_lock((pthread_mutex_t*)fMuxPixmap); + + // Copy a drawable (i.e. pixmap) to another drawable (pixmap, window). + // The graphics context gc will be used and the source will be copied + // from src_x,src_y,src_x+width,src_y+height to dest_x,dest_y. + // XCopyArea(fDisplay, src, dest, (GC) gc, src_x, src_y, width, height, + // dest_x, dest_y); + if (fPixmap!=kNone) //@@@ + gVirtualX->CopyArea(fPixmap, fId, fDefGC, + 0, 0, fWidth, fHeight, + GetBorderWidth(), GetBorderWidth()); +/* + if (fPix) + { + cout << "put" << flush; + XPutImage((Display*)gVirtualX->GetDisplay(), fId, + fDefGC, fPix, + 0, 0, GetBorderWidth(), GetBorderWidth(), + fWidth, fHeight); + + cout << "done " << endl; + } +*/ + pthread_mutex_unlock((pthread_mutex_t*)fMuxPixmap); +} + + +#include +void MGImage::DrawImg(const char *buffer) +{ + if (pthread_mutex_trylock((pthread_mutex_t*)fMuxPixmap)) + return; + + for (UInt_t y=0; yGetDisplay(); + Screen *scr = DefaultScreenOfDisplay(dsp); + Visual *vis = DefaultVisualOfScreen(scr); + + cout << vis->visualid << endl; + cout << vis->c_class << endl; + cout << vis->bits_per_rgb << endl; + cout << vis->map_entries << endl; + Visual visual; + visual.c_class = StaticGray; + + int n; + XPixmapFormatValues *fmt = XListPixmapFormats(dsp, &n); + + cout << "N: " << n << endl; + for (int i=0; imap_entries; i++) + { + XColor color; + char data[4]; + + color.flags = DoRed | DoGreen | DoBlue; + color.pixel = i; + color.red = (256*i/vis->map_entries) << 8; + color.green = (256*i/vis->map_entries) << 8; + color.blue = (256*i/vis->map_entries) << 8; + + XAllocColor(dsp, colormap, &color); + + cout << color.pixel <<" " << flush; + } + fPix = XCreateImage(dsp, vis, 8, ZPixmap, 0, + buffer, 768, 576, 32, 0); + + cout << "Colors" << visual.visualid << flush; + + } + cout << "Done " << (void*)fPix << endl; + */ + Pixmap_t mask = kNone; + PictureAttributes_t attr; + attr.fMask = kNone; + if (fPixmap!=kNone) // @@@ + gVirtualX->DeletePixmap(fPixmap); // XFreePixmap(fDisplay, (Pixmap) pmap); + + // Create a pixture pixmap from data. The picture attributes + // are used for input and output. Returns kTRUE in case of success, + // kFALSE otherwise. If mask does not exist it is set to kNone. + // + // XpmAttributes xpmattr; + // + // MapPictureAttributes(attr, xpmattr); + // + // Int_t res = XpmCreatePixmapFromData(fDisplay, id, data, (Pixmap*)&pict, + // (Pixmap*)&pict_mask, &xpmattr); + // + // MapPictureAttributes(attr, xpmattr, kFALSE); + // XpmFreeAttributes(&xpmattr); + // + // if (res == XpmSuccess || res == XpmColorError) + // return kTRUE; + // + // if (pict) { + // XFreePixmap(fDisplay, (Pixmap)pict); + // pict = kNone; + // } + // if (pict_mask) { + // XFreePixmap(fDisplay, (Pixmap)pict_mask); + // pict_mask = kNone; + // } + // return kFALSE; + fPixmap=kNone; + if (!gVirtualX->CreatePictureFromData(fId, fBuffer, fPixmap, + mask, attr)) + { + cout << "Warning: Error in CreatePictureFromData" << endl; + fPixmap=kNone; + } + + pthread_mutex_unlock((pthread_mutex_t*)fMuxPixmap); + DoRedraw(); +} + +void MGImage::DrawColImg(const char *gbuf, const char *cbuf) +{ + if (pthread_mutex_trylock((pthread_mutex_t*)fMuxPixmap)) + return; + + for (UInt_t y=0; yDeletePixmap(fPixmap); + fPixmap=kNone; + + if (!gVirtualX->CreatePictureFromData(fId, fBuffer, fPixmap, + mask, attr)) + { + cout << "Warning: Error in CreatePictureFromData" << endl; + fPixmap=kNone; + } + + pthread_mutex_unlock((pthread_mutex_t*)fMuxPixmap); + DoRedraw(); +} + Index: trunk/MagicSoft/Cosy/gui/MGImage.h =================================================================== --- trunk/MagicSoft/Cosy/gui/MGImage.h (revision 732) +++ trunk/MagicSoft/Cosy/gui/MGImage.h (revision 732) @@ -0,0 +1,44 @@ +#ifndef MGIMAGE_H +#define MGIMAGE_H + +// +// This File contains the declaration of the MGImage-class +// +// Author: Thomas Bretz +// Version: V1.0 (1-8-2000) + +#include + +class MGImage : public TGFrame +{ + char fColors[0x100][3]; + + char **fBuffer; + char **fBody; + + GContext_t fDefGC; + Drawable_t fId; + Pixmap_t fPixmap; + + UInt_t fWidth; + UInt_t fHeight; + + void *fMuxPixmap; //! test + +public: + +public: + MGImage(const TGWindow* p, UInt_t w, UInt_t h, UInt_t options = kSunkenFrame, ULong_t back = fgDefaultFrameBackground); + ~MGImage(); + + void DoRedraw(); + + void Resize(UInt_t w, UInt_t h); + void Resize(TGDimension size); + void MoveResize(Int_t x, Int_t y, UInt_t w, UInt_t h); + + void DrawImg(const char *buffer); + void DrawColImg(const char *gbuf, const char *cbuf); +}; + +#endif // MGIMAGE_H