Index: trunk/MagicSoft/Cosy/MCosy.cc =================================================================== --- trunk/MagicSoft/Cosy/MCosy.cc (revision 738) +++ trunk/MagicSoft/Cosy/MCosy.cc (revision 738) @@ -0,0 +1,760 @@ +#include "MCosy.h" + +#include +#include +#include + +#include +#include +#include + +#include "MGCosy.h" + +#include "macs.h" +#include "timer.h" +#include "slalib.h" +#include "slamac.h" +#include "shaftencoder.h" + +#include // PRIO_PROCESS + +typedef struct tm tm_t; + +#define GEAR_RATIO_ALT 75.55 // 75.25 VERY IMPORTANT! unit=RE/SE +#define GEAR_RATIO_AZ 179.8 // VERY IMPORTANT! unit=RE/SE + +const XY kGearRatio(GEAR_RATIO_ALT, GEAR_RATIO_AZ); +const XY kGearRatio2(GEAR_RATIO_ALT*16384.0/360.0, GEAR_RATIO_AZ*16384.0/360.0); + +double Rad2SE(double rad) +{ + return 16384.0/D2PI*rad; +} + +double Rad2ZdRE(double rad) +{ + return 16384.0/D2PI*rad*kGearRatio.X(); +} + +double Rad2AzRE(double rad) +{ + return 16384.0/D2PI*rad*kGearRatio.Y(); +} + +double Deg2ZdRE(double rad) +{ + return rad*kGearRatio2.X(); +} + +double Deg2AzRE(double rad) +{ + return rad*kGearRatio2.Y(); +} + +//double Rad2Deg(double rad) +//{ +// return 360.0/D2PI*rad; +//} + +ZdAz MCosy::CorrectTarget(const ZdAz &src, const ZdAz &dst) +{ + // src [se] + // dst [rad] + + // fAltMax = 70 + // fAltMin = -105/110 + // fAzMin = -355 + // fAzMax = 355 + + ZdAz source = src * 360.0/16384.0; + ZdAz dest = dst * 360.0/D2PI; + + if (dest.Zd()>-1e-6 && dest.Zd()<1e-6) + return dst; + + const float fZdMin = -67; + const float fZdMax = 67; + const float fAzMin = -29; + const float fAzMax = 423; + + // + // This corrects to target for the shortest distance, not for the fastest move! + // + ZdAz s = source-dest; + + float min = s.Sqr(); + + // + // Is it enought to search inside one revolution? + // + ZdAz ret = dest; + + for (int i=-5; i<5+1; i++) + { + const ZdAz p(i%2 ? -dest.Zd() : dest.Zd(), dest.Az() - i*180); + + // + // Range Check + // + if (p.Zd()fZdMax) + continue; + + if (p.Az()fAzMax) + continue; + + // + // Calculate distance + // + s = source-p; + + const float dist = s.Sqr(); + + if (dist > min) + continue; + + // + // New shortest distance + // + ret = p; + min = dist; + } + return ret*(16384.0/360.0); +} + + +ZdAz MCosy::GetSePos() +{ + const int p0 = fAlt1->GetPos(); + const int p1 = fAlt2->GetPos(); + const int p2 = fAz->GetPos(); + + const int a0 = p0; //p0>8192?p0-16384:p0; + const int a1 = p1; //p1>8192?p1-16384:p1; + const int a2 = p2; //p2>8192?p2-16384:p2; + + // + // interpolate shaft encoder positions + // + const float a = (float)(a0-a1)/2; + + // + // calculate 'regelabweichung' + // + return ZdAz(a, a2); +} + +ZdAz MCosy::GetRePos() +{ + return ZdAz(fMac2->GetPos(), fMac1->GetPos()); +} + +int MCosy::SetPosition(const ZdAz &dst) // [rad] +{ + // FIXME: CORRECT BY fOffset !!!!!!!!!! + + // + // Calculate new target position (shortest distance to go) + // + const ZdAz src = GetSePos(); + const ZdAz dest = CorrectTarget(src, dst); + + cout << "Positioning to Target:" << endl; +// cout << "Source Alt: " << src.Alt() << "se Az:" << src.Az() << "se -> Alt: " << srcd.Alt() << kDEG << " Az:" << srcd.Az() << kDEG << endl; +// cout << "Shortest Dest Alt: " << dest.Alt() << "se Az:" << dest.Az() << "se -> Alt: " << sed.Alt() << kDEG << " Az:" << sed.Az() << kDEG << endl; + + for (int i=0; i<10; i++) + { + // + // Get Shaft Encoder Positions + // + const ZdAz p=GetSePos(); + + // + // calculate control deviation and rounded cd + // + ZdAz rd = dest-p; // [se] + + ZdAz cd = rd; // [se] + cd.Round(); + + // + // check if we reached the correct position already + // + if (!(int)cd.Zd() && !(int)cd.Az()) + { + cout << "Positioning done with " << i << "manuvers." << endl; + return TRUE; + } + + // + // change units from se to re + // + rd *= kGearRatio; // [re] + + // + // Set velocities + // + const int vr = fMac1->GetVelRes(); + const float maxvel = (i?0.1:0.9)*vr; // maxvel = 90% + const float maxacc = (i?0.1:0.5)*vr; // maxacc = 50%; + + const float diff = i?1:fabs(rd.Ratio()); + + cout << "Salt/Saz: " << diff << endl; + + if (diff <1) + { + fMac1->SetVelocity(maxvel); + fMac1->SetAcceleration(maxacc); + fMac1->SetDeceleration(maxacc); + + fMac2->SetVelocity(maxvel*diff); + fMac2->SetAcceleration(maxacc*diff); + fMac2->SetDeceleration(maxacc*diff); + } + else + { + fMac1->SetVelocity(maxvel/diff); + fMac1->SetAcceleration(maxacc/diff); + fMac1->SetDeceleration(maxacc/diff); + + fMac2->SetVelocity(maxvel); + fMac2->SetAcceleration(maxacc); + fMac2->SetDeceleration(maxacc); + } + + rd.Round(); + + cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl; + cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl; + cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl; + + // + // repositioning (relative) + // + if ((int)cd.Zd()) fMac2->StartRelPos(rd.Zd()); + if ((int)cd.Az()) fMac1->StartRelPos(rd.Az()); + + cout << "Waiting for positioning..." << flush; + + WaitForSdos(); + + cout << "SDO..." << flush; + + while (fMac1->IsPositioning() || fMac2->IsPositioning()) + { + if (StopWaitingForSDO()) + return FALSE; + + usleep(1); + } + + cout << "done." << endl; + } + + cout << "Positioning ERROR!" << endl; + return FALSE; +} + +void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad] +{ + // + // Position to actual position + // + Timer t; + t.GetTime(); + + RaDec pm; + ZdAz dest = RaDec2ZdAz(t.GetMjd(), dst, pm); + + if (!SetPosition(dest)) + { + cout << "ERROR: Cannot start tracking, unable to reach requested position." << endl; + return; + } + + if (StopWaitingForSDO()) + return; + + // + // calculate offset from present se position + // + + const ZdAz sepos = GetSePos()*kGearRatio; + + fMac1->ReqPos(); + fMac2->ReqPos(); + + const ZdAz repos=GetRePos(); + + fOffset = sepos-repos; + + cout << "Offset: " << sepos.Zd() << "re, " << sepos.Az() << "re" << endl; + cout << "Offset: " << repos.Zd() << "re, " << repos.Az() << "re" << endl; + cout << "Offset: " << fOffset.Zd() << "re, " << fOffset.Az() << "re" << endl; + + // + // Start revolution mode + // + fMac2->SetAcceleration(0.90*fMac2->GetVelRes()); + fMac2->SetDeceleration(0.90*fMac2->GetVelRes()); + fMac1->SetAcceleration(0.90*fMac1->GetVelRes()); + fMac1->SetDeceleration(0.90*fMac1->GetVelRes()); + + fMac2->SetRpmMode(TRUE); + fMac1->SetRpmMode(TRUE); + + /*-*/ int s = t.GetSecs(); + cout << "Start tracking: Ra: " << Rad2Deg(dst.Ra()) << kDEG << " Dec: "; + cout << Rad2Deg(dst.Dec()) << kDEG << endl; + + // + // Initialize Tracker (slalib or starguider) + // + fRaDec = dst; + fTracking = kTRUE; + + // + // We want to reach the theoretical position exactly in about 0.5s + // + const float dt = 1; // 1 second + while (!StopWaitingForSDO()) + { + // + // Request Real Position from Drive + // + Timer t; + t.GetTime(); + + // + // Request theoretical Position for a time in the future (To+dt) from CPU + // + dest = CorrectTarget(GetSePos(), RaDec2ZdAz(t.GetMjd()+dt/(60*60*24), dst, pm)); + + fMac2->RequestSDO(0x6004); + fMac1->RequestSDO(0x6004); + WaitForSdos(); + if (StopWaitingForSDO()) + { + lout << "Error 6004 happened" << endl; + SkipPendingSdos(); + break; + } + + // + // Copy fOffset to a local variable + // + ZdAz offset = fOffset; + + // + // distance between (To+dt) and To [re] + // position time difference < 5usec + // + dest *= kGearRatio; + dest -= GetRePos() + offset; + + // + // Velocity to go [re/min] to reach the right position at time t+dt + // correct for the duration of RaDec2AltAz + // + const ZdAz v = dest*60.0/(dt-(fMac2->GetTime()-t)); + + // + // calculate real velocity of future [re/min] + // + ZdAz vt = v/4; + vt.Round(); + + if (v.Zd()>.9*fMac1->GetVelRes() || v.Az()>.9*fMac2->GetVelRes()) + { + cout << "Error: Tracking speed faster than possible maximum velocity." << endl; + break; + } + + // + // Set theoretical velocity (as early after calculation as possible) + // + // + // Maybe we should attenuate the changes + // + fMac2->SendSDO(0x3006, 1, (LWORD_t)vt.Zd()); // SetRpmVelocity [re/min] + fMac1->SendSDO(0x3006, 1, (LWORD_t)vt.Az()); // SetRpmVelocity [re/min] + WaitForSdos(); + if (StopWaitingForSDO()) + { + lout << "Error 3006 happened" << endl; + SkipPendingSdos(); + break; + } + + // + // Now do 'unnecessary' things + // calculate control deviation - for the moment for convinience + // + if (fMac1->GetTime()-s > 1) + { + ZdAz dest0=CorrectTarget(GetSePos(), + RaDec2ZdAz(fMac2->GetMjd(), dst, pm)); + dest0 *= kGearRatio; + dest0 -= GetRePos()+offset; + dest0.Round(); + cout << "Control deviation: "; + cout << setw(4) << (int)fOffset.Zd() << " "; + cout << setw(4) << (int)fOffset.Az() << " re "; + + cout << setw(4) << dest0.Zd() << " "; + cout << setw(4) << dest0.Az() << " re V: "; + cout << setw(4) << vt.Zd() << " "; + cout << setw(4) << vt.Az() << " re/min/4" << endl; + s = (int)fMac1->GetTime(); + } + + // + // Update speed as often as possible. + // make sure, that dt is around 10 times larger than the + // update time + // + // usleep(50000); // 0.05s + } + + fTracking = kFALSE; + + // + // Stop revolution mode + // + fMac2->SetRpmMode(FALSE); + fMac1->SetRpmMode(FALSE); + + cout << "Tracking stopped." << endl; +} + +ZdAz MCosy::RaDec2ZdAz(const double mjd, const RaDec &dst, const RaDec &pm) +{ + int status; + + // + // calculate observers location (goe) + // + double fPhi, fElong; + slaDaf2r(51, 38, 48.0, &fPhi, &status); + slaDaf2r( 9, 56, 36.0, &fElong, &status); + + // cout << "fPhi: 51ø38'48.0\" = " << 360.0/D2PI*fPhi << endl; + // cout << "fElong: 9ø56'36.0\" = " << 360.0/D2PI*fElong << endl; + + // + // ----- calculate star independent parameters ---------- + // + double fAmprms[21]; + double fAoprms[14]; + slaMappa(2000.0, mjd, fAmprms); + slaAoppa(mjd, 0, // mjd, UT1-UTC + fElong, fPhi, 148, // g”ttingen long, lat, height + 0, 0, // polar motion x, y-coordinate (radians) + 273.155, 1013.25, 0.5, // temp, pressure, humidity + 0.2, 0.0065, // wavelength, tropo lapse rate + fAoprms); + + // + // ---- Mean to apparent ---- + // + double r=0, d=0; + slaMapqkz(dst.Ra(), dst.Dec(), (double*)fAmprms, &r, &d); + // + // Doesn't work - don't know why + // + // slaMapqk(dst.Ra(), dst.Dec(), pm.Ra(), pm.Dec(), // ra, dec (rad), r, d (rad) + // 0, 0, fAmprms, &r, &d); + // + + // + // -- apparent to observed -- + // + double r1=0; // ra + double d1=0; // dec + double h0=0; // ha + + double az, zd; + slaAopqk (r, d, fAoprms, + &az, // observed azimuth (radians: N=0,E=90) + &zd, // observed zenith distance (radians) + &h0, // observed hour angle (radians) + &d1, // observed declination (radians) + &r1); // observed right ascension (radians) + + return ZdAz(zd, az); +} + +void *MCosy::Proc(int msg, void *mp) +{ + switch (msg) + { + case WM_STOP: + cout << "Stopping positioning." << endl; + fMac1->SetDeceleration(0.5*fMac1->GetVelRes()); + fMac2->SetDeceleration(0.5*fMac2->GetVelRes()); + cout << "Stoping" << endl; + fMac1->SetRpmMode(FALSE); + fMac2->SetRpmMode(FALSE); + cout << "Done." << endl; + while (fMac1->IsPositioning() || fMac2->IsPositioning()) + usleep(1); + cout << "Positioning stopped." << endl; + return NULL; + + case WM_PRESET: + cout << "WM_PRESET: START" << endl; + fAlt1->SetPreset(); + fAlt2->SetPreset(); + fAz->SetPreset(); + cout << "WM_PRESET: DONE (return 0xaffe)" << endl; + return (void*)0xaffe; + + case WM_POLARIS: + { + cout << "WM_POLARIS: START" << endl; + Timer t; + t.GetTime(); + + RaDec rd(37.94, 89.2644); + ZdAz za=MCosy::RaDec2ZdAz(t.GetMjd(), rd*D2PI/360.0)*16384.0/D2PI; + + cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl; + + ZdAz sepos = GetSePos(); + cout << "Got Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl; + + fAlt1->SetPreset(za.Zd()); + fAlt2->SetPreset(-za.Zd()); + fAz->SetPreset(za.Az()); + + cout << "WM_PRESET: DONE (return 0xaffe)" << endl; + } + return (void*)0xaffe; + + case WM_POSITION: + cout << "WM_POSITION: START" << endl; + { + ZdAz dest = *((ZdAz*)mp); + SetPosition(dest*D2PI/360.0); + } + cout << "WM_POSITION: DONE (return 0x7777)" << endl; + return (void*)0x7777; + + case WM_TRACK: + cout << "WM_TRACK: START" << endl; + { + RaDec dest = *((RaDec*)mp); + TrackPosition(dest*D2PI/360.0); + } + cout << "WM_TRACK: DONE (return 0x8888)" << endl; + return (void*)0x8888; + } + cout << "Unknown Msg" << endl; + return (void*)0xffffffff; +} + +void MCosy::TalkThread() +{ + // + // Start the Network + // + Network::Start(); + PostMsg(WM_PRESET, 0, 0); +/* + cout << "PostMsg(WM_PRESET)" << endl; + void *rc = + cout << hex << "WM_PRESET: ret=" << rc << endl; + + RaDec dest = RaDec(45.0, 30.0)*D2PI/360.0; + + cout << "PostMsg(WM_TRACK)" << endl; + cout << sizeof(RaDec) << "==" << sizeof(dest) << endl; + rc=PostMsg(WM_TRACK, &dest, sizeof(dest)); + cout << "DEST killed." << endl; +*/ + // AltAz dest = AltAz(45.0, 30.0); + // double ra, dec; + // slaDaf2r( 71, 0, 0, &ra, &status); // 0 WARNING: RANGE + // slaDaf2r( 89, 0, 0, &dec, &status); // 49 + // cout << "Start tracking: Ra: " << Rad2Deg(ra) << kDEG << " Dec: " << Rad2Deg(dec) << kDEG << endl; + + // dest = AltAz(-46.0, 210); + // SetPosition(dest); + setpriority(PRIO_PROCESS, 0, 10); + + while (1) + { + // + // wait until a tracking session is started + // + while (!fTracking) + usleep(1); + + RaDec pm; + ZdAz sollalt; // [se] + ZdAz sollaz; // [se] + ZdAz old; + + ZdAz ist=fOffset/kGearRatio; // [se] + // + // only update fOffset while tracking + // + while (fTracking) + { + usleep(100000/*00*/); // 0.1s + + // + // Make changes (eg wind) smoother - attenuation of control function + // + + ZdAz offset(fOffset.Zd()*9.0/10.0+((ist.Zd()-sollalt.Zd())*kGearRatio.X())/10.0, + fOffset.Az()*9.0/10.0+((ist.Az()-sollaz.Az()) *kGearRatio.Y())/10.0); + + fOffset = offset; + // fOffset.Zd(((offset.Zd()>1000)||(offset.Zd()<-1000))?0:offset.Zd()); + // fOffset.Az(((offset.Az()>1000)||(offset.Az()<-1000))?0:offset.Az()); + + // + // get position, where we are + // + ist = GetSePos(); // [se] + + // + // if the position didn't change continue + // + if ((int)ist.Zd() == (int)old.Zd() && + (int)ist.Az() == (int)old.Az()) + continue; + + // + // if Alt Shaftencoder changed position + // + if ((int)ist.Zd() != (int)old.Zd()) + { + // + // Get time from last shaftencoder position change + // + const double t = (fAlt1->GetMjd()+fAlt2->GetMjd())/2.0; + + // + // calculate were we should be + // + sollalt = CorrectTarget(ist, RaDec2ZdAz(t, fRaDec, pm)); + + old.Zd(ist.Zd()); + } + + // + // if Alt Shaftencoder changed position + // + if ((int)ist.Az() != (int)old.Az()) + { + // + // Get time from last shaftencoder position change + // + const double t = fAz->GetMjd(); + + // + // calculate were we should be + // + sollaz = CorrectTarget(ist, RaDec2ZdAz(t, fRaDec, pm)); + + old.Az(ist.Az()); + } + } + } +} + +void *MCosy::MapTalkThread(void *arg) +{ + pthread_detach(pthread_self()); + + MCosy *cosy = (MCosy*)arg; + + cosy->TalkThread(); + + cosy->lout << "- Sending Thread done." << endl; + + return NULL; +} + +int MCosy::StopWaitingForSDO() +{ + return Break() || fMac1->HasError() || fMac2->HasError(); +} + +void MCosy::Start() +{ + if (fTxThrd) + { + cout << "Error: tx thread already started." << endl; + return; + } + + lout << "- Starting sending Thread." << endl; + + fTxThrd = new pthread_t; + pthread_create(fTxThrd, NULL, MapTalkThread, this); +} + +void MCosy::Stop() +{ + if (!fTxThrd) + return; + + pthread_cancel(*fTxThrd); + + delete fTxThrd; + fTxThrd = NULL; + + lout << "- Sending Thread stopped." << endl; + + SkipPendingSdos(); + + Network::Stop(); +} + +MCosy::MCosy(const char *dev, const int baud, ostream &out) +: Network(dev, baud, out), fTxThrd(NULL), fTracking(kFALSE) +{ + // + // Create Nodes + // + fMac1=new Macs(1, lout); + fMac2=new Macs(2, lout); + fAlt1=new ShaftEncoder(4, lout); + fAlt2=new ShaftEncoder(5, lout); + fAz =new ShaftEncoder(6, lout); + + // + // Connect the devices to the network + // + SetNode(fMac1); + SetNode(fMac2); + SetNode(fAlt1); + SetNode(fAlt2); + SetNode(fAz); + + MGCosy *fWin=new MGCosy(this, gClient->GetRoot(), 1, 1); + + fAz->SetDisplay(fWin->GetLabel1()); + fAlt1->SetDisplay(fWin->GetLabel2()); + fAlt2->SetDisplay(fWin->GetLabel3()); +} + +void MCosy::TerminateApp() +{ + gSystem->ExitLoop(); +} + +MCosy::~MCosy() +{ + delete fAz; + delete fAlt2; + delete fAlt1; + delete fMac1; + delete fMac2; + + delete fWin; +} Index: trunk/MagicSoft/Cosy/MCosy.h =================================================================== --- trunk/MagicSoft/Cosy/MCosy.h (revision 738) +++ trunk/MagicSoft/Cosy/MCosy.h (revision 738) @@ -0,0 +1,65 @@ +#ifndef MCOSY_H +#define MCOSY_H + +#include "coord.h" +#include "msgqueue.h" +#include "network.h" + +#define kDEG ((char)0xb0) // Linux 'ø' + +#define WM_PRESET 0x1000 +#define WM_POSITION 0x1001 +#define WM_TRACK 0x1002 +#define WM_STOP 0x1003 +#define WM_POLARIS 0x1004 + +class ShaftEncoder; +class Macs; +class MGCosy; + +class MCosy : public Network, public MsgQueue +{ +private: + ShaftEncoder *fAlt1; + ShaftEncoder *fAlt2; + ShaftEncoder *fAz; + + Macs *fMac1; + Macs *fMac2; + + MGCosy *fWin; + + pthread_t *fTxThrd; + + ZdAz fOffset; // Tracking Offset between SE and calc-pos [re] + RaDec fRaDec; // Position to track + int fTracking; // Flag for present tracking action + + static void *MapTalkThread(void *arg); + + ZdAz GetSePos(); // [se] + ZdAz GetRePos(); + + void TalkThread(); + int SetPosition(const ZdAz &dst); + + void TerminateApp(); + + void TrackPosition(const RaDec &dst); // ra, dec [rad] + + int StopWaitingForSDO(); + +public: + MCosy(const char *dev, const int baud, ostream &out=cout); + ~MCosy(); + + void Start(); + void Stop(); + + void *Proc(int msg, void *mp); + + static ZdAz CorrectTarget(const ZdAz &src, const ZdAz &dst); + static ZdAz RaDec2ZdAz(const double mjd, const RaDec &pos, const RaDec &pm=RaDec(0,0)); +}; + +#endif Index: trunk/MagicSoft/Cosy/MStarguider.cc =================================================================== --- trunk/MagicSoft/Cosy/MStarguider.cc (revision 738) +++ trunk/MagicSoft/Cosy/MStarguider.cc (revision 738) @@ -0,0 +1,95 @@ +#include "MStarguider.h" +/* +#include +#include + +#include +#include +#include +#include + +#include "timer.h" +#include "Writer.h" +#include "grab-v4l.h" +#include "StarDisplay.h" +#include "CameraDisplay.h" +*/ +/* ---------------------------------------------------------------------- */ +/* +void MStarguider::ReadoutLoop() +{ + //disp->Update(); + + //--- WriterPng png(768, 576); + //--- Writer task1(&png, 25*5/1); // one png per five seconds + + // --- + fCcd = new Camera; + CameraDisplay *task=new CameraDisplay(gClient->GetRoot()); + + fCcd->SetIntegrationNum(1); +// fCcd->Loop(task, 25*60*15); // 15min + + // + // Stop Application Loop + // + + // + // FIXME! stupid workaround for tasks without a gui! + // + //--- + // TCanvas *c = new TCanvas("Dummy", "Dummy"); + +// gStopLoop= 1; +// while (fCcd->IsRunning()) +// sleep(1); + + delete fCcd; + //delete task; + + gSystem->ExitLoop(); + gSystem->DispatchOneEvent(kTRUE); + // ReturnFromRun(); // SetReturnFromRun(); + + + cout << "Event Loop Done." << endl; +} + +void *MStarguider::MapReadoutLoop(void *arg) +{ + MStarguider *starg = (MStarguider*)arg; + + starg->ReadoutLoop(); + + return NULL; +} + +void MStarguider::StartReadoutLoop() +{ + pthread_t thread; + if (pthread_create(&thread, NULL, MapReadoutLoop, this)) + { + cout << "Error starting main thread." << endl; + exit(0); + } + pthread_detach(thread); +} + +MStarguider::MStarguider(int *argc, char **argv, void *options=0, + int numOptions=0) +: TApplication("Starguider", argc, argv, options, numOptions), fCcd(NULL) +{ + + signal(SIGINT, SignalRcvd); + + //--- + + StartReadoutLoop(); +} + +MStarguider::~MStarguider() +{ + //--- +} + +*/ Index: trunk/MagicSoft/Cosy/MStarguider.h =================================================================== --- trunk/MagicSoft/Cosy/MStarguider.h (revision 738) +++ trunk/MagicSoft/Cosy/MStarguider.h (revision 738) @@ -0,0 +1,24 @@ +#ifndef MSTARGUIDER_H +#define MSTARGUIDER_H + +#include "Camera.h" +#include "Starguider.h" + +class MStarguider : public Camera +{ +private: + Starguider *fDisplay; + +public: + MStarguider() + { + fDisplay = new Starguider; + } + + ~MStarguider() + { + delete fDisplay; + } +}; + +#endif Index: trunk/MagicSoft/Cosy/Makefile =================================================================== --- trunk/MagicSoft/Cosy/Makefile (revision 738) +++ trunk/MagicSoft/Cosy/Makefile (revision 738) @@ -0,0 +1,134 @@ +################################################################## +# +# makefile +# +# for the MARS software +# +################################################################## +# @maintitle + +# @code + +# +# ----->>> settings for compiler +# + +CC = gcc +CXX = g++ +AR = ar -rc +AS = as +LD = ld +RANLIB = ranlib + +# +# ----->>> settings for compilation +# + +PROGRAMS = cosy starg + +ROOTFLAGS = `root-config --cflags` +ROOTGLIBS = `root-config --glibs` + +# +# connect the include files defined in the config.mk file +# +# WARNING: the result (whether the linkage works or not) depends on the +# order of the libraries +# +# +#LIBS = -L. -L/usr/lib -lstdc++ -lpthread +INCLUDES = -I. -I/usr/X11R6/include -Icandrv -Idevdrv -Ibase -Igui -Iincl -Islalib -Ivideodev -Icatalog +LIBS = -L. -L/usr/X11R6/lib -lpthread -lpng -lz +#INCLUDES = -I. -Ibase -Igui -Islalib + +WARNING = -Wall -fno-rtti -fno-exceptions +OPTIM = -O2 +DEBUG = -g +DEFINES = -DCPU=486 -DBUS_LITTLE_ENDIAN -DDEBUG=0 -DLINUX -DSHOW + +CFLAGS = $(OPTIM) $(DEBUG) $(WARNING) $(INCLUDES) $(ROOTFLAGS) $(DEFINES) +CXXFLAGS = $(CFLAGS) + +CINT = M + +# +# ----->>> mars libraries +# + +#------------------------------------------------------------------------------ +#.SILENT: + +.SUFFIXES: .c .cc .h .o + +SLASRC = slalib/cldj.c slalib/dranrm.c slalib/mappa.c slalib/aoppa.c \ + slalib/mapqk.c slalib/aopqk.c slalib/epj.c slalib/evp.c \ + slalib/dvn.c slalib/prenut.c slalib/geoc.c slalib/refco.c \ + slalib/eqeqx.c slalib/dcs2c.c slalib/dvdv.c slalib/dmxv.c \ + slalib/dcc2s.c slalib/refz.c slalib/refro.c slalib/prec.c \ + slalib/nut.c slalib/dmxm.c slalib/nutc.c slalib/range.c \ + slalib/aoppat.c slalib/deuler.c slalib/drange.c slalib/daf2r.c \ + slalib/dh2e.c slalib/gmst.c slalib/de2h.c slalib/altaz.c \ + slalib/gmsta.c slalib/dat.c slalib/dtt.c slalib/oapqk.c \ + slalib/ampqk.c slalib/dimxv.c slalib/mapqkz.c slalib/dr2af.c \ + slalib/dd2tf.c + +SLAOBJ = $(SLASRC:.c=.o) + +SRCFILES = candrv/vmodican.cc \ + candrv/canopen.cc \ + candrv/network.cc \ + candrv/nodedrv.cc \ + candrv/sdolist.cc \ + devdrv/shaftencoder.cc \ + devdrv/macs.cc \ + gui/MGCosy.cc \ + gui/MGImage.cc \ + gui/MGCoordinate.cc \ + gui/MGCoordinates.cc \ + base/timer.cc \ + base/msgqueue.cc \ + base/File.cc \ + base/MStopwatch.cc \ + catalog/SaoFile.cc \ + catalog/StarCatalog.cc \ + videodev/Camera.cc \ + videodev/Filter.cc \ + videodev/Writer.cc \ + MStarguider.cc \ + MCosy.cc \ + Starguider.cc + +SRCS = $(SRCFILES) +HEADERS = $(SRCFILES:.cc=.h) +OBJS = $(SRCFILES:.cc=.o) + +############################################################ + +all: $(PROGRAMS) + +$(SLAOBJ): + echo 'Compiling $*.c' + $(CC) -c $(CFLAGS) -pedantic $*.c -o $*.o + +#$(PROGRAMS): $(PROGRAMS:=.o) $(OBJS) $(SLAOBJ) +# @echo " Linking $@ ..." +# $(CXX) $(CXXFLAGS) $@.o $(LIBS) $(ROOTGLIBS) -o $@ +$(PROGRAMS): $(SLAOBJ) $(OBJS) $(PROGRAMS:=.o) + @echo " Linking $@ ..." + $(CXX) $(CXXFLAGS) $(LIBS) $(SLAOBJ) $(OBJS) $(ROOTGLIBS) -o $@ $@.o + +#dox: +# @echo " Creating html documentation:" +# root -b -q dohtml.C + + +include Makefile.rules + +clean: rmcint rmobjs rmcore rmbin rmbak + +tar: clean + @echo "Making tar-file" + @tar -cvf ../cosy.tar * + @gzip -9 ../cosy.tar + +# @endcode Index: trunk/MagicSoft/Cosy/Makefile.conf.linux-gnu =================================================================== --- trunk/MagicSoft/Cosy/Makefile.conf.linux-gnu (revision 738) +++ trunk/MagicSoft/Cosy/Makefile.conf.linux-gnu (revision 738) @@ -0,0 +1,30 @@ +################################################################## +# +# config.mk +# +# @file config.mk +# +################################################################## +# @maintitle + +# compilers + +CC = gcc +CXX = g++ +F77 = f77 +AR = ar -rc +# +# ----->>> settings for compilation +# + +OPTIM = -O2 -Wall -fno-rtti -fno-exceptions -fPIC +DEBUG = -g + +# uncomment this for quiet compilation + +#.SILENT: + +# @endcode +##EOF + + Index: trunk/MagicSoft/Cosy/Makefile.mk =================================================================== --- trunk/MagicSoft/Cosy/Makefile.mk (revision 738) +++ trunk/MagicSoft/Cosy/Makefile.mk (revision 738) @@ -0,0 +1,109 @@ +################################################################## +# +# makefile +# +# for the MARS software +# +################################################################## +# @maintitle + +# @code + +# +# ----->>> settings for compiler +# + +CC = gcc +CXX = g++ +AR = ar -rc +AS = as +LD = ld +RANLIB = ranlib + +# +# ----->>> settings for compilation +# + +PROGRAMS = cosy + + +ROOTFLAGS = `root-config --cflags` +ROOTGLIBS = `root-config --glibs` + +# +# connect the include files defined in the config.mk file +# +# WARNING: the result (whether the linkage works or not) depends on the +# order of the libraries +# +# +LIBS = -L/usr/lib -lstdc++ -lpthread +INCLUDES = -Iincl -I. + +WARNING = -Wall -fno-rtti -fno-exceptions +OPTIM = -O2 +DEBUG = -g +DEFINES = -DCPU=486 -DBUS_LITTLE_ENDIAN -DDEBUG=0 -DLINUX -DSHOW + +CFLAGS = $(OPTIM) $(DEBUG) $(WARNING) $(INCLUDES) $(ROOTFLAGS) $(DEFINES) +CXXFLAGS = $(CFLAGS) + +# +# uncomment this for quiet compilation +# + + +include Makefile.conf.general + +# @endcode + +# @code + +CINT = M + +# +# ----->>> mars libraries +# + +#------------------------------------------------------------------------------ +.SILENT: + +.SUFFIXES: .c .cc .h .o + +SRCFILES = + +SRCS = $(SRCFILES) +HEADERS = $(SRCFILES:.cc=.h) +OBJS = $(SRCFILES:.cc=.o) + +############################################################ + +all: rmlib $(PROGRAMS) + +#dox: +# @echo " Creating html documentation:" +# root -b -q dohtml.C + + +include Makefile.rules + +clean: rmlib rmcint rmobjs rmcore rmbin + +mrproper: clean rmbak + @rm -f macros/*~ + @rm -rf htmldoc + @echo "cd mbase" + @cd mbase; make mrproper; cd .. + @echo "cd .." + @echo "cd mraw" + @cd mraw; make mrproper; cd .. + @echo "cd .." + @echo "cd mgui" + @cd mgui; make mrproper; cd .. + @echo "cd .." + @echo "cd mdatacheck" + @cd mdatacheck; make mrproper; cd .. + @echo "cd .." + + +# @endcode Index: trunk/MagicSoft/Cosy/Makefile.rules =================================================================== --- trunk/MagicSoft/Cosy/Makefile.rules (revision 738) +++ trunk/MagicSoft/Cosy/Makefile.rules (revision 738) @@ -0,0 +1,65 @@ + +depend: + @makedepend $(SRCS) $(INCLUDES) -I$(INCLUDE_CPLUS) $(ROOTCFLAGS) \ + -f Makefile.depend 2> kk.kk ; cat kk.kk + +$(LIB): $(OBJS) $(CINT)Cint.o + @echo " - Building Library $(LIB) ... " + $(AR) $(LIB) *.o + +$(CINT)Cint.cc: $(HEADERS) + @echo + @echo " - Generating dictionary $(CINT)Cint.cc ..." + + $(ROOTSYS)/bin/rootcint -f $(CINT)Cint.cc \ + -c $(INCLUDES) $(HEADERS) $(CINT)Incl.h $(CINT)LinkDef.h + +.cxx.o: + @echo "Compiling " $< + $(CXX) $(CXXFLAGS) -c $< -o $@ + +.cc.o: + @echo "Compiling " $< + $(CXX) $(CXXFLAGS) -c $< -o $@ + +.c.o: + @echo "Compiling " $< + $(CC) $(CFLAGS) -c $< -o $@ +# +# The cleaning facility +# + +rmcint: + @echo "Removing cint-stuff..." + @rm -rf *Cint.* + +rmobjs: + @echo "Removing object files..." + @rm -f *.o + @rm -f base/*.o + @rm -f gui/*.o + @rm -f candrv/*.o + @rm -f devdrv/*.o + @rm -f slalib/*.o + +rmcore: + @echo "Removing core files..." + @rm -rf core* + +rmbin: + @echo "Removing binary files..." + @rm -rf $(PROGRAMS) so_locations + +rmbak: + @echo "Removing backup files..." + @rm -f *~ kk.kk *.bak + @rm -f base/*~ base/kk.kk base/*.bak + @rm -f gui/*~ gui/kk.kk gui/*.bak + @rm -f candrv/*~ candrv/kk.kk candrv/*.bak + @rm -f devdrv/*~ devdrv/kk.kk devdrv/*.bak + @rm -f slalib/*~ slalib/kk.kk slalib/*.bak + +cflags: + @echo $(INCLUDES) $(CXXFLAGS) + + Index: trunk/MagicSoft/Cosy/Starguider.cc =================================================================== --- trunk/MagicSoft/Cosy/Starguider.cc (revision 738) +++ trunk/MagicSoft/Cosy/Starguider.cc (revision 738) @@ -0,0 +1,305 @@ +#include "Starguider.h" + +#include // cout + +#include +#include +#include // TGHorizontal3DLine + +#include "MGImage.h" +#include "MGCoordinates.h" + +#include "Filter.h" +#include "Writer.h" +#include "timer.h" + +enum { + IDM_kFilter, + IDM_kCatalog, + IDM_kStart, + IDM_kStop, + IDM_kFileType, + IDM_kPPM, + IDM_kPNG, + IDM_kOnce, + IDM_kContinous, + IDM_kRate25ps, + IDM_kRate5ps, + IDM_kRate1s, + IDM_kRate5s, + IDM_kRate30s, + IDM_kRate1m, + IDM_kRate5m +}; + +Starguider::Starguider() +: Camera(), TGMainFrame(gClient->GetRoot(), 768, 700), fRaDec(180, 40) +{ + // p = pointer to MainFrame (not owner) + + const TGWindow *p=gClient->GetRoot(); + + // + // Create Menu for Starguider Display + // + fDisplay = new TGPopupMenu(p); + fDisplay->AddEntry("&Filter", IDM_kFilter); + fDisplay->AddEntry("Sao &Catalog", IDM_kCatalog); + fDisplay->Associate(this); + fList.Add(fDisplay); + + fFileType = new TGPopupMenu(p); + fFileType->AddEntry("PP&M", IDM_kPPM); + fFileType->AddEntry("&PNG", IDM_kPNG); + fFileType->CheckEntry(IDM_kPNG); + fFileType->Associate(this); + fList.Add(fFileType); + + fWriteType = new TGPopupMenu(p); + fWriteType->AddEntry("Once", IDM_kOnce); + fWriteType->AddEntry("Continous", IDM_kContinous); + fWriteType->CheckEntry(IDM_kOnce); + fWriteType->Associate(this); + fList.Add(fWriteType); + + fWriteRate = new TGPopupMenu(p); + fWriteRate->AddEntry("25/s", IDM_kRate25ps); + fWriteRate->AddEntry("5/s", IDM_kRate5ps); + fWriteRate->AddEntry("1s", IDM_kRate1s); + fWriteRate->AddEntry("5s", IDM_kRate5s); + fWriteRate->AddEntry("30s", IDM_kRate30s); + fWriteRate->AddEntry("1min", IDM_kRate1m); + fWriteRate->AddEntry("5min", IDM_kRate5m); + fWriteRate->CheckEntry(IDM_kRate1m); + fWriteRate->Associate(this); + fList.Add(fWriteRate); + + fWrtRate = 25*60; + + fWrite = new TGPopupMenu(p); + fWrite->AddEntry("&Start", IDM_kStart); + fWrite->AddEntry("Sto&p", IDM_kStop); + fWrite->AddSeparator(); + fWrite->AddPopup("File &Type", fFileType); + fWrite->AddPopup("&Write Type", fWriteType); + fWrite->AddPopup("Write &Rate", fWriteRate); + fWrite->DisableEntry(IDM_kStop); + fWrite->Associate(this); + fList.Add(fWrite); + + fMenu = new TGMenuBar(this, 0, 0, kHorizontalFrame); + fMenu->AddPopup("&Display", fDisplay, NULL); + fMenu->AddPopup("&Write", fWrite, NULL); + fMenu->Resize(fMenu->GetDefaultSize()); + AddFrame(fMenu); //, new TGLayoutHints (kLHintsNormal, 0, 4, 0, 0)); + fList.Add(fMenu); + + fCRaDec = new MGCoordinates(this, kTRUE, + "Right Ascension [\xb0]:", "Declination [\xb0]:"); + fCRaDec->Move(1, fMenu->GetDefaultHeight()+584); + AddFrame(fCRaDec); + fList.Add(fCRaDec); + + fCZdAz = new MGCoordinates(this, kFALSE, + "Zenith Dist. [\xb0]:", "Azimuth [\xb0]:"); + fCZdAz->Move(240+12, fMenu->GetDefaultHeight()+584); + AddFrame(fCZdAz); + fList.Add(fCZdAz); + + // TGHorizontal3DLine *fLineSep = new TGHorizontal3DLine(this); + // AddFrame(fLineSep, new TGLayoutHints (kLHintsNormal | kLHintsExpandX)); + // fList.Add(fLineSep); + + // + // Create Image Display + // + fImage = new MGImage(this, 768, 576); + fImage->Move(0, fMenu->GetDefaultHeight()); + AddFrame(fImage); + fList.Add(fImage); + + // + // Make everything visible + // + SetWindowName("Starguider Main Window"); + SetIconName("Starguider"); + + MapSubwindows(); + MapWindow(); + + + fSao.SetLimitMag(9.0); + fSao.SetPixSize(0.006); +} + +Starguider::~Starguider() +{ + cout << "Camera Display destroyed." << endl; +} + +void Starguider::Layout() +{ + // Resize(GetDefaultSize()); +} + +void Starguider::CloseWindow() +{ + cout << "EventDisplay::CloseWindow: Exit Application Loop." << endl; + + ExitLoop(); + + gSystem->ExitLoop(); +} + +Bool_t Starguider::ProcessMessage(Long_t msg, Long_t mp1, Long_t mp2) +{ + switch (GET_MSG(msg)) + { + case kC_COMMAND: + switch (GET_SUBMSG(msg)) + { + case kCM_MENU: + switch (mp1) + { + case IDM_kCatalog: + if (fDisplay->IsEntryChecked(IDM_kCatalog)) + fDisplay->UnCheckEntry(IDM_kCatalog); + else + fDisplay->CheckEntry(IDM_kCatalog); + return kTRUE; + + case IDM_kFilter: + if (fDisplay->IsEntryChecked(IDM_kFilter)) + fDisplay->UnCheckEntry(IDM_kFilter); + else + fDisplay->CheckEntry(IDM_kFilter); + return kTRUE; + + case IDM_kStart: + fWrite->DisableEntry(IDM_kStart); + fWrite->EnableEntry(IDM_kStop); + return kTRUE; + + case IDM_kStop: + fWrite->DisableEntry(IDM_kStop); + fWrite->EnableEntry(IDM_kStart); + return kTRUE; + + case IDM_kPNG: + fFileType->CheckEntry(IDM_kPNG); + fFileType->UnCheckEntry(IDM_kPPM); + return kTRUE; + + case IDM_kPPM: + fFileType->CheckEntry(IDM_kPPM); + fFileType->UnCheckEntry(IDM_kPNG); + return kTRUE; + + case IDM_kOnce: + fWriteType->CheckEntry(IDM_kOnce); + fWriteType->UnCheckEntry(IDM_kContinous); + return kTRUE; + + case IDM_kContinous: + fWriteType->CheckEntry(IDM_kContinous); + fWriteType->UnCheckEntry(IDM_kOnce); + return kTRUE; + + case IDM_kRate25ps: + case IDM_kRate5ps: + case IDM_kRate1s: + case IDM_kRate5s: + case IDM_kRate30s: + case IDM_kRate1m: + case IDM_kRate5m: + for (int i=IDM_kRate25ps; i<=IDM_kRate5m; i++) + if (mp1==i) + fWriteRate->CheckEntry(i); + else + fWriteRate->UnCheckEntry(i); + switch (mp1) + { + case IDM_kRate25ps: + fWrtRate = 1; + return kTRUE; + case IDM_kRate5ps: + fWrtRate = 5; + return kTRUE; + case IDM_kRate1s: + fWrtRate = 25; + return kTRUE; + case IDM_kRate5s: + fWrtRate = 5*25; + return kTRUE; + case IDM_kRate30s: + fWrtRate = 30*25; + return kTRUE; + case IDM_kRate1m: + fWrtRate = 60*25; + return kTRUE; + case IDM_kRate5m: + fWrtRate = 5*60*25; + return kTRUE; + } + return kTRUE; + } + break; + } + break; + } + return kTRUE; +} + +void Starguider::Execute(const unsigned long n, char *img, struct timeval *tm) +{ + + if (!fWrite->IsEntryEnabled(IDM_kStart) && + (!(n%fWrtRate) || fWriteType->IsEntryChecked(IDM_kOnce))) + { + if (fFileType->IsEntryChecked(IDM_kPNG)) + { + static int num = 0; + + char name[80]; + sprintf(name, "pix/file%04d.png", num); + Writer::Png(name, img, tm); + } + + if (fFileType->IsEntryChecked(IDM_kPPM)) + { + static int num = 0; + char name[80]; + sprintf(name, "pix/file%04d.ppm", num); + Writer::Ppm(name, img); + } + + if (fWriteType->IsEntryChecked(IDM_kOnce)) + ProcessMessage(MK_MSG(kC_COMMAND, kCM_MENU), IDM_kStop, 0); + } + + if (!(n%25)) + { + cout << "Img: " << n << endl; + + if (fDisplay->IsEntryChecked(IDM_kFilter)) + Filter::Execute(img); + + if (fDisplay->IsEntryChecked(IDM_kCatalog)) + { + char cimg[768*576]; + + XY xy = fCRaDec->GetCoordinates(); + + fRaDec.Set(xy.X(), xy.Y()); + + Timer time(tm); + fSao.GetImg(img, cimg, time.GetMjd(), fRaDec); + fImage->DrawColImg(img, cimg); + + fCZdAz->SetCoordinates(fSao.GetZdAz()); + } + else + fImage->DrawImg(img); + } + +} Index: trunk/MagicSoft/Cosy/Starguider.h =================================================================== --- trunk/MagicSoft/Cosy/Starguider.h (revision 738) +++ trunk/MagicSoft/Cosy/Starguider.h (revision 738) @@ -0,0 +1,63 @@ +#ifndef STARGUIDER_H +#define STARGUIDER_H + +#ifndef ROOT_TGFrame +#include +#endif +#ifndef CAMERA_H +#include "Camera.h" +#endif + +#include "MGList.h" +#include "MGImage.h" +#include "StarCatalog.h" + +class TGMenuBar; +class TGPopupMenu; + +class MGImage; +class MGCoordinates; + +class Starguider : public Camera, public TGMainFrame +{ +private: + MGList fList; + + TGMenuBar *fMenu; + MGImage *fImage; + + TGPopupMenu *fDisplay; + TGPopupMenu *fWrite; + TGPopupMenu *fFileType; + TGPopupMenu *fWriteType; + TGPopupMenu *fWriteRate; + + MGCoordinates *fCRaDec; + MGCoordinates *fCZdAz; + + StarCatalog fSao; + + RaDec fRaDec; + + int fWrtRate; + + void SetPixSize(const double pixsize); + +public: + Starguider(); + virtual ~Starguider(); + + void Update(); + + void Layout(); + void CloseWindow(); + + Bool_t ProcessMessage(Long_t msg, Long_t parm1, Long_t parm2); + + // + // Execution of one frame - this function may be overloaded! + // + void Execute(const unsigned long n, char *img, struct timeval *tm); +}; + +#endif Index: trunk/MagicSoft/Cosy/aposs/Magic.m =================================================================== --- trunk/MagicSoft/Cosy/aposs/Magic.m (revision 738) +++ trunk/MagicSoft/Cosy/aposs/Magic.m (revision 738) @@ -0,0 +1,589 @@ +/*-------------------------------------------------------------------------*/ +/* DIM section for Array definition (arrays are globals) */ +/*-------------------------------------------------------------------------*/ + +DIM errlist[9] + +/* ----------------------------------------------------------------------- */ +/* */ +/* Version: */ +/* */ +kVERSION = 0 /* */ +kSUBVERSION = 50 /* */ +/* */ +/* HISTORY: */ +/* */ +/* * V0.50: */ +/* - Rearanged many object numbers */ +/* - Added object dictonary to comments */ +/* */ +/* * V0.43: */ +/* - Added Object 0x1003, 0x1004, 0x1005 */ +/* */ +/* * V0.42: */ +/* - Added APOS to PDO1 */ +/* */ +/* * V0.41: */ +/* - Period Interrupt diabled while HOME */ +/* */ +/* * V0.40: */ +/* - Introduced object 0x1010, 0x1011 */ +/* */ +/* * V0.38: */ +/* - Introduced object 0x100a */ +/* */ +/* * V0.37: */ +/* - ON ERROR GOSUB moved after new init section */ +/* */ +/* * V0.36: */ +/* - Enahnced Initialization (NOWAIT OFF, etc.) */ +/* */ +/* * V0.35: */ +/* - SDO 0x4003/EXIT introduced */ +/* */ +/* * V0.34: */ +/* - PDO1 as answer to a SDO added (maybe SDO changes state) */ +/* */ +/* ----------------------------------------------------------------------- */ + +PRINT "Magic Mics V", kVERSION, ".", kSUBVERSION /* */ + +/* ----------------------------------------------------------------------- */ +/* */ +/* Object Dictionary: */ +/* */ +/* 0x1003 x rw Read delete error list (subidx 0-9) */ +/* 0x1004 0 ro Nr of PDOs (transmit) */ +/* 1 ro Nr of PDOs (synchron) */ +/* 2 ro Nr of PDOs (asynchron) */ +/* 0x1005 x ro COB ID for Syncs */ +/* 0x100a x ro Software Version */ +/* 0x100b x ro Node number */ +/* 0x100e x ro COB ID for Guarding */ +/* 0x1010 x wo Write data to EEPROM */ +/* 0x1014 x ro COB ID for Emergency */ +/* 0x1800 x rw Enable PDO1 (Axe Status, Position) */ +/* 0x2000 0 rw Maximum positioning error */ +/* 1 rw Negative Software Endswitch */ +/* 2 rw Positive Software Endswitch */ +/* 0x2002 x rw Velocity */ +/* 0x2003 0 wo Acceleration */ +/* 1 wo Deceleration */ +/* 0x3000 x wo Motor 'on', 'off', 'stop' */ +/* 0x3001 x wo Home 'home' */ +/* 0x3002 x wo Reopen Communication 'open' */ +/* 0x3003 x wo Exit Program 'exit' */ +/* 0x3006 0 wo Velocity Mode 'strt', 'stop' */ +/* 1 wo VelMode Velocity */ +/* 0x3008 x wo Nowait 'on', 'off' */ +/* 0x6000 x rw Rotation Direction */ +/* 0x6002 x rw Velocity Resolution */ +/* 0x6003 0 wo Define present position as origin */ +/* 1 wo Define new origin (0=delete) */ +/* 2 rw Home Offset */ +/* 0x6004 0 rw Absolute Position */ +/* 1 wo Relative Position */ +/* 1 ro Control Position */ +/* 0x6501 x rw Encoder Resolution */ +/* 0x6502 x rw Maximum Velocity */ +/* 0x6508 x ro Time since switch on */ +/* */ +/* ----------------------------------------------------------------------- */ + + +/*-------------------------------------------------------------------------*/ +/* section for global constants */ +/*-------------------------------------------------------------------------*/ +SET PRGPAR 0 /* Restart Program on Exit */ + +SET ENCODERTYPE 0 /* Incremental Encoder */ +SET ENCODER 500 /* Encoder has 500 Ticks */ +SET VELMAX 3600 /* Motor: Maximum rounds per minute */ +SET POSERR 1500 /* Maximum tolarable Position error (qc) 0.1° */ +SET ENDSWMOD 1 /* At End Switch Stop Motor with Max Decel. */ +SET ERRCOND 2 /* Motor Stop */ +SET POSDRCT 1 /* rotation direction */ +SET POSFACT_Z 1 /* 1 user unit (be) = POSFACT_Z/POSFACT_N qc */ +SET POSFACT_N 1 /* */ +SET HOME_FORCE 1 /* Force Home positioning on startup */ +SET HOME_OFFSET 0 /* Offset between index and home position */ +SET HOMETYPE 0 /* drive to home, reverse, go to next index */ +SET RAMPTYPE 0 /* Ramp Type: 0=Trapez, 1=Sinus */ + +/*----------------*/ +/* Inputs */ +/*----------------*/ +SET I_REFSWITCH -2 /* Reference Switch, Input 2, leading edge */ +SET I_POSLIMITSW -2 /* Pos Limit Switch, Input 2, leading edge */ +SET I_NEGLIMITSW -1 /* Neg Limit Switch, Input 1, leading edge */ +SET I_BREAK 0 /* Input which brakes a running program */ +SET I_CONTINUE 0 /* Input to continue a broken program */ +SET I_ERRCLR 0 /* Input to clear error */ + +/*----------------*/ +/* Outputs */ +/*----------------*/ +SET O_AXMOVE 0 /* Motor control is working */ +SET O_BRAKE 0 /* Brake */ +SET O_ERROR 0 /* error occured */ + +/*----------------*/ +/* Dflt vel & acc */ +/*----------------*/ +vres = (GET ENCODER)*(GET VELMAX) /* ticks/R * R/M = ticks/min */ +SET VELRES vres /* Set velocity units */ +SET HOME_VEL -(25*vres%100) /* Home position velocity: 25% */ +SET HOME_RAMP (25*vres%100) /* Home position accel: 25% */ +SET DFLTACC (10*vres%100) /* Default acceleratio: 10% */ +SET DFLTVEL (10*vres%100) /* Default velocity: 10% */ + +/*----------------*/ +/* Software range */ +/*----------------*/ +SET SWPOSLIMACT 0 /* positive software limit switch inactive */ +SET SWNEGLIMACT 0 /* negative software limit switch inactive */ +SET POSLIMIT 0 /* positive software limit (qc) */ +SET NEGLIMIT 0 /* negative software limit (qc) */ + +/*-------------------------------------------------------------------------*/ +/* const section for constant velues */ +/*-------------------------------------------------------------------------*/ +kTRUE = 1 +kFALSE = 0 + +pdotime = 100 +pdo1on = kFALSE + +/*-------------------------------------------------------------------------*/ +/* Can Open Difinitions */ +/*-------------------------------------------------------------------------*/ +CANDEL -1 +nodenr = GET CANNR +PRINT "Initializing Node Nr.", nodenr +pdo1 = DEFCANOUT (0x180+nodenr) 8 +pdo2 = DEFCANOUT (0x280+nodenr) 8 +sdotx = DEFCANOUT (0x580+nodenr) 8 +sdorx = DEFCANIN (0x600+nodenr) 8 + +err = REOPEN 0 0 + +/*-------------------------------------------------------------------------*/ +/* Init */ +/*-------------------------------------------------------------------------*/ +MOTOR STOP +MOTOR OFF +CVEL 0 +NOWAIT OFF +OUT 1 1 + +/*-------------------------------------------------------------------------*/ +/* ON ... GOSUB ... definitions */ +/*-------------------------------------------------------------------------*/ +/* ON CANMSG GOSUB PROC_CANMSG */ +i = 8 +while (i) do + errlist[i] = 0 + i = i - 1 +endwhile + +ON ERROR GOSUB PROC_ERROR + +/*-------------------------------------------------------------------------*/ +/* Program Main Loop */ +/*-------------------------------------------------------------------------*/ +MAIN: + canhi = 0 + canlo = 1 + + PRINT "Starting Mainloop..." + MAINLOOP: + rc = CANIN sdorx 0 0 canhi canlo + if (rc==0) then /* It must be tested because ON PERIOD breaks 'wait for obj' */ + gosub PROC_SDORX + endif + goto MAINLOOP +ENDMAIN: + MOTOR STOP + MOTOR OFF + OUT 1 0 + EXIT + +/*-------------------------------------------------------------------------*/ +/* Part for Programs called with GOSUB */ +/*-------------------------------------------------------------------------*/ + +SUBMAINPROG + /*----------------------------------*/ + /* PROC_CANOPENMSG */ + /*----------------------------------*/ + SUBPROG PROC_SDOSET + idx = canhi&0xff00 | (canhi>>16)&0xff + subidx = canhi&0xff + sdoval = (canlo&0xff)<<24 | (canlo&0xff00)<<8 | (canlo>>8)&0xff00 | (canlo>>24)&0xff +/* + PRINT "Setting Idx:", idx, "/", subidx, " to ", sdoval +*/ + if (idx == 0x1003 and subidx == 0 and sdoval == 0) then + i = 0 + while (i<9) do + errlist[i] = 0 + i = i + 1 + endwhile + elseif (idx == 0x1010 and sdoval == 's'<<24|'a'<<16|'v'<<8|'e') then + SAVEPROM + elseif (idx == 0x1800 and subidx == 1) then + ON PERIOD 0 GOSUB PROC_PDO1 + if (sdoval>>31) then + pdo1on = kFALSE + else + ON PERIOD pdotime GOSUB PROC_PDO1 + pdo1on = kTRUE + endif + elseif (idx == 0x2000) then + if (subidx == 0) then + SET POSERR sdoval + elseif (subidx == 1) then + if ((sdoval>>30)&1) then + SET NEGLIMIT sdoval & 0x3fffffff + SET SWNEGLIMACT 1 + else + SET SWNEGLIMACT 0 + endif + elseif (subidx == 2) then + if ((sdoval>>31)&1) then + SET POSLIMIT sdoval & 0x3fffffff + SET SWPOSLIMACT 1 + else + SET SWPOSLIMACT 0 + endif + endif + elseif (idx == 0x2002) then + VEL sdoval + elseif (idx == 0x2003) then + if (subidx) then + DEC sdoval + else + ACC sdoval + endif + elseif (idx == 0x3000) then + if (sdoval == 'o'<<24|'n'<<16) then + MOTOR ON + elseif (sdoval == 'o'<<24|'f'<<16|'f'<<8) then + MOTOR OFF + elseif (sdoval == 's'<<24|'t'<<16|'o'<<8|'p') then + MOTOR STOP + endif + elseif (idx == 0x3001) then + if (sdoval == 'h'<<24|'o'<<16|'m'<<8|'e') then + limitsw = GET I_POSLIMITSW + set I_POSLIMITSW 0 + /* Disable Interrupt - same problem than with CANIN */ + ON PERIOD 0 GOSUB PROC_PDO1 + HOME + /* Reenable interrupt */ + if (pdo1on) then + ON PERIOD pdotime GOSUB PROC_PDO1 + endif + SET I_POSLIMITSW limitsw + endif + elseif (idx == 0x3002 and sdoval == 'o'<<24|'p'<<16|'e'<<8|'n') then + sdoval=REOPEN 2 0 + if (sdoval) then + PRINT "Error Reopen" + endif + elseif (idx == 0x3003 and sdoval == 'e'<<24|'x'<<16|'i'<<8|'t') then + CANOUT sdotx (canhi&0xffffff | 0x60000000) 0 + EXIT + elseif (idx == 0x3006) then + if (subidx == 0) then + if (sdoval == 's'<<24|'t'<<16|'r'<<8|'t') then + CVEL 0 + CSTART + elseif (sdoval == 's'<<24|'t'<<16|'o'<<8|'p') then + CSTOP + endif + elseif (subidx == 1) then + CVEL sdoval + endif + elseif (idx == 0x3008) then + if (sdoval == 'o'<<24|'n'<<16) then + NOWAIT ON + elseif (sdoval == 'o'<<24|'f'<<16|'f'<<8) then + NOWAIT OFF + endif +/* elseif (idx == 0x4000 and + (sdoval>>24)&0xff == 'S' and + (sdoval>>16)&0xff == 'T' and + (sdoval>>8) &0xff == 'O' and + (sdoval) &0xff == 'P') then + CANOUT sdotx (canhi&0xffffff | 0x60000000) 0 + goto ENDMAIN +*/ elseif (idx == 0x6000) then + if (sdoval&1) then + SET POSDRCT -1 + else + SET POSDRCT 1 + endif +/* elseif (idx == 0x6001) then + SET ENCODER sdoval*/ + elseif (idx == 0x6002) then + SET VELRES sdoval + elseif (idx == 0x6003) then + if (subidx == 0) then + DEF ORIGIN + elseif (subidx == 1) then + if (sdoval==0) then + RST ORIGIN + else + SET ORIGIN sdoval + endif + elseif (subidx == 2) then + SET HOME_OFFSET sdoval + endif + elseif (idx == 0x6004) then + if (subidx==0) then + POSA sdoval + elseif (subidx==1) then + POSR sdoval + endif + elseif (idx == 0x6200) then + pdotime = sdoval + if (pdo1on) then + ON PERIOD 0 GOSUB PROC_PDO1 + ON PERIOD pdotime GOSUB PROC_PDO1 + endif + elseif (idx == 0x6501) then + SET ENCODER sdoval + elseif (idx == 0x6502) then + SET VELMAX sdoval + else + CANOUT sdotx (canhi&0xffffff | 0x80000000) 0 + goto ENDSDOSET + endif + + if (pdo1on) then + GOSUB PROC_PDO1 + endif + + CANOUT sdotx (canhi&0xffffff | 0x60000000) 0 +/* + PRINT "Sdo Set ", idx, "/", subidx +*/ + ENDSDOSET: + RETURN + + /*----------------------------------*/ + + SUBPROG PROC_SDOREQ + idx = canhi&0xff00 | (canhi>>16)&0xff + subidx = canhi&0xff +/* + PRINT "Requesting Idx:", idx, "/", subidx +*/ + if (idx == 0x1003) then + if (subidx >=0 and subidx<=9) then + sdoval = errlist[subidx] + endif + elseif (idx == 0x1004) then + if (subidx == 0) then + sdoval = 1 + elseif (subidx == 1) then + sdoval = 0 + elseif (subidx == 2) then + sdoval = 1 + endif + elseif (idx == 0x1005) then + sdoval = 1<<31 | 0x80 + elseif (idx == 0x100b) then + sdoval = nodenr + elseif (idx == 0x100a) then + sdoval = (kVERSION<<16) | kSUBVERSION + elseif (idx == 0x100e) then + sdoval = 0x700 | nodenr + elseif (idx == 0x1010) then + sdoval = 1 + elseif (idx == 0x1011) then + sdoval = 0 + elseif (idx == 0x1014) then + sdoval = 0x80 | nodenr + elseif (idx == 0x1800) then + if (subidx == 1) then + sdoval = (~pdo1on)<<31 | (0x0180 + nodenr) + elseif (subidx == 2) then + sdoval = 0xfe + elseif (subidx == 3) then + sdoval = 0 + endif + elseif (idx == 0x2000) then + if (subidx == 0) then + sdoval = GET POSERR + elseif (subidx == 1) then + sdoval = GET NEGLIMIT | (GET SWNEGLIMACT) << 30 + elseif (subidx == 2) then + sdoval = GET POSLIMIT | (GET SWPOSLIMACT) << 31 + endif + elseif (idx == 0x2001) then + sdoval = AXEND + elseif (idx == 0x2002) then + sdoval = AVEL + elseif (idx == 0x2003) then + if (subidx==0) then + sdoval = INB 0 + elseif (subidx>0 and subidx<2) then + sdoval = IN subidx + endif + elseif (idx == 0x2004) then + sdoval = STAT + elseif (idx == 0x4000) then + WAITAX + elseif (idx == 0x6000) then + if (GET POSDRCT == 1) then + sdoval = 0 + elseif (GET POSDRCT == -1) then + sdoval = 1 + endif + elseif (idx == 0x6002) then + sdoval = GET VELRES + elseif (idx == 0x6003) then + sdoval = GET HOME_OFFSET + elseif (idx == 0x6004) then + if (subidx == 0) then + sdoval = APOS + elseif (subidx==1) then + sdoval = CPOS + endif + elseif (idx == 0x6501) then + sdoval = GET ENCODER + elseif (idx == 0x6502) then + sdoval = GET VELMAX + elseif (idx == 0x6508) thenä + sdoval = TIME + else + CANOUT sdotx (canhi&0xffffff | 0x80000000) 0 + goto ENDSDOREQ + endif + + canlo = (sdoval&0xff)<<24 | (sdoval&0xff00)<<8 | (sdoval>>8)&0xff00 | (sdoval>>24)&0xff + CANOUT sdotx (canhi&0xffffff | 0x43000000) canlo +/* + PRINT "Returning: ", sdoval +*/ + ENDSDOREQ: + RETURN + + /*----------------------------------*/ + + SUBPROG PROC_SDORX +/* --Echo-- + CANOUT sdotx canhi canlo +*/ + cmd = canhi>>24 + if (cmd==0x23 OR cmd==0x2B OR cmd==0x2F) then + gosub PROC_SDOSET + elseif (cmd == 0x40) then + gosub PROC_SDOREQ + else + PRINT "Unknown SDO cmd", cmd + CANOUT sdotx (canhi&0xffffff | 0x80000000) 0 + endif + ENDSDORX: + RETURN + + /*----------------------------------*/ + /* PROC_CANMSG */ + /* called if a canmsg with */ + /* cobid=2*CANNR+1 is received */ + /* Warning: This doesn't fit to */ + /* CanOpen specification */ + /*----------------------------------*/ + SUBPROG PROC_CANMSG + varnr = InMsg(-1) + PRINT "varnr=", varnr, "msgval=", msgval + RETURN + +/*-------------------------------------------------------------------------*/ +/* PDO 1 Interrupt */ +/*-------------------------------------------------------------------------*/ + SUBPROG PROC_PDO1 + CANOUT pdo1 AXEND APOS + RETURN + +/*-------------------------------------------------------------------------*/ +/* Error sub proc */ +/*-------------------------------------------------------------------------*/ + SUBPROG PROC_ERROR + MOTOR STOP + errinf = 0 + + /* Tell the bus that an error occured */ + CANOUT pdo2 0 0 + + i = errlist[0] + 1 + while (i>1) do + errlist[i] = errlist[i-1] + i = i - 1 + endwhile + errlist[1] = ERRNO + if (errlist[0]<8) then + errlist[0] = errlist[0] + 1 + endif + + /* check if the error is repairable and repair */ + if (errlist[1]==6) then + PRINT "No home forced!" + ERRCLR + elseif (errlist[1]==8) then + PRINT "Schleppabstand überschritten" + ERRCLR + errinf = 0xaffe + elseif (errlist[1]==9) then + PRINT "Did'n find zero index." + ERRCLR + elseif (errlist[1]==25) then + lsw = -(GET I_POSLIMITSW) + if (IN lsw == 0) then + PRINT "Positive endswitch activated at position ", APOS + SET I_POSLIMITSW 0 + ERRCLR + CVEL (vres%100) /* 1% */ + ACC (10*vres%100) + DEC (10*vres%100) + CSTART + WHILE (IN lsw == 0) DO ENDWHILE + CSTOP + SET I_POSLIMITSW -lsw + errinf = 1 + endif + lsw = -(GET I_NEGLIMITSW) + if (IN lsw == 0) then + PRINT "Negative endswitch activated at position ", APOS + SET I_NEGLIMITSW 0 + ERRCLR + vres = GET VELRES + CVEL -(vres%100) /* 1% */ + ACC (10*vres%100) /* 10% */ + DEC (10*vres%100) /* 10% */ + OUT 1 1 + MOTOR ON + CSTART + WHILE (IN lsw == 0) DO ENDWHILE + CSTOP + SET I_NEGLIMITSW -lsw + errinf = -1 + endif + elseif (errlist[1]==84) then + PRINT "Too many (>12) ON TIME interrupts." + ERRCLR + ELSE + PRINT "Error Function Called: ERRNO=", errlist[1] + endif + + /* tell the bus what exactly happened */ + CANOUT pdo2 errlist[1] errinf + RETURN + +/*-------------------------------------------------------------------------*/ +/* End of part for Programs called with GOSUB */ +/*-------------------------------------------------------------------------*/ + +ENDPROG Index: trunk/MagicSoft/Cosy/base/timer.cc =================================================================== --- trunk/MagicSoft/Cosy/base/timer.cc (revision 737) +++ trunk/MagicSoft/Cosy/base/timer.cc (revision 738) @@ -1,4 +1,5 @@ #include "timer.h" +#include #include @@ -106,3 +107,11 @@ } +const char *Timer::GetTimeStr() +{ + sprintf(fDateStr, "%d/%02d/%02d %d:%02d:%02d.%06li", + fYea, fMon, fDay, fHor, fMin, fSec, (long)(1000000.0*fMs)); + + return fDateStr; +} + 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 737) +++ trunk/MagicSoft/Cosy/base/timer.h (revision 738) @@ -18,4 +18,6 @@ int fYea; + char fDateStr[27]; + public: Timer() : fMs(0), fSec(0), fSecs(0), fMin(0), fHor(0), fDay(0), fMon(0), fYea(0) {} @@ -32,4 +34,6 @@ double GetMjd(); + const char *GetTimeStr(); + void Print(); Index: trunk/MagicSoft/Cosy/cosy.cc =================================================================== --- trunk/MagicSoft/Cosy/cosy.cc (revision 738) +++ trunk/MagicSoft/Cosy/cosy.cc (revision 738) @@ -0,0 +1,51 @@ +#include "MCosy.h" + +#include +#include +#include + +#include +#include +#include + +#include "MGCosy.h" + +#include "macs.h" +#include "timer.h" +#include "slalib.h" +#include "slamac.h" +#include "shaftencoder.h" + +#include // PRIO_PROCESS + +int main(int argc, char **argv) +{ + // + // this must move to MGCosy !!!! (or MApplication) + // + ofstream lout("cosy.log"); + lout << "Starting Cosy" << endl; + + // + // start the main window + // + lout << "- Initialising Root environment." << endl; + TROOT root("Cosy", "Magic Control System"); + TApplication app ("App", &argc, argv); + + // + // Create the Network. Device: /dev/dpm_00, Rate: 500kbps + // + MCosy *cosy = new MCosy("/dev/dpm_00", 500, lout); + cosy->Start(); + + lout << "- Starting mailoop." << endl; + app.Run(); + + cosy->Stop(); + + delete cosy; + + lout << "- Terminating Program." << endl; + cout << "The End." << endl; +} Index: trunk/MagicSoft/Cosy/hex.cc =================================================================== --- trunk/MagicSoft/Cosy/hex.cc (revision 738) +++ trunk/MagicSoft/Cosy/hex.cc (revision 738) @@ -0,0 +1,11 @@ +#include +#include +#include + +int main(int argc, char **argv) +{ + if (argc==2) + cout << hex << setfill('0') << setw(8) << atol(argv[1]) << endl; + + return 0; +} Index: trunk/MagicSoft/Cosy/starg.cc =================================================================== --- trunk/MagicSoft/Cosy/starg.cc (revision 738) +++ trunk/MagicSoft/Cosy/starg.cc (revision 738) @@ -0,0 +1,28 @@ +#include + +#include +#include + +#include "Starguider.h" + +/* ---------------------------------------------------------------------- */ + +extern void InitGui(); +VoidFuncPtr_t initfuncs[] = { InitGui, 0 }; + +int main(int argc, char **argv) +{ + TROOT root("GUI", "GUI test environement", initfuncs); + + TApplication app("Starguider", &argc, argv); + + Starguider starg; + + starg.Loop(0); + + app.Run(); + + cout << "Exit." << endl; + + return 0; +} Index: trunk/MagicSoft/Cosy/videodev/Camera.cc =================================================================== --- trunk/MagicSoft/Cosy/videodev/Camera.cc (revision 738) +++ trunk/MagicSoft/Cosy/videodev/Camera.cc (revision 738) @@ -0,0 +1,331 @@ +#include "Camera.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "MStopwatch.h" + +#include "videodev.h" + +#include +#include + +inline int Camera::Ioctl(int req, void *opt, const char *str) +{ + int rc = ioctl(fd, req, opt); + + if (rc==-1) + { + cout << "Error! Ioctl " << req << ": "; + cout << (str?str:strerror(errno)) << " (rc=" << rc << ")" << endl; + } + + return rc; +} + +void Camera::Error(const char *str, int fatal) +{ + cout << endl + << (fatal?"Fatal ":"") << "Error! " << str << ": " << strerror(errno) + << endl; + + if (fatal) + exit(1); +} +void Camera::SigInit() +{ + struct sigaction act, old; + + memset(&act, 0, sizeof(act)); + + act.sa_handler = SigAlarm; + + sigemptyset(&act. sa_mask); + sigaction(SIGALRM, &act, &old); + + // signal(SIGINT, ctrlc); +} + +void Camera::SigAlarm(int signal) +{ + cout << "Camera: oops: got sigalarm" << endl; + exit(1); +} + +char *Camera::GetImg(unsigned int frame) +{ + // wait until grabbing is finished?? + // + // give signal SIGALARM + const int SYNC_TIMEOUT = 1; + + alarm(SYNC_TIMEOUT); + Ioctl(VIDIOCSYNC, &frame); // sync with mmap grabbing + alarm(0); + + return pMapBuffer+iOffsets[frame]; +} + +int Camera::StartGrab(unsigned int frame) +{ + static struct video_mmap gb = + { + 0, // frame + rows, cols, // height, width + VIDEO_PALETTE_RGB24 // palette + }; + + gb.frame = frame&1; + + // + // capture frame + // + if (Ioctl(VIDIOCMCAPTURE, &gb) != -1) + return true; + + if (errno == EAGAIN) + cout << "Grabber chip can't sync" << endl; + + return false; +} + +Camera::Camera() : fd(-1), iBufferSize(0) +{ + cout << "Starting thread..." << flush; + pthread_cond_init(&fCond, NULL); + pthread_mutex_init(&fMux, NULL); + pthread_mutex_lock(&fMux); + pthread_create(&fThread, NULL, MapThread, this); + cout << "done." << endl; + + cout << "/dev/video: opening..." << flush; + + // + // ------ Open device /dev/video ------ + // + do + { + fd = open("/dev/video", O_RDWR); + usleep(1); + } + while (errno==19 && fd==-1); + + if (fd == -1) + Error("open /dev/video"); + + fcntl(fd, F_SETFD, FD_CLOEXEC); // Close device on exit + SigInit(); + + // + // get input channel 0 information + // + struct video_channel ch; + ch.channel = 0; + Ioctl(VIDIOCGCHAN, &ch); + + // + // ioctl probe, switch to input 0 + // + Ioctl(VIDIOCSCHAN, &ch, "You need a bttv version > 0.5.13"); + + // + // map grab buffer, get size and offset + // + struct video_mbuf buffers; + Ioctl(VIDIOCGMBUF, &buffers); + + iBufferSize = buffers.size; + iOffsets[0] = buffers.offsets[0]; + iOffsets[1] = buffers.offsets[1]; + + // + // map file (device) into memory + // + pMapBuffer = mmap(0, iBufferSize, PROT_READ|PROT_WRITE, MAP_SHARED, fd, 0); + + if ((int)pMapBuffer == -1) + Error("mmap"); + + cout << "OK." << endl; + + cout << "Buffer Offset " << buffers.offsets[1] << endl; + cout << "grab: use: 768x576 24 bit TrueColor (LE: bgr)" << endl; +} + +Camera::~Camera() +{ + cout << "Stopping thread..." << flush; + pthread_cancel(fThread); + pthread_mutex_destroy(&fMux); + pthread_cond_destroy(&fCond); + cout << "done." << endl; + + cout << "/dev/video: closing... " << flush; + + // unmap device memory + if ((int)pMapBuffer != -1) + munmap(pMapBuffer, iBufferSize); + + if (fd != -1) + { + close(fd); + fd = -1; + } + + cout << " Done." << endl; +} + + +void Camera::Execute(const unsigned long n, + char *img, + struct timeval *tm) +{ + cout << "Img: " << n << " " << (void*)img << endl; +} + +void *Camera::MapThread(void *arg) +{ + Camera *cam = (Camera*)arg; + + // setpriority(PRIO_PROCESS, 0, 10); + pthread_detach(pthread_self()); + + cam->Thread(); + + return 0; +} + + +void Camera::LoopStep(const unsigned long n) +{ + char *img = GetImg(n&1); + + gettimeofday(&fTime, NULL); + + const char *end = img + cols*rows*depth; + char *beg = fImg; + + while (img < end) + { + *beg = *img; + + img += depth; + beg++; + } +} + +void Camera::Thread() +{ +#define IsOdd(i) (2*(i/2)!=i) + while (1) + { + pthread_cond_wait(&fCond, &fMux); + MStopwatch t; + t.Start(); + + unsigned long i=0; + + if (!StartGrab(0)) + continue; + + if (!StartGrab(1)) + continue; + + while (!(fStop || fNum && i==fNum-2)) + { + LoopStep(i); + if (!StartGrab(i&1)) + break; + Execute(i, fImg, &fTime); + i++; + } + + if (!IsOdd(i)) + { + LoopStep(i); + Execute(i, fImg, &fTime); + i++; + } + + LoopStep(i); + Execute(i, fImg, &fTime); + i++; + + // + // Wait until processing of frame 1 finished. + // + t.Stop(); + t.Print(i); + + fRunning = 0; + } +} + +void Camera::Loop(unsigned long nof) +{ + if (2*(nof/2) != nof) + { + cout << "Sorry, only even values are allowed!" << endl; + return; + } + + // + // Stop running loop + // + fStop = 1; + + // + // Wait until loop is stopped (pthread_cond_wait is executing) + // set new number of frames to process + // + pthread_mutex_lock(&fMux); + fNum = nof; + fStop = 0; + fRunning = 1; + pthread_mutex_unlock(&fMux); + + // + // Start execution + // + pthread_cond_signal(&fCond); +} + +void Camera::SetPicPar(int bright, int hue, int contrast) +{ + struct video_picture pict; + + Ioctl(VIDIOCGPICT, &pict); // get + + if (contrast != -1) + pict.contrast = contrast; + + if (bright != -1) + pict.brightness = bright; + + if (hue != -1) + pict.hue = hue; + + Ioctl(VIDIOCSPICT, &pict); //set +} + +void Camera::GetPicPar(int *bright, int *hue, int *contrast) +{ + struct video_picture pict; + + Ioctl(VIDIOCGPICT, &pict); // get + + *contrast = pict.contrast; + *bright = pict.brightness; + *hue = pict.hue; +} + Index: trunk/MagicSoft/Cosy/videodev/Camera.h =================================================================== --- trunk/MagicSoft/Cosy/videodev/Camera.h (revision 738) +++ trunk/MagicSoft/Cosy/videodev/Camera.h (revision 738) @@ -0,0 +1,97 @@ +#ifndef CAMERA_H +#define CAMERA_H + +#include +#include + +class Camera +{ +private: + // + // Geometry + // + static const int cols = 768; + static const int rows = 576; + static const int depth = 3; + + // + // Hardware Descriptors + // + int fd; + int iBufferSize; + int iOffsets[2]; + + char *pMapBuffer; + + // + // Thread interface + // + unsigned long fNum; + + int fStop; + int fRunning; + + char fImg[cols*rows]; + struct timeval fTime; + + pthread_t fThread; + pthread_mutex_t fMux; + pthread_cond_t fCond; + + // + // Hardware dependant functions + // + static void SigAlarm(int signal); + + int Ioctl(int req, void *opt, const char *str=NULL); + + void SigInit(); + void Error(const char *str, int fatal=true); + + int StartGrab(unsigned int frame); + + char *GetImg(unsigned int frame); + + // + // Execution thread which processes the pictures + // + void Thread(); + static void *MapThread(void *arg); + + void LoopStep(const unsigned long n); + +public: + Camera(); + virtual ~Camera(); + + // + // Starts, stops the execution + // + void Loop(const unsigned long nof=0); + void ExitLoop() + { + fStop = 1; + while (IsRunning()) + usleep(1); + } + + // + // flag if the execution is running or not + // + int IsRunning() const { return fRunning; } + + // + // Execution of one frame - this function may be overloaded! + // + virtual void Execute(const unsigned long n, + char *img, struct timeval *tm); + + // + // hardware features + // + void SetPicPar(int bright, int hue, int contrast); + void GetPicPar(int *bright, int *hue, int *contrast); + +}; + +#endif Index: trunk/MagicSoft/Cosy/videodev/Filter.cc =================================================================== --- trunk/MagicSoft/Cosy/videodev/Filter.cc (revision 738) +++ trunk/MagicSoft/Cosy/videodev/Filter.cc (revision 738) @@ -0,0 +1,198 @@ +#include "Filter.h" + +#include // memset +#include // cout + +void Filter::DrawBox(const int x1, const int y1, + const int x2, const int y2, + char *buffer, const int col) +{ + for (int x=x1; xval) + *min = val; + } + + mean /= (768-2*offset)*(576-2*offset); + + return mean; +} + +float Filter::SDev(const char *buffer, const int offset, const double mean) +{ + // + // calculate sigma + // + double sdev=0.0; + + for (int x=offset; x<768-offset; x++) + for (int y=offset; y<576-offset; y++) + { + const float val = mean - (unsigned char)buffer[y*768+x]; + + sdev += val*val; + } + + sdev /= (768-2*offset)*(576-2*offset)-1; + + return sqrt(sdev); +} + +int Filter::GetMeanPosition(const char *bitmap, const int x, const int y, + const int box) +{ + unsigned int sumx=0; + unsigned int sumy=0; + + unsigned int sum=0; + + for (int dx=x-box; dxbuffer[3*(x+y*768)]; // + + sumx += m*dx; + sumy += m*dy; + sum += m; + } + + const float px = (float)sumx/sum; + const float py = (float)sumy/sum; + + return (int)py*768 + (int)px; +} + +void Filter::Execute(char *img) +{ + const int offset = 10; + + int max; + int min; + + const float mean = Mean(img, offset, &min, &max); + const float sdev = SDev(img, offset, mean); + + const float cut = mean + 3*sdev; + + // + // clean image from noise + // + for (int x=0; x<768; x++) + for (int y=0; y<576; y++) + { + if (img[y*768+x]>cut) + continue; + + img[y*768+x] = 0; + } + + // + // find mean points + // + const int maxpnt = 0x1000; + + int pos[maxpnt+1][2]; // FIXME + int cnt = 0; + + for (int x=offset; x<768-offset; x++) + { + for (int y=offset; y<576-offset; y++) + { + if (img[x+768*y]==0) + continue; + + const int ipos = GetMeanPosition(img, x, y, 5); + + int j; + for (j=0; j0xa0) + { + points++; + + int px = pos[i][0]%768; + int py = pos[i][0]/768; + + MarkPoint(px, py, marker, pos[i][1]); + } + } + + // + // Copy markers into image + // + for (int x=0; x<768*576; x++) + { + if (!marker[x]) + continue; + + img[x]=marker[x]; + } +} + Index: trunk/MagicSoft/Cosy/videodev/Filter.h =================================================================== --- trunk/MagicSoft/Cosy/videodev/Filter.h (revision 738) +++ trunk/MagicSoft/Cosy/videodev/Filter.h (revision 738) @@ -0,0 +1,27 @@ +#ifndef FILTER_H +#define FILTER_H + +class Filter +{ + static void DrawBox(const int x1, const int y1, + const int x2, const int y2, + char *buffer, const int col); + + static void MarkPoint(const int x, const int y, + char *buffer, const int col); + + static float Mean(const char *buffer, const int offset, + int *min, int *max); + + static float SDev(const char *buffer, const int offset, + const double mean); + + static int GetMeanPosition(const char *bitmap, + const int x, const int y, + const int box); + +public: + static void Execute(char *img); +}; + +#endif Index: trunk/MagicSoft/Cosy/videodev/Writer.cc =================================================================== --- trunk/MagicSoft/Cosy/videodev/Writer.cc (revision 738) +++ trunk/MagicSoft/Cosy/videodev/Writer.cc (revision 738) @@ -0,0 +1,125 @@ +#include "Writer.h" + +#include // cout +#include // ofstream + +#include // FILE +#include + +#include "timer.h" + +void Writer::Png(const char *fname, const char *buf, + struct timeval *date) +{ + cout << "Writing PNG '" << fname << "'" << endl; + + // + // open file + // + FILE *fd = fopen(fname, "w"); + if (!fd) + return; + + // + // allocate memory + // + png_structp fPng = png_create_write_struct(PNG_LIBPNG_VER_STRING, + NULL, NULL, NULL); + + if (!fPng) + { + cout << "Warning: Unable to create PNG structure" << endl; + fclose(fd); + return; + } + + + png_infop fInfo = png_create_info_struct(fPng); + + if (!fInfo) + { + cout << "Warning: Unable to create PNG info structure" << endl; + png_destroy_write_struct (&fPng, NULL); + fclose(fd); + return; + } + + fInfo->width = 768; + fInfo->height = 576; + fInfo->bit_depth = 8; + fInfo->color_type = PNG_COLOR_TYPE_GRAY; + + // + // set jump-back point in case of errors + // + if (setjmp(fPng->jmpbuf)) + { + cout << "longjmp Warning: PNG encounterd an error!" << endl; + png_destroy_write_struct (&fPng, &fInfo); + fclose(fd); + return; + } + + // + // connect file to PNG-Structure + // + png_init_io(fPng, fd); + + // png_set_compression_level (fPng, Z_BEST_COMPRESSION); + + // + // Write header + // + png_write_info (fPng, fInfo); + + // + // Write Time Chunks + // + if (date) + { + char text[36]; + + Timer time(date); + sprintf(text, "*** %s ***", time.GetTimeStr()); + png_write_chunk(fPng, (png_byte*)"UTC", (png_byte*)text, strlen(text)); + sprintf(text,"*** %s %s %.1f %i ***", tzname[0], tzname[1], 1.0/3600*timezone, daylight); + png_write_chunk(fPng, (png_byte*)"ZONE", (png_byte*)text, strlen(text)); + } + + // + // Write bitmap data + // + for (unsigned int y=0; y<768*576; y+=768) + png_write_row (fPng, (png_byte*)buf+y); + + // + // Write footer + // + png_write_end (fPng, fInfo); + + // + // free memory + // + png_destroy_write_struct (&fPng, &fInfo); + + fclose(fd); +} + +void Writer::Ppm(const char *fname, const char *img) +{ + cout << "Writing PPM '" << fname << "'" << endl; + + // + // open file for writing + // + ofstream fout(fname); + if (!fout) + return; + + // + // write buffer to file + // + fout << "P6\n768 576\n255\n"; + for (char const *buf = img; buf < img+768*576; buf++) + fout << *buf << *buf << *buf; +} Index: trunk/MagicSoft/Cosy/videodev/Writer.h =================================================================== --- trunk/MagicSoft/Cosy/videodev/Writer.h (revision 738) +++ trunk/MagicSoft/Cosy/videodev/Writer.h (revision 738) @@ -0,0 +1,16 @@ +#ifndef WRITER_H +#define WRITER_H + +#include + +class Writer; + +class Writer +{ +public: + + static void Ppm(const char *fname, const char *img); + static void Png(const char *fname, const char *buf, struct timeval *date); +}; + +#endif Index: trunk/MagicSoft/Cosy/videodev/videodev.h =================================================================== --- trunk/MagicSoft/Cosy/videodev/videodev.h (revision 738) +++ trunk/MagicSoft/Cosy/videodev/videodev.h (revision 738) @@ -0,0 +1,283 @@ +#ifndef __LINUX_VIDEODEV_H +#define __LINUX_VIDEODEV_H + +#include + +/*#ifdef __KERNEL__ + +#if LINUX_VERSION_CODE >= 0x020100 +#include +#endif + +struct video_device +{ + char name[32]; + int type; + int hardware; + + int (*open)(struct video_device *, int mode); + void (*close)(struct video_device *); + long (*read)(struct video_device *, char *, unsigned long, int noblock); + // Do we need a write method ? + long (*write)(struct video_device *, const char *, unsigned long, int noblock); +#if LINUX_VERSION_CODE >= 0x020100 + unsigned int (*poll)(struct video_device *, struct file *, poll_table *); +#endif + int (*ioctl)(struct video_device *, unsigned int , void *); + int (*mmap)(struct video_device *, const char *, unsigned long); + int (*initialize)(struct video_device *); + void *priv; // Used to be 'private' but that upsets C++ + int busy; + int minor; +}; + +extern int videodev_init(void); +#define VIDEO_MAJOR 81 +extern int video_register_device(struct video_device *, int type); + +#define VFL_TYPE_GRABBER 0 +#define VFL_TYPE_VBI 1 +#define VFL_TYPE_RADIO 2 +#define VFL_TYPE_VTX 3 + +extern void video_unregister_device(struct video_device *); +#endif +*/ + +#define VID_TYPE_CAPTURE 1 /* Can capture */ +#define VID_TYPE_TUNER 2 /* Can tune */ +#define VID_TYPE_TELETEXT 4 /* Does teletext */ +#define VID_TYPE_OVERLAY 8 /* Overlay onto frame buffer */ +#define VID_TYPE_CHROMAKEY 16 /* Overlay by chromakey */ +#define VID_TYPE_CLIPPING 32 /* Can clip */ +#define VID_TYPE_FRAMERAM 64 /* Uses the frame buffer memory */ +#define VID_TYPE_SCALES 128 /* Scalable */ +#define VID_TYPE_MONOCHROME 256 /* Monochrome only */ +#define VID_TYPE_SUBCAPTURE 512 /* Can capture subareas of the image */ + +struct video_capability +{ + char name[32]; + int type; + int channels; /* Num channels */ + int audios; /* Num audio devices */ + int maxwidth; /* Supported width */ + int maxheight; /* And height */ + int minwidth; /* Supported width */ + int minheight; /* And height */ +}; + + +struct video_channel +{ + int channel; + char name[32]; + int tuners; + __u32 flags; +#define VIDEO_VC_TUNER 1 /* Channel has a tuner */ +#define VIDEO_VC_AUDIO 2 /* Channel has audio */ + __u16 type; +#define VIDEO_TYPE_TV 1 +#define VIDEO_TYPE_CAMERA 2 + __u16 norm; /* Norm set by channel */ +}; + +struct video_tuner +{ + int tuner; + char name[32]; + ulong rangelow, rangehigh; /* Tuner range */ + __u32 flags; +#define VIDEO_TUNER_PAL 1 +#define VIDEO_TUNER_NTSC 2 +#define VIDEO_TUNER_SECAM 4 +#define VIDEO_TUNER_LOW 8 /* Uses KHz not MHz */ +#define VIDEO_TUNER_NORM 16 /* Tuner can set norm */ +#define VIDEO_TUNER_STEREO_ON 128 /* Tuner is seeing stereo */ + __u16 mode; /* PAL/NTSC/SECAM/OTHER */ +#define VIDEO_MODE_PAL 0 +#define VIDEO_MODE_NTSC 1 +#define VIDEO_MODE_SECAM 2 +#define VIDEO_MODE_AUTO 3 + __u16 signal; /* Signal strength 16bit scale */ +}; + +struct video_picture +{ + __u16 brightness; + __u16 hue; + __u16 colour; + __u16 contrast; + __u16 whiteness; /* Black and white only */ + __u16 depth; /* Capture depth */ + __u16 palette; /* Palette in use */ +#define VIDEO_PALETTE_GREY 1 /* Linear greyscale */ +#define VIDEO_PALETTE_HI240 2 /* High 240 cube (BT848) */ +#define VIDEO_PALETTE_RGB565 3 /* 565 16 bit RGB */ +#define VIDEO_PALETTE_RGB24 4 /* 24bit RGB */ +#define VIDEO_PALETTE_RGB32 5 /* 32bit RGB */ +#define VIDEO_PALETTE_RGB555 6 /* 555 15bit RGB */ +#define VIDEO_PALETTE_YUV422 7 /* YUV422 capture */ +#define VIDEO_PALETTE_YUYV 8 +#define VIDEO_PALETTE_UYVY 9 /* The great thing about standards is ... */ +#define VIDEO_PALETTE_YUV420 10 +#define VIDEO_PALETTE_YUV411 11 /* YUV411 capture */ +#define VIDEO_PALETTE_RAW 12 /* RAW capture (BT848) */ +#define VIDEO_PALETTE_YUV422P 13 /* YUV 4:2:2 Planar */ +#define VIDEO_PALETTE_YUV411P 14 /* YUV 4:1:1 Planar */ +#define VIDEO_PALETTE_YUV420P 15 /* YUV 4:2:0 Planar */ +#define VIDEO_PALETTE_YUV410P 16 /* YUV 4:1:0 Planar */ +#define VIDEO_PALETTE_PLANAR 13 /* start of planar entries */ +#define VIDEO_PALETTE_COMPONENT 7 /* start of component entries */ +}; + +struct video_audio +{ + int audio; /* Audio channel */ + __u16 volume; /* If settable */ + __u16 bass, treble; + __u32 flags; +#define VIDEO_AUDIO_MUTE 1 +#define VIDEO_AUDIO_MUTABLE 2 +#define VIDEO_AUDIO_VOLUME 4 +#define VIDEO_AUDIO_BASS 8 +#define VIDEO_AUDIO_TREBLE 16 + char name[16]; +#define VIDEO_SOUND_MONO 1 +#define VIDEO_SOUND_STEREO 2 +#define VIDEO_SOUND_LANG1 4 +#define VIDEO_SOUND_LANG2 8 + __u16 mode; /* detected audio carriers or one to set */ + __u16 balance; /* Stereo balance */ + __u16 step; /* Step actual volume uses */ +}; + +struct video_clip +{ + __s32 x,y; + __s32 width, height; + struct video_clip *next; /* For user use/driver use only */ +}; + +struct video_window +{ + __u32 x,y; /* Position of window */ + __u32 width,height; /* Its size */ + __u32 chromakey; + __u32 flags; + struct video_clip *clips; /* Set only */ + int clipcount; +#define VIDEO_WINDOW_INTERLACE 1 +#define VIDEO_CLIP_BITMAP -1 +/* bitmap is 1024x625, a '1' bit represents a clipped pixel */ +#define VIDEO_CLIPMAP_SIZE (128 * 625) +}; + +struct video_capture +{ + __u32 x,y; /* Offsets into image */ + __u32 width, height; /* Area to capture */ + __u16 decimation; /* Decimation divder */ + __u16 flags; /* Flags for capture */ +#define VIDEO_CAPTURE_ODD 0 /* Temporal */ +#define VIDEO_CAPTURE_EVEN 1 +}; + +struct video_buffer +{ + void *base; + int height,width; + int depth; + int bytesperline; +}; + +struct video_mmap +{ + unsigned int frame; /* Frame (0 - n) for double buffer */ + int height,width; + unsigned int format; /* should be VIDEO_PALETTE_* */ +}; + +struct video_key +{ + __u8 key[8]; + __u32 flags; +}; + + +#define VIDEO_MAX_FRAME 32 + +struct video_mbuf +{ + int size; /* Total memory to map */ + int frames; /* Frames */ + int offsets[VIDEO_MAX_FRAME]; +}; + + +#define VIDEO_NO_UNIT (-1) + + +struct video_unit +{ + int video; /* Video minor */ + int vbi; /* VBI minor */ + int radio; /* Radio minor */ + int audio; /* Audio minor */ + int teletext; /* Teletext minor */ +}; + +#define VIDIOCGCAP _IOR('v',1,struct video_capability) /* Get capabilities */ +#define VIDIOCGCHAN _IOWR('v',2,struct video_channel) /* Get channel info (sources) */ +#define VIDIOCSCHAN _IOW('v',3,struct video_channel) /* Set channel */ +#define VIDIOCGTUNER _IOWR('v',4,struct video_tuner) /* Get tuner abilities */ +#define VIDIOCSTUNER _IOW('v',5,struct video_tuner) /* Tune the tuner for the current channel */ +#define VIDIOCGPICT _IOR('v',6,struct video_picture) /* Get picture properties */ +#define VIDIOCSPICT _IOW('v',7,struct video_picture) /* Set picture properties */ +#define VIDIOCCAPTURE _IOW('v',8,int) /* Start, end capture */ +#define VIDIOCGWIN _IOR('v',9, struct video_window) /* Set the video overlay window */ +#define VIDIOCSWIN _IOW('v',10, struct video_window) /* Set the video overlay window - passes clip list for hardware smarts , chromakey etc */ +#define VIDIOCGFBUF _IOR('v',11, struct video_buffer) /* Get frame buffer */ +#define VIDIOCSFBUF _IOW('v',12, struct video_buffer) /* Set frame buffer - root only */ +#define VIDIOCKEY _IOR('v',13, struct video_key) /* Video key event - to dev 255 is to all - cuts capture on all DMA windows with this key (0xFFFFFFFF == all) */ +#define VIDIOCGFREQ _IOR('v',14, unsigned long) /* Set tuner */ +#define VIDIOCSFREQ _IOW('v',15, unsigned long) /* Set tuner */ +#define VIDIOCGAUDIO _IOR('v',16, struct video_audio) /* Get audio info */ +#define VIDIOCSAUDIO _IOW('v',17, struct video_audio) /* Audio source, mute etc */ +#define VIDIOCSYNC _IOW('v',18, int) /* Sync with mmap grabbing */ +#define VIDIOCMCAPTURE _IOW('v',19, struct video_mmap) /* Grab frames */ +#define VIDIOCGMBUF _IOR('v', 20, struct video_mbuf) /* Memory map buffer info */ +#define VIDIOCGUNIT _IOR('v', 21, struct video_unit) /* Get attached units */ +#define VIDIOCGCAPTURE _IOR('v',22, struct video_capture) /* Get frame buffer */ +#define VIDIOCSCAPTURE _IOW('v',23, struct video_capture) /* Set frame buffer - root only */ + +#define BASE_VIDIOCPRIVATE 192 /* 192-255 are private */ + + +#define VID_HARDWARE_BT848 1 +#define VID_HARDWARE_QCAM_BW 2 +#define VID_HARDWARE_PMS 3 +#define VID_HARDWARE_QCAM_C 4 +#define VID_HARDWARE_PSEUDO 5 +#define VID_HARDWARE_SAA5249 6 +#define VID_HARDWARE_AZTECH 7 +#define VID_HARDWARE_SF16MI 8 +#define VID_HARDWARE_RTRACK 9 +#define VID_HARDWARE_ZOLTRIX 10 +#define VID_HARDWARE_SAA7146 11 +#define VID_HARDWARE_VIDEUM 12 /* Reserved for Winnov videum */ +#define VID_HARDWARE_RTRACK2 13 +#define VID_HARDWARE_PERMEDIA2 14 /* Reserved for Permedia2 */ +#define VID_HARDWARE_RIVA128 15 /* Reserved for RIVA 128 */ + +/* + * Initialiser list + */ + +struct video_init +{ + char *name; + int (*init)(struct video_init *); +}; + +#endif