Index: branches/FACT++_lidctrl_usb/Makefile.am =================================================================== --- branches/FACT++_lidctrl_usb/Makefile.am (revision 18787) +++ branches/FACT++_lidctrl_usb/Makefile.am (revision 18788) @@ -40,4 +40,5 @@ mcp feedback ratescan ratecontrol \ tngweather lidctrl gpsctrl pfminictrl sqmctrl pwrctrl \ + lidctrl_usb \ magicweather magiclidar \ smartfact evtserver getevent \ @@ -121,4 +122,5 @@ gpsctrl.man \ lidctrl.man \ + lidctrl_usb.man \ magiclidar.man \ magicweather.man \ @@ -393,4 +395,9 @@ libConfiguration.la -lQtXml +lidctrl_usb_SOURCES = src/lidctrl_usb.cc src/LocalControl.h +lidctrl_usb_LDADD = libDim++.la libDim.la libDimExtension.la \ + libStateMachine.la libTools.la libTime.la \ + libConfiguration.la -lQtXml + gpsctrl_SOURCES = src/gpsctrl.cc src/LocalControl.h gpsctrl_LDADD = libDim++.la libDim.la libDimExtension.la \ Index: branches/FACT++_lidctrl_usb/Makefile.in =================================================================== --- branches/FACT++_lidctrl_usb/Makefile.in (revision 18787) +++ branches/FACT++_lidctrl_usb/Makefile.in (revision 18788) @@ -99,12 +99,13 @@ ratescan$(EXEEXT) ratecontrol$(EXEEXT) tngweather$(EXEEXT) \ lidctrl$(EXEEXT) gpsctrl$(EXEEXT) pfminictrl$(EXEEXT) \ - sqmctrl$(EXEEXT) pwrctrl$(EXEEXT) magicweather$(EXEEXT) \ - magiclidar$(EXEEXT) smartfact$(EXEEXT) evtserver$(EXEEXT) \ - getevent$(EXEEXT) temperature$(EXEEXT) timecheck$(EXEEXT) \ - showlog$(EXEEXT) fitsdump$(EXEEXT) fitscheck$(EXEEXT) \ - fitsselect$(EXEEXT) zfits$(EXEEXT) $(am__EXEEXT_1) \ - $(am__EXEEXT_2) $(am__EXEEXT_3) $(am__EXEEXT_4) \ - $(am__EXEEXT_5) $(am__EXEEXT_6) $(am__EXEEXT_7) \ - $(am__EXEEXT_8) $(am__EXEEXT_9) $(am__EXEEXT_10) + sqmctrl$(EXEEXT) pwrctrl$(EXEEXT) lidctrl_usb$(EXEEXT) \ + magicweather$(EXEEXT) magiclidar$(EXEEXT) smartfact$(EXEEXT) \ + evtserver$(EXEEXT) getevent$(EXEEXT) temperature$(EXEEXT) \ + timecheck$(EXEEXT) showlog$(EXEEXT) fitsdump$(EXEEXT) \ + fitscheck$(EXEEXT) fitsselect$(EXEEXT) zfits$(EXEEXT) \ + $(am__EXEEXT_1) $(am__EXEEXT_2) $(am__EXEEXT_3) \ + $(am__EXEEXT_4) $(am__EXEEXT_5) $(am__EXEEXT_6) \ + $(am__EXEEXT_7) $(am__EXEEXT_8) $(am__EXEEXT_9) \ + $(am__EXEEXT_10) @HAS_FITS_TRUE@am__append_1 = datalogger @HAS_V8_TRUE@am__append_2 = dimctrl dimserver @@ -466,4 +467,8 @@ lidctrl_DEPENDENCIES = libDim++.la libDim.la libDimExtension.la \ libStateMachine.la libTools.la libTime.la libConfiguration.la +am_lidctrl_usb_OBJECTS = src/lidctrl_usb.$(OBJEXT) +lidctrl_usb_OBJECTS = $(am_lidctrl_usb_OBJECTS) +lidctrl_usb_DEPENDENCIES = libDim++.la libDim.la libDimExtension.la \ + libStateMachine.la libTools.la libTime.la libConfiguration.la am_log_OBJECTS = src/log.$(OBJEXT) log_OBJECTS = $(am_log_OBJECTS) @@ -674,8 +679,9 @@ $(ftm_SOURCES) $(ftmctrl_SOURCES) $(gcn_SOURCES) \ $(getevent_SOURCES) $(gpsctrl_SOURCES) $(lidctrl_SOURCES) \ - $(log_SOURCES) $(logtime_SOURCES) $(magiclidar_SOURCES) \ - $(magicweather_SOURCES) $(makedata_SOURCES) \ - $(makeplots_SOURCES) $(makeschedule_SOURCES) $(mcp_SOURCES) \ - $(moon_SOURCES) $(pfminictrl_SOURCES) $(pwrctrl_SOURCES) \ + $(lidctrl_usb_SOURCES) $(log_SOURCES) $(logtime_SOURCES) \ + $(magiclidar_SOURCES) $(magicweather_SOURCES) \ + $(makedata_SOURCES) $(makeplots_SOURCES) \ + $(makeschedule_SOURCES) $(mcp_SOURCES) $(moon_SOURCES) \ + $(pfminictrl_SOURCES) $(pwrctrl_SOURCES) \ $(ratecontrol_SOURCES) $(ratescan_SOURCES) $(readfits_SOURCES) \ $(rootifysql_SOURCES) sched.c $(scheduler_SOURCES) \ @@ -701,9 +707,9 @@ $(fscctrl_SOURCES) $(ftm_SOURCES) $(ftmctrl_SOURCES) \ $(gcn_SOURCES) $(getevent_SOURCES) $(gpsctrl_SOURCES) \ - $(lidctrl_SOURCES) $(log_SOURCES) $(logtime_SOURCES) \ - $(magiclidar_SOURCES) $(magicweather_SOURCES) \ - $(makedata_SOURCES) $(makeplots_SOURCES) \ - $(makeschedule_SOURCES) $(mcp_SOURCES) $(moon_SOURCES) \ - $(pfminictrl_SOURCES) $(pwrctrl_SOURCES) \ + $(lidctrl_SOURCES) $(lidctrl_usb_SOURCES) $(log_SOURCES) \ + $(logtime_SOURCES) $(magiclidar_SOURCES) \ + $(magicweather_SOURCES) $(makedata_SOURCES) \ + $(makeplots_SOURCES) $(makeschedule_SOURCES) $(mcp_SOURCES) \ + $(moon_SOURCES) $(pfminictrl_SOURCES) $(pwrctrl_SOURCES) \ $(ratecontrol_SOURCES) $(ratescan_SOURCES) $(readfits_SOURCES) \ $(rootifysql_SOURCES) sched.c $(scheduler_SOURCES) \ @@ -1052,4 +1058,5 @@ @HAS_HELP2MAN_TRUE@ gpsctrl.man \ @HAS_HELP2MAN_TRUE@ lidctrl.man \ +@HAS_HELP2MAN_TRUE@ lidctrl_usb.man \ @HAS_HELP2MAN_TRUE@ magiclidar.man \ @HAS_HELP2MAN_TRUE@ magicweather.man \ @@ -1296,4 +1303,9 @@ libConfiguration.la -lQtXml +lidctrl_usb_SOURCES = src/lidctrl_usb.cc src/LocalControl.h +lidctrl_usb_LDADD = libDim++.la libDim.la libDimExtension.la \ + libStateMachine.la libTools.la libTime.la \ + libConfiguration.la -lQtXml + gpsctrl_SOURCES = src/gpsctrl.cc src/LocalControl.h gpsctrl_LDADD = libDim++.la libDim.la libDimExtension.la \ @@ -2204,4 +2216,10 @@ @rm -f lidctrl$(EXEEXT) $(AM_V_CXXLD)$(CXXLINK) $(lidctrl_OBJECTS) $(lidctrl_LDADD) $(LIBS) +src/lidctrl_usb.$(OBJEXT): src/$(am__dirstamp) \ + src/$(DEPDIR)/$(am__dirstamp) + +lidctrl_usb$(EXEEXT): $(lidctrl_usb_OBJECTS) $(lidctrl_usb_DEPENDENCIES) $(EXTRA_lidctrl_usb_DEPENDENCIES) + @rm -f lidctrl_usb$(EXEEXT) + $(AM_V_CXXLD)$(CXXLINK) $(lidctrl_usb_OBJECTS) $(lidctrl_usb_LDADD) $(LIBS) src/log.$(OBJEXT): src/$(am__dirstamp) src/$(DEPDIR)/$(am__dirstamp) @@ -2663,4 +2681,5 @@ @AMDEP_TRUE@@am__include@ @am__quote@src/$(DEPDIR)/gpsctrl.Po@am__quote@ @AMDEP_TRUE@@am__include@ @am__quote@src/$(DEPDIR)/lidctrl.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@src/$(DEPDIR)/lidctrl_usb.Po@am__quote@ @AMDEP_TRUE@@am__include@ @am__quote@src/$(DEPDIR)/log.Po@am__quote@ @AMDEP_TRUE@@am__include@ @am__quote@src/$(DEPDIR)/logtime.Po@am__quote@ Index: branches/FACT++_lidctrl_usb/biasctrl.rc =================================================================== --- branches/FACT++_lidctrl_usb/biasctrl.rc (revision 18787) +++ branches/FACT++_lidctrl_usb/biasctrl.rc (revision 18788) @@ -1,3 +1,3 @@ -database=readpo:readc0nf1g@10.0.100.21/programoptions +database=readpo:readc0nf1g@database/programoptions console=2 quiet=yes Index: branches/FACT++_lidctrl_usb/ftmctrl.rc =================================================================== --- branches/FACT++_lidctrl_usb/ftmctrl.rc (revision 18787) +++ branches/FACT++_lidctrl_usb/ftmctrl.rc (revision 18788) @@ -1,1 +1,1 @@ -database=readpo:readc0nf1g@10.0.100.21/programoptions +database=readpo:readc0nf1g@database/programoptions Index: branches/FACT++_lidctrl_usb/src/lidctrl_usb.cc =================================================================== --- branches/FACT++_lidctrl_usb/src/lidctrl_usb.cc (revision 18788) +++ branches/FACT++_lidctrl_usb/src/lidctrl_usb.cc (revision 18788) @@ -0,0 +1,326 @@ +#include +#include +#include + +#include "Dim.h" +#include "Event.h" +#include "Shell.h" +#include "StateMachineDim.h" +#include "StateMachineAsio.h" +#include "ConnectionUSB.h" +#include "Configuration.h" +#include "Console.h" + +#include "tools.h" + +#include "LocalControl.h" +#include "HeadersLid.h" + +namespace ba = boost::asio; +namespace bs = boost::system; +namespace dummy = ba::placeholders; + +using namespace std::placeholders; +using namespace std; + +using namespace Lid; + +#include "DimDescriptionService.h" + +class ConnectionLid : public ConnectionUSB +{ +public: + vector fRecievedBytes; + vector fCollectedBytes; + State::states_t fStateOfArduino; + State::states_t fLastStateOfArduino; + + DimDescribedService fDim; + message_t fLastMessage; + boost::asio::deadline_timer fReadTimer; + + void PrintMessage(const message_t msg) + { + Out() << "LastMessage:" << endl; + Out() << " OuterMotor:" << endl; + Out() << " Speed: " << msg.outer_motor_speed << endl; + Out() << " Current: " << msg.outer_motor_current.mean << endl; + Out() << " Position: " << msg.outer_motor_position.mean << endl; + Out() << " InnerMotor:" << endl; + Out() << " Speed: " << msg.inner_motor_speed << endl; + Out() << " Current: " << msg.inner_motor_current.mean << endl; + Out() << " Position: " << msg.inner_motor_position.mean << endl; + Out() << " LastCommand: " << msg.current_cmd << endl; + Out() << " State: " << (int)msg.system_state << endl; + } + + void CollectBytes(size_t bytes_received) + { + // append fRecievedBytes to fCollectedBytes + std::copy( + fRecievedBytes.begin(), + fRecievedBytes.begin() + bytes_received, + std::back_inserter(fCollectedBytes) + ); + } + + void TruncateUntilStartFlagFound() + { + // search for the 1st occurence of "start:" in the whole fCollectedBytes + const string search_string("start:"); + string s(fCollectedBytes.begin(), fCollectedBytes.end()); + int pos=s.find(search_string); + if (pos != 0){ + Out() << "TruncateUntilStartFlagFound: pos=" << pos << endl; + } + // cut off the first 'pos' bytes from fCollectedBytes + vector( + fCollectedBytes.begin() + pos, + fCollectedBytes.end() + ).swap(fCollectedBytes); + } + + complete_message_t + PopCompleteMessage(){ + + complete_message_t tmp; + memcpy(&tmp, fCollectedBytes.data(), sizeof(complete_message_t)); + // cut off the bytes we just copied into tmp + std::vector( + fCollectedBytes.begin() + sizeof(complete_message_t), + fCollectedBytes.end() + ).swap(fCollectedBytes); + return tmp; + } + + void UpdateDimService(const message_t& msg){ + dim_service_t update; + + update.inner_current_mean = (float)(msg.inner_motor_current.mean) * 3.4; + update.outer_current_mean = (float)(msg.outer_motor_current.mean) * 3.4; + update.inner_position_mean = (float)(msg.inner_motor_position.mean); + update.outer_position_mean = (float)(msg.outer_motor_position.mean); + update.inner_speed = (int32_t)msg.inner_motor_speed; + update.outer_speed = (int32_t)msg.outer_motor_speed; + + // update only if some interesting happened, i.e. + // - if the fStateOfArduino is not one of the steady states: kOpen or kclosed + // - if the fStateOfArduino just changed. + if ( (fLastStateOfArduino != fStateOfArduino) || !((fStateOfArduino == State::kOpen) || (fStateOfArduino == State::kClosed)) ) + { + fDim.Update(update); + } + fLastStateOfArduino = fStateOfArduino; + } + + void HandleReceivedData(const boost::system::error_code&, size_t bytes_received) + { + CollectBytes(bytes_received); + if (fCollectedBytes.size() > 2*sizeof(complete_message_t)) + { + TruncateUntilStartFlagFound(); + complete_message_t complete_message = PopCompleteMessage(); + message_t msg = complete_message.msg; + + if (Tools::Fletcher16(&msg, 1) == complete_message.checksum) + { + fLastMessage = msg; + fStateOfArduino = static_cast(msg.system_state + 1); + UpdateDimService(msg); + } + } + ConnectionEstablished(); + } + + void DoAsyncRead(){ + async_read( + *this, + boost::asio::buffer(fRecievedBytes, 2 * sizeof(complete_message_t)), + boost::bind( + &ConnectionLid::HandleReceivedData, + this, + dummy::error, + dummy::bytes_transferred + ) + ); + } + + void ConnectionEstablished() + { + fReadTimer.expires_from_now(boost::posix_time::milliseconds(50)); + fReadTimer.async_wait(boost::bind(&ConnectionLid::DoAsyncRead, this)); + } + + ConnectionLid(ba::io_service& ioservice, MessageImp &imp) : + ConnectionUSB(ioservice, imp()), + fRecievedBytes(2 * sizeof(complete_message_t)), + fCollectedBytes(0), + fStateOfArduino(State::kOpen), + fLastStateOfArduino(State::kOpen), + fDim("LID_CONTROL/MOTORS", "F:1;F:1;F:1;F:1;I:1;I:1", + "|inner_current_mean[mA]: " + "|outer_current_mean[mA]: " + "|inner_position_mean[LSB]: " + "|outer_position_mean[LSB]: " + "|inner_speed[LSB]: " + "|outer_speed[LSB]: "), + fLastMessage(), + fReadTimer(ioservice) + { + SetLogStream(&imp); + } + + State::states_t GetStatus() + { + return fStateOfArduino; + } +}; + +// ------------------------------------------------------------------------ + +template +class StateMachineLid : public StateMachineAsio +{ + int Wrap(boost::function f) + { + f(); + return T::GetCurrentState(); + } + + function Wrapper(function func) + { + return bind(&StateMachineLid::Wrap, this, func); + } + + bool CheckEventSize(size_t has, const char *name, size_t size) + { + if (has==size) + return true; + + ostringstream msg; + msg << name << " - Received event has " << has << " bytes, but expected " << size << "."; + T::Fatal(msg); + return false; + } + +private: + S fLid; + + int Execute() + { + return fLid.GetStatus(); + } + + int Open() + { + std::array cmd = {{'o'}}; + fLid.PostMessage(cmd); + return T::GetCurrentState(); + } + + int Close() + { + std::array cmd = {{'c'}}; + fLid.PostMessage(cmd); + return T::GetCurrentState(); + } + + int Print() + { + fLid.PrintMessage(fLid.fLastMessage); + return T::GetCurrentState(); + } + +public: + StateMachineLid(ostream &out=cout) : + StateMachineAsio(out, "LID_CONTROL"), fLid(*this, *this) + { + T::AddStateName(State::kOpen, "Open", "Lid is open"); + T::AddStateName(State::kClosed, "Closed", "Lid is closed"); + T::AddStateName(State::kUpperOpening, "UpperOpening", "upper(right) lid is opening"); + T::AddStateName(State::kHalfOpen, "HalfOpen", "upper(right) lid open, lower(left) lid closed"); + T::AddStateName(State::kLowerOpening, "LowerOpening", "lower(left) lid is opening"); + T::AddStateName(State::kLowerClosing, "LowerClosing", "lower(left) lid is closing"); + T::AddStateName(State::kUpperClosing, "UpperClosing", "upper(right) lid is closing"); + T::AddStateName(State::kUnknown, "Unknown", "Position unknown - arduino just restarted"); + + T::AddEvent("OPEN")(bind(&StateMachineLid::Open, this))("Open Lid"); + T::AddEvent("CLOSE")(bind(&StateMachineLid::Close, this))("Close Lid"); + T::AddEvent("PRINT")(bind(&StateMachineLid::Print, this))("Print the last message"); + } + + ~StateMachineLid() + { + } + + int EvalOptions(Configuration &conf) + { + if (conf.Has("dev")) + { + fLid.SetEndpoint(conf.Get("dev")); + T::Message("Setting device to "+fLid.URL()); + fLid.Connect(); + } + return -1; + } +}; + +// ------------------------------------------------------------------------ + +#include "Main.h" + +template +int RunShell(Configuration &conf) +{ + return Main::execute>(conf); +} + +void SetupConfiguration(Configuration &conf) +{ + po::options_description control("Lid control options"); + control.add_options() + ("no-dim,d", po_bool(), "Disable dim services") + ("dev", var("lid-arduino"), "Device address of USB port to lid-arduino") + ("quiet,q", po_bool(true), "Disable printing contents of all received messages (except dynamic data) in clear text.") + ; + + conf.AddOptions(control); +} + +void PrintUsage() +{ + cout << + "This lidctrl_usb program controls the Lid.\n" + "Usage: lidctrl_usb [-c type] [OPTIONS]\n" + " or: biaslidctrl_usbctrl [OPTIONS]\n"; + cout << endl; +} + +void PrintHelp() +{ + Main::PrintHelp>(); +} + +int main(int argc, const char* argv[]) +{ + Configuration conf(argv[0]); + conf.SetPrintUsage(PrintUsage); + Main::SetupConfiguration(conf); + SetupConfiguration(conf); + + if (!conf.DoParse(argc, argv, PrintHelp)) + return 127; + + { + // No console access at all + if (!conf.Has("console")) + { + return RunShell(conf); + } + if (conf.Get("console")==0) + return RunShell(conf); + else + return RunShell(conf); + } + return 0; +}