Index: /trunk/FACT++/src/drivectrl.cc =================================================================== --- /trunk/FACT++/src/drivectrl.cc (revision 15362) +++ /trunk/FACT++/src/drivectrl.cc (revision 15363) @@ -129,4 +129,344 @@ uint64_t fTrackingCounter; + void ProcessDriveStatus(const string &line) + { + Message(line); + } + + bool ProcessStargReport(const string &line) + { + istringstream stream(line); + + // 0: Error + // 1: Standby + // 2: Monitoring + uint16_t status1; + stream >> status1; + /*const Time t1 = */ReadTime(stream); + + uint16_t status2; + stream >> status2; + /*const Time t2 = */ReadTime(stream); + + double misszd, missaz; + stream >> misszd >> missaz; + + const double zd = ReadAngle(stream); + const double az = ReadAngle(stream); + + double cx, cy; + stream >> cx >> cy; + + int ncor; + stream >> ncor; + + double bright, mjd; + stream >> bright >> mjd; + + int nled, nring, nstars; + stream >> nled >> nring >> nstars; + + if (stream.fail()) + return false; + + DimStarguider data; + + data.fMissZd = misszd; + data.fMissAz = missaz; + data.fNominalZd = zd; + data.fNominalAz = az; + data.fCenterX = cx; + data.fCenterY = cy; + data.fNumCorrelated = ncor; + data.fBrightness = bright; + data.fNumLeds = nled; + data.fNumRings = nring; + data.fNumStars = nstars; + + UpdateStarguider(Time(mjd), data); + + return true; + } + + bool ProcessTpointReport(const string &line) + { + istringstream stream(line); + + uint16_t status1; + stream >> status1; + const Time t1 = ReadTime(stream); + + uint16_t status2; + stream >> status2; + /*const Time t2 =*/ ReadTime(stream); + + char type; + stream >> type; + if (type != 'T') + return false; + + double az1, alt1, az2, alt2, ra, dec, dzd, daz; + stream >> az1 >> alt1 >> az2 >> alt2 >> ra >> dec >> dzd >> daz; + + // c: center, s:start + double mjd, cmag, smag, cx, cy, sx, sy; + stream >> mjd >> cmag >> smag >> cx >> cy >> sx >> sy; + + int nled, nring, nstar, ncor; + stream >> nled >> nring >> nstar >> ncor; + + double bright, mag; + stream >> bright >> mag; + + string name; + stream >> name; + + if (stream.fail()) + return false; + + DimTPoint tpoint; + + tpoint.fRa = ra; + tpoint.fDec = dec; + + tpoint.fNominalZd = 90-alt1-dzd; + tpoint.fNominalAz = 90-az1 +daz; + + tpoint.fPointingZd = 90-alt1; + tpoint.fPointingAz = az1; + + tpoint.fFeedbackZd = 90-alt2; + tpoint.fFeedbackAz = az2; + + tpoint.fNumLeds = nled; + tpoint.fNumRings = nring; + + tpoint.fCenterX = cx; + tpoint.fCenterY = cy; + tpoint.fCenterMag = cmag; + + tpoint.fStarX = sx; + tpoint.fStarY = sy; + tpoint.fStarMag = smag; + + tpoint.fRealMag = mag; + + UpdateTPoint(t1, tpoint, name); + + return true; + } + + bool ProcessDriveReport(const string &line) + { + // DRIVE-REPORT M1 + // 01 2011 05 14 11 31 19 038 + // 02 1858 11 17 00 00 00 000 + // + 000 00 000 + 000 00 000 + // + 000 00 000 + // 55695.480081 + // + 000 00 000 + 000 00 000 + // + 000 00 000 + 000 00 000 + // 0000.000 0000.000 + // 0 2 + + // status + // year month day hour minute seconds millisec + // year month day hour minute seconds millisec + // ra(+ h m s) dec(+ d m s) ha(+ h m s) + // mjd + // zd(+ d m s) az(+ d m s) + // zd(+ d m s) az(+ d m s) + // zd_err az_err + // armed(0=unlocked, 1=locked) + // stgmd(0=none, 1=starguider, 2=starguider off) + istringstream stream(line); + + uint16_t status1; + stream >> status1; + const Time t1 = ReadTime(stream); + + uint16_t status2; + stream >> status2; + /*const Time t2 =*/ ReadTime(stream); + + const double ra = ReadAngle(stream); + const double dec = ReadAngle(stream); + const double ha = ReadAngle(stream); + + double mjd; + stream >> mjd; + + const double zd1 = ReadAngle(stream); // Nominal (zd/az asynchronous, dev synchronous, mjd synchronous with zd) + const double az1 = ReadAngle(stream); // Nominal (zd/az asynchronous, dev synchronous, mjd synchronous with z) + const double zd2 = ReadAngle(stream); // Masured (zd/az synchronous, dev asynchronous, mjd asynchronous) + const double az2 = ReadAngle(stream); // Measurd (zd/az synchronous, dev asynchronous, mjd asynchronous) + + double zd_err, az_err; + stream >> zd_err; // Deviation = Nominal - Measured + stream >> az_err; // Deviation = Nominal - Measured + + uint16_t armed, stgmd; + stream >> armed; + stream >> stgmd; + + uint32_t pdo3; + stream >> hex >> pdo3; + + if (stream.fail()) + return false; + + // Status 0: Error + // Status 1: Stopped + // Status 3: Stopping || Moving + // Status 4: Tracking + if (status1==0) + status1 = StateMachineImp::kSM_Error - Drive::State::kNotReady; + + const bool ready = (pdo3&0xef00ef)==0xef00ef; + if (!ready) + fState = Drive::State::kNotReady; + else + fState = status1==1 ? + Drive::State::kReady+armed : + Drive::State::kNotReady+status1; + + // kDisconnected = 1, + // kConnected, + // kNotReady, + // kReady, + // kArmed, + // kMoving, + // kTracking, + // kOnTrack, + + // pdo3: + // 1 Ab + // 2 1 + // 4 Emergency + // 8 OverVolt + // 10 Move (Drehen-soll) + // 20 Af + // 40 1 + // 80 Power on Az + // ------------------ + // 100 NOT UPS Alarm + // 200 UPS on Battery + // 400 UPS charging + + // Power cut: 2ef02ef + // charging: 4ef04ef + + // Convert to deg + zd_err /= 3600; + az_err /= 3600; + + // Calculate absolut deviation on the sky + const double dev = GetDevAbs(zd1, zd1-zd_err, az_err)*3600; + + // If any other state than tracking or a deviation + // larger than 60, reset the counter + if (fState!=State::kTracking || dev>fDeviationLimit) + fTrackingCounter = 0; + else + fTrackingCounter++; + + // If in tracking, at least five consecutive reports (5s) + // must be below 60arcsec deviation, this is considered OnTrack + if (fState==State::kTracking && fTrackingCounter>=fDeviationCounter) + fState = State::kOnTrack; + + // Having th state as Tracking will reset the counter + if (fState==State::kOnTrack && dev>fDeviationMax) + fState = State::kTracking; + + // 206 206 ce ce pwr vlt emcy fs | pwr vlt emcy fs + // 239 239 ef ef pwr vlt emcy fs bb rf | pwr vlt emcy fs bb rf + // 111 78 6f 4e vlt emcy fs bb rf | emcy fs bb + + /* + fArmed = data[3]&0x01; // armed status + fPosActive = data[3]&0x02; // positioning active + fRpmActive = data[3]&0x04; // RPM mode switched on + // data[3]&0x08; // - unused - + // data[3]&0x10; // - unused - + // data[3]&0x20; // - unused - + //fInControl = data[3]&0x40; // motor uncontrolled + // data[3]&0x80; // axis resetted (after errclr, motor stop, motor on) + + fStatus = data[3]; + } + + const LWORD_t stat = data[0] | (data[1]<<8); + if (fStatusPdo3!=stat) + { + gLog << inf << MTime(-1) << ": " << GetNodeName() << " - PDO3(0x" << hex << (int)stat << dec << ") = "; + const Bool_t ready = stat&0x001; + const Bool_t fuse = stat&0x002; + const Bool_t emcy = stat&0x004; + const Bool_t vltg = stat&0x008; + const Bool_t mode = stat&0x010; + const Bool_t rf = stat&0x020; + const Bool_t brake = stat&0x040; + const Bool_t power = stat&0x080; + const Bool_t alarm = stat&0x100; // UPS Alarm (FACT only) + const Bool_t batt = stat&0x200; // UPS on battery (FACT only) + const Bool_t charge = stat&0x400; // UPS charging (FACT only) + if (ready) gLog << "DKC-Ready "; + if (fuse) gLog << "FuseOk "; + if (emcy) gLog << "EmcyOk "; + if (vltg) gLog << "OvervoltOk "; + if (mode) gLog << "SwitchToManualMode "; + if (rf) gLog << "RF "; + if (brake) gLog << "BrakeOpen "; + if (power) gLog << "PowerOn "; + if (alarm) gLog << "UPS-PowerLoss "; + if (batt) gLog << "UPS-OnBattery "; + if (charge) gLog << "UPS-Charging "; + gLog << endl; + + fStatusPdo3 = stat; + }*/ + + // ((stat1&0xffff)<<16)|(stat2&0xffff) + // no alarm, no batt, no charge + const array state = {{ uint8_t(pdo3>>16), uint8_t(pdo3), uint8_t(pdo3>>24) }}; + UpdateStatus(t1, state); + + const array point = {{ zd2, az2 }}; + UpdatePointing(t1, point); + + const array track = + {{ + ra, dec, ha, + zd1, az1, + zd_err, az_err, + dev + }}; + if (mjd>0) + UpdateTracking(Time(mjd), track); + + // ---- DIM ----> t1 as event time + // status1 + // mjd + // ra/dec/ha + // zd/az (nominal) + // zd/az (current) + // err(zd/az) + // [armed] [stgmd] + + // Maybe: + // POINTING_POSITION --> t1, zd/az (current), [armed, stgmd, status1] + // + // if (mjd>0) + // TRACKING_POSITION --> mjd, zd/az (nominal), err(zd/az) + // ra/dec, ha(not well defined), + // [Nominal + Error == Current] + + // MJD is the time which corresponds to the nominal position + // t1 is the time which corresponds to the current position/HA + + return true; + } + protected: void HandleReceivedReport(const boost::system::error_code& err, size_t bytes_received) @@ -164,5 +504,5 @@ if (line.substr(0, 13)=="DRIVE-STATUS ") { - Message(line.substr(70)); + ProcessDriveStatus(line.substr(70)); return; } @@ -170,123 +510,11 @@ if (line.substr(0, 13)=="STARG-REPORT ") { - istringstream stream(line.substr(16)); - - // 0: Error - // 1: Standby - // 2: Monitoring - uint16_t status1; - stream >> status1; - /*const Time t1 = */ReadTime(stream); - - uint16_t status2; - stream >> status2; - /*const Time t2 = */ReadTime(stream); - - double misszd, missaz; - stream >> misszd >> missaz; - - const double zd = ReadAngle(stream); - const double az = ReadAngle(stream); - - double cx, cy; - stream >> cx >> cy; - - int ncor; - stream >> ncor; - - double bright, mjd; - stream >> bright >> mjd; - - int nled, nring, nstars; - stream >> nled >> nring >> nstars; - - if (stream.fail()) - return; - - DimStarguider data; - - data.fMissZd = misszd; - data.fMissAz = missaz; - data.fNominalZd = zd; - data.fNominalAz = az; - data.fCenterX = cx; - data.fCenterY = cy; - data.fNumCorrelated = ncor; - data.fBrightness = bright; - data.fNumLeds = nled; - data.fNumRings = nring; - data.fNumStars = nstars; - - UpdateStarguider(Time(mjd), data); - + ProcessStargReport(line.substr(16)); return; - } if (line.substr(0, 14)=="TPOINT-REPORT ") { - istringstream stream(line.substr(17)); - - uint16_t status1; - stream >> status1; - const Time t1 = ReadTime(stream); - - uint16_t status2; - stream >> status2; - /*const Time t2 =*/ ReadTime(stream); - - char type; - stream >> type; - if (type != 'T') - return; - - double az1, alt1, az2, alt2, ra, dec, dzd, daz; - stream >> az1 >> alt1 >> az2 >> alt2 >> ra >> dec >> dzd >> daz; - - // c: center, s:start - double mjd, cmag, smag, cx, cy, sx, sy; - stream >> mjd >> cmag >> smag >> cx >> cy >> sx >> sy; - - int nled, nring, nstar, ncor; - stream >> nled >> nring >> nstar >> ncor; - - double bright, mag; - stream >> bright >> mag; - - string name; - stream >> name; - - if (stream.fail()) - return; - - DimTPoint tpoint; - - tpoint.fRa = ra; - tpoint.fDec = dec; - - tpoint.fNominalZd = 90-alt1-dzd; - tpoint.fNominalAz = 90-az1 +daz; - - tpoint.fPointingZd = 90-alt1; - tpoint.fPointingAz = az1; - - tpoint.fFeedbackZd = 90-alt2; - tpoint.fFeedbackAz = az2; - - tpoint.fNumLeds = nled; - tpoint.fNumRings = nring; - - tpoint.fCenterX = cx; - tpoint.fCenterY = cy; - tpoint.fCenterMag = cmag; - - tpoint.fStarX = sx; - tpoint.fStarY = sy; - tpoint.fStarMag = smag; - - tpoint.fRealMag = mag; - - UpdateTPoint(t1, tpoint, name); - + ProcessTpointReport(line.substr(17)); return; } @@ -294,161 +522,5 @@ if (line.substr(0, 13)=="DRIVE-REPORT ") { - // DRIVE-REPORT M1 - // 01 2011 05 14 11 31 19 038 - // 02 1858 11 17 00 00 00 000 - // + 000 00 000 + 000 00 000 - // + 000 00 000 - // 55695.480081 - // + 000 00 000 + 000 00 000 - // + 000 00 000 + 000 00 000 - // 0000.000 0000.000 - // 0 2 - - // status - // year month day hour minute seconds millisec - // year month day hour minute seconds millisec - // ra(+ h m s) dec(+ d m s) ha(+ h m s) - // mjd - // zd(+ d m s) az(+ d m s) - // zd(+ d m s) az(+ d m s) - // zd_err az_err - // armed(0=unlocked, 1=locked) - // stgmd(0=none, 1=starguider, 2=starguider off) - istringstream stream(line.substr(16)); - - uint16_t status1; - stream >> status1; - const Time t1 = ReadTime(stream); - - uint16_t status2; - stream >> status2; - /*const Time t2 =*/ ReadTime(stream); - - const double ra = ReadAngle(stream); - const double dec = ReadAngle(stream); - const double ha = ReadAngle(stream); - - double mjd; - stream >> mjd; - - const double zd1 = ReadAngle(stream); // Nominal (zd/az asynchronous, dev synchronous, mjd synchronous with zd) - const double az1 = ReadAngle(stream); // Nominal (zd/az asynchronous, dev synchronous, mjd synchronous with z) - const double zd2 = ReadAngle(stream); // Masured (zd/az synchronous, dev asynchronous, mjd asynchronous) - const double az2 = ReadAngle(stream); // Measurd (zd/az synchronous, dev asynchronous, mjd asynchronous) - - double zd_err, az_err; - stream >> zd_err; // Deviation = Nominal - Measured - stream >> az_err; // Deviation = Nominal - Measured - - uint16_t armed, stgmd; - stream >> armed; - stream >> stgmd; - - uint32_t pdo3; - stream >> hex >> pdo3; - - if (stream.fail()) - return; - - // Status 0: Error - // Status 1: Stopped - // Status 3: Stopping || Moving - // Status 4: Tracking - if (status1==0) - status1 = StateMachineImp::kSM_Error - Drive::State::kNotReady; - - const bool ready = (pdo3&0xef00ef)==0xef00ef; - if (!ready) - fState = Drive::State::kNotReady; - else - fState = status1==1 ? - Drive::State::kReady+armed : - Drive::State::kNotReady+status1; - - // kDisconnected = 1, - // kConnected, - // kNotReady, - // kReady, - // kArmed, - // kMoving, - // kTracking, - // kOnTrack, - - // pdo3: - // 1 Ab - // 2 1 - // 4 Emergency - // 8 OverVolt - // 10 Move (Drehen-soll) - // 20 Af - // 40 1 - // 80 Power on Az - // ------------------ - // 100 NOT UPS Alarm - // 200 UPS on Battery - // 400 UPS charging - - // Power cut: 2ef02ef - // charging: 4ef04ef - - // Convert to deg - zd_err /= 3600; - az_err /= 3600; - - // Calculate absolut deviation on the sky - const double dev = GetDevAbs(zd1, zd1-zd_err, az_err)*3600; - - // If any other state than tracking or a deviation - // larger than 60, reset the counter - if (fState!=State::kTracking || dev>fDeviationLimit) - fTrackingCounter = 0; - else - fTrackingCounter++; - - // If in tracking, at least five consecutive reports (5s) - // must be below 60arcsec deviation, this is considered OnTrack - if (fState==State::kTracking && fTrackingCounter>=fDeviationCounter) - fState = State::kOnTrack; - - // Having th state as Tracking will reset the counter - if (fState==State::kOnTrack && dev>fDeviationMax) - fState = State::kTracking; - - const array state = {{ uint8_t(pdo3>>16), uint8_t(pdo3), uint8_t(pdo3>>24) }}; - UpdateStatus(t1, state); - - const array point = {{ zd2, az2 }}; - UpdatePointing(t1, point); - - const array track = - {{ - ra, dec, ha, - zd1, az1, - zd_err, az_err, - dev - }}; - if (mjd>0) - UpdateTracking(Time(mjd), track); - - // ---- DIM ----> t1 as event time - // status1 - // mjd - // ra/dec/ha - // zd/az (nominal) - // zd/az (current) - // err(zd/az) - // [armed] [stgmd] - - // Maybe: - // POINTING_POSITION --> t1, zd/az (current), [armed, stgmd, status1] - // - // if (mjd>0) - // TRACKING_POSITION --> mjd, zd/az (nominal), err(zd/az) - // ra/dec, ha(not well defined), - // [Nominal + Error == Current] - - // MJD is the time which corresponds to the nominal position - // t1 is the time which corresponds to the current position/HA - + ProcessDriveReport(line.substr(16)); return; } @@ -1119,5 +1191,5 @@ Time fSunRise; - + /* int ShiftSunRise() { @@ -1134,5 +1206,5 @@ return Drive::State::kLocked; - } + }*/ int Execute() @@ -1145,4 +1217,5 @@ poll_one(); + /* if (T::GetCurrentState()==Drive::State::kLocked) return ShiftSunRise(); @@ -1152,5 +1225,25 @@ if (Time()>fSunRise) return Park(); - } + }*/ + + if (Time()>fSunRise) + { + if (T::GetCurrentState()>Drive::State::kLocked) + return Park(); + + if (T::GetCurrentState()==Drive::State::kLocked) + { + fSunRise = Time().GetNextSunRise(); + + ostringstream msg; + msg << "Next sun-rise will be at " << fSunRise; + T::Info(msg); + + return Drive::State::kLocked; + } + } + + if (T::GetCurrentState()==Drive::State::kLocked) + return Drive::State::kLocked; const int state = fDrive.GetState();