Changeset 18726 for branches


Ignore:
Timestamp:
01/10/17 14:25:45 (8 years ago)
Author:
(none)
Message:
tidy up
Location:
branches/FACT++_lidctrl_usb/src
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • branches/FACT++_lidctrl_usb/src/HeadersLid.h

    r18725 r18726  
    55{
    66
    7     struct motor_status_t{
    8         float current_mean;
    9         float position_mean;
    10         int32_t speed;
     7    // this describes the single dim service this server is emitting.
     8    struct dim_service_t{
     9        float inner_current_mean;
     10        float outer_current_mean;
     11
     12        float inner_position_mean;
     13        float outer_position_mean;
     14
     15        int32_t inner_speed;
     16        int32_t outer_speed;
    1117    } __attribute__((__packed__));
    1218
     19    // The following two structures describe the message as it is sent
     20    // from the arduino via USB to the PC.
    1321    struct mean_var_samples_t {
    1422        uint32_t mean;
    … …  
    3139    }__attribute__((packed));
    3240
     41    struct complete_message_t {
     42        char start_flag[6];  // the string "start:"
     43        message_t msg;
     44        uint16_t checksum;
     45    }__attribute__((packed));
     46
     47
    3348    namespace State
    3449    {

  • branches/FACT++_lidctrl_usb/src/lidctrl.cc

    r18725 r18726  
    2828#include "DimDescriptionService.h"
    2929
    30 
    3130class ConnectionLid : public ConnectionUSB
    3231{
    … …  
    3433    vector<char> fRecievedBytes;
    3534    vector<char> fCollectedBytes;
    36     State::states_t state;
    37     State::states_t last_state;
    38 
    39     DimDescribedService fDimInnerMotor;
    40     DimDescribedService fDimOuterMotor;
     35    State::states_t fStateOfArduino;
     36    State::states_t fLastStateOfArduino;
     37
     38    DimDescribedService fDim;
    4139    message_t fLastMessage;
    4240
    … …  
    5654    }
    5755
     56    void CollectBytes(size_t bytes_received)
     57    {
     58        // append fRecievedBytes to fCollectedBytes
     59        std::copy(
     60            fRecievedBytes.begin(),
     61            fRecievedBytes.begin() + bytes_received,
     62            std::back_inserter(fCollectedBytes)
     63        );
     64    }
     65
     66    void TruncateUntilStartFlagFound()
     67    {
     68        // search for the 1st occurence of "start:" in the whole fCollectedBytes
     69        const string search_string("start:");
     70        string s(fCollectedBytes.begin(), fCollectedBytes.end());
     71        int pos=s.find(search_string);
     72        // cut off the first 'pos' bytes from fCollectedBytes
     73        vector<decltype(fCollectedBytes)::value_type>(
     74            fCollectedBytes.begin() + pos,
     75            fCollectedBytes.end()
     76        ).swap(fCollectedBytes);
     77    }
     78
     79    complete_message_t
     80    PopCompleteMessage(){
     81
     82        complete_message_t tmp;
     83        memcpy(&tmp, fCollectedBytes.data(), sizeof(complete_message_t));
     84        // cut off the bytes we just copied into tmp
     85        std::vector<decltype(fCollectedBytes)::value_type>(
     86            fCollectedBytes.begin() + sizeof(complete_message_t),
     87            fCollectedBytes.end()
     88        ).swap(fCollectedBytes);
     89        return tmp
     90    }
     91
     92    void UpdateDimService(const message_t& msg){
     93        dim_service_t update;
     94
     95        update.inner_current_mean = (float)(msg.inner_motor_current.mean) * 3.4;
     96        update.outer_current_mean = (float)(msg.outer_motor_current.mean) * 3.4;
     97        update.inner_position_mean = (float)(msg.inner_motor_position.mean);
     98        update.outer_position_mean = (float)(msg.outer_motor_position.mean);
     99        update.inner_speed = (int32_t)msg.inner_motor_speed;
     100        update.outer_speed = (int32_t)msg.outer_motor_speed;
     101
     102        // update only if some interesting happened, i.e.
     103        //  - if the fStateOfArduino is not one of the steady states: kOpen or kclosed
     104        //  - if the fStateOfArduino just changed.
     105        if ( (fLastStateOfArduino != fStateOfArduino) || !((fStateOfArduino == State::kOpen) || (fStateOfArduino == State::kClosed)) )
     106        {
     107            fDim.Update(update);
     108        }
     109        fLastStateOfArduino = fStateOfArduino;
     110    }
     111
    58112    void HandleReceivedData(const boost::system::error_code&, size_t bytes_received, int bar, int foo)
    59113    {
    60         int complete_msg_size = 6 /*start:*/ + sizeof(message_t) + sizeof(uint16_t)/*chksum*/;  // 54 bytes
    61 
    62         std::copy(fRecievedBytes.begin(), fRecievedBytes.begin()+bytes_received, std::back_inserter(fCollectedBytes));
    63         if (fCollectedBytes.size() > 2*complete_msg_size)
     114        CollectBytes(size_t bytes_received);
     115        if (fCollectedBytes.size() > 2*sizeof(complete_message_t))
    64116        {
    65             string s(fCollectedBytes.begin(), fCollectedBytes.end());
    66             string search_string("start:");
    67             int pos=s.find(search_string);
    68             std::vector<decltype(fCollectedBytes)::value_type>(fCollectedBytes.begin()+pos, fCollectedBytes.end()).swap(fCollectedBytes);
    69             string complete_message(fCollectedBytes.begin(), fCollectedBytes.begin()+complete_msg_size);
    70             std::vector<decltype(fCollectedBytes)::value_type>(fCollectedBytes.begin()+complete_msg_size, fCollectedBytes.end()).swap(fCollectedBytes);
    71             string part = s.substr(6, sizeof(message_t));
    72             message_t msg;
    73             memcpy(&msg, part.data(), sizeof(message_t));
    74 
    75             const uint16_t chk0 = Tools::Fletcher16(part.c_str(), sizeof(message_t));
    76             uint16_t chk1;
    77             part = s.substr(6+sizeof(message_t), sizeof(uint16_t));
    78             memcpy(&chk1, part.data(), sizeof(uint16_t));
    79             if (chk0==chk1)
     117            TruncateUntilStartFlagFound();
     118            complete_message_t complete_message = PopCompleteMessage();
     119            message_t msg = complete_message.msg;
     120
     121            if (Tools::Fletcher16(&msg, 1) == complete_message.checksum)
    80122            {
    81123                fLastMessage = msg;
    82                 state = static_cast<State::states_t>(msg.system_state + 1);
    83                 part = s.substr(6, sizeof(message_t)-1); // -1: omit the state
    84 
    85                 motor_status_t inner_motor_status;
    86                 inner_motor_status.current_mean = (float)(msg.inner_motor_current.mean) * 3.4;
    87                 inner_motor_status.position_mean = (float)(msg.inner_motor_position.mean);
    88                 inner_motor_status.speed = (int32_t)msg.inner_motor_speed;
    89 
    90                 motor_status_t outer_motor_status;
    91                 outer_motor_status.current_mean = (float)(msg.outer_motor_current.mean) * 3.4;
    92                 outer_motor_status.position_mean = (float)(msg.outer_motor_position.mean);
    93                 outer_motor_status.speed = (int32_t)msg.outer_motor_speed;
    94                 if ( (last_state != state) || !((state == State::kOpen) || (state == State::kClosed)) )
    95                 {
    96                     fDimInnerMotor.Update(inner_motor_status);
    97                     fDimOuterMotor.Update(outer_motor_status);
    98                 }
    99 
    100                 last_state = state;
     124                fStateOfArduino = static_cast<State::states_t>(msg.system_state + 1);
     125                UpdateDimService(msg);
    101126            }
    102127        }
    103 
    104         AsyncRead(ba::buffer(fRecievedBytes, 128), 0, 0);
     128        AsyncRead(ba::buffer(fRecievedBytes, 2 * sizeof(complete_message_t)), 0, 0);
    105129    }
    106130
    107131    void ConnectionEstablished()
    108132    {
    109         AsyncRead(ba::buffer(fRecievedBytes, 128), 0, 0);
     133        AsyncRead(ba::buffer(fRecievedBytes, 2 * sizeof(complete_message_t)), 0, 0);
    110134    }
    111135
    112136    ConnectionLid(ba::io_service& ioservice, MessageImp &imp) :
    113137        ConnectionUSB(ioservice, imp()),
    114         fRecievedBytes(128),
     138        fRecievedBytes(2 * sizeof(complete_message_t)),
    115139        fCollectedBytes(0),
    116         state(State::kOpen),
    117         fDimInnerMotor("LID_CONTROL/INNER_MOTOR", "F:1;F:1;I:1",
    118             "|current_mean[mA]: "
    119             "|position_mean[LSB]: "
    120             "|speed[LSB]: "),
    121         fDimOuterMotor("LID_CONTROL/OUTER_MOTOR", "F:1;F:1;I:1",
    122             "|current_mean[mA]: "
    123             "|position_mean[LSB]: "
    124             "|speed[LSB]: "),
    125        fLastMessage()
    126 
     140        fStateOfArduino(State::kOpen),
     141        fLastStateOfArduino(State::kOpen),
     142        fDim("LID_CONTROL/INNER_MOTOR", "F:1;F:1;F:1;F:1;I:1;I:1",
     143            "|inner_current_mean[mA]: "
     144            "|outer_current_mean[mA]: "
     145            "|inner_position_mean[LSB]: "
     146            "|outer_position_mean[LSB]: "
     147            "|inner_speed[LSB]: "
     148            "|outer_speed[LSB]: "),
     149        fLastMessage()
    127150    {
    128151        SetLogStream(&imp);
    … …  
    131154    State::states_t GetStatus()
    132155    {
    133         return state;
     156        return fStateOfArduino;
    134157    }
    135158};
    … …  
    169192        return fLid.GetStatus();
    170193    }
    171 
    172194
    173195    int Open()
Note: See TracChangeset for help on using the changeset viewer.