Index: branches/FACT++_lidctrl_usb/src/HeadersLid.h =================================================================== --- branches/FACT++_lidctrl_usb/src/HeadersLid.h (revision 18725) +++ branches/FACT++_lidctrl_usb/src/HeadersLid.h (revision 18726) @@ -5,10 +5,18 @@ { - struct motor_status_t{ - float current_mean; - float position_mean; - int32_t speed; + // this describes the single dim service this server is emitting. + struct dim_service_t{ + float inner_current_mean; + float outer_current_mean; + + float inner_position_mean; + float outer_position_mean; + + int32_t inner_speed; + int32_t outer_speed; } __attribute__((__packed__)); + // The following two structures describe the message as it is sent + // from the arduino via USB to the PC. struct mean_var_samples_t { uint32_t mean; @@ -31,4 +39,11 @@ }__attribute__((packed)); + struct complete_message_t { + char start_flag[6]; // the string "start:" + message_t msg; + uint16_t checksum; + }__attribute__((packed)); + + namespace State { Index: branches/FACT++_lidctrl_usb/src/lidctrl.cc =================================================================== --- branches/FACT++_lidctrl_usb/src/lidctrl.cc (revision 18725) +++ branches/FACT++_lidctrl_usb/src/lidctrl.cc (revision 18726) @@ -28,5 +28,4 @@ #include "DimDescriptionService.h" - class ConnectionLid : public ConnectionUSB { @@ -34,9 +33,8 @@ vector fRecievedBytes; vector fCollectedBytes; - State::states_t state; - State::states_t last_state; - - DimDescribedService fDimInnerMotor; - DimDescribedService fDimOuterMotor; + State::states_t fStateOfArduino; + State::states_t fLastStateOfArduino; + + DimDescribedService fDim; message_t fLastMessage; @@ -56,73 +54,98 @@ } + 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); + // 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, int bar, int foo) { - int complete_msg_size = 6 /*start:*/ + sizeof(message_t) + sizeof(uint16_t)/*chksum*/; // 54 bytes - - std::copy(fRecievedBytes.begin(), fRecievedBytes.begin()+bytes_received, std::back_inserter(fCollectedBytes)); - if (fCollectedBytes.size() > 2*complete_msg_size) + CollectBytes(size_t bytes_received); + if (fCollectedBytes.size() > 2*sizeof(complete_message_t)) { - string s(fCollectedBytes.begin(), fCollectedBytes.end()); - string search_string("start:"); - int pos=s.find(search_string); - std::vector(fCollectedBytes.begin()+pos, fCollectedBytes.end()).swap(fCollectedBytes); - string complete_message(fCollectedBytes.begin(), fCollectedBytes.begin()+complete_msg_size); - std::vector(fCollectedBytes.begin()+complete_msg_size, fCollectedBytes.end()).swap(fCollectedBytes); - string part = s.substr(6, sizeof(message_t)); - message_t msg; - memcpy(&msg, part.data(), sizeof(message_t)); - - const uint16_t chk0 = Tools::Fletcher16(part.c_str(), sizeof(message_t)); - uint16_t chk1; - part = s.substr(6+sizeof(message_t), sizeof(uint16_t)); - memcpy(&chk1, part.data(), sizeof(uint16_t)); - if (chk0==chk1) + TruncateUntilStartFlagFound(); + complete_message_t complete_message = PopCompleteMessage(); + message_t msg = complete_message.msg; + + if (Tools::Fletcher16(&msg, 1) == complete_message.checksum) { fLastMessage = msg; - state = static_cast(msg.system_state + 1); - part = s.substr(6, sizeof(message_t)-1); // -1: omit the state - - motor_status_t inner_motor_status; - inner_motor_status.current_mean = (float)(msg.inner_motor_current.mean) * 3.4; - inner_motor_status.position_mean = (float)(msg.inner_motor_position.mean); - inner_motor_status.speed = (int32_t)msg.inner_motor_speed; - - motor_status_t outer_motor_status; - outer_motor_status.current_mean = (float)(msg.outer_motor_current.mean) * 3.4; - outer_motor_status.position_mean = (float)(msg.outer_motor_position.mean); - outer_motor_status.speed = (int32_t)msg.outer_motor_speed; - if ( (last_state != state) || !((state == State::kOpen) || (state == State::kClosed)) ) - { - fDimInnerMotor.Update(inner_motor_status); - fDimOuterMotor.Update(outer_motor_status); - } - - last_state = state; + fStateOfArduino = static_cast(msg.system_state + 1); + UpdateDimService(msg); } } - - AsyncRead(ba::buffer(fRecievedBytes, 128), 0, 0); + AsyncRead(ba::buffer(fRecievedBytes, 2 * sizeof(complete_message_t)), 0, 0); } void ConnectionEstablished() { - AsyncRead(ba::buffer(fRecievedBytes, 128), 0, 0); + AsyncRead(ba::buffer(fRecievedBytes, 2 * sizeof(complete_message_t)), 0, 0); } ConnectionLid(ba::io_service& ioservice, MessageImp &imp) : ConnectionUSB(ioservice, imp()), - fRecievedBytes(128), + fRecievedBytes(2 * sizeof(complete_message_t)), fCollectedBytes(0), - state(State::kOpen), - fDimInnerMotor("LID_CONTROL/INNER_MOTOR", "F:1;F:1;I:1", - "|current_mean[mA]: " - "|position_mean[LSB]: " - "|speed[LSB]: "), - fDimOuterMotor("LID_CONTROL/OUTER_MOTOR", "F:1;F:1;I:1", - "|current_mean[mA]: " - "|position_mean[LSB]: " - "|speed[LSB]: "), - fLastMessage() - + fStateOfArduino(State::kOpen), + fLastStateOfArduino(State::kOpen), + fDim("LID_CONTROL/INNER_MOTOR", "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() { SetLogStream(&imp); @@ -131,5 +154,5 @@ State::states_t GetStatus() { - return state; + return fStateOfArduino; } }; @@ -169,5 +192,4 @@ return fLid.GetStatus(); } - int Open()