Add basic MFX support to arducontrol driver

A lot of polishing is still needed.  Locomotive discovery in particular
has several deficiencies.

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index e5d94ef9d5d180e0bf3f7f5a5cfdcd847f48ae77..7111f53012b5ec177e23f718dcf5fdc607286307 100644 (file)
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -11,23 +11,34 @@ namespace R2C2 {
 
 ArduControl::ProtocolInfo ArduControl::protocol_info[2] =
 {
-       { 79, 14, 4 }  // MM
+       { 79, 14, 4 },       // MM
+       { 0x3FFF, 126, 15 }  // MFX
 };
 
 ArduControl::ArduControl(const string &dev):
        serial(dev),
-       debug(true),
+       debug(1),
        power(false),
        next_refresh(refresh_cycle.end()),
        refresh_counter(0),
        active_accessory(0),
        n_s88_octets(0),
+       mfx_announce_serial(0),
+       next_mfx_address(1),
        thread(*this)
 {
        QueuedCommand cmd;
        cmd.command[0] = READ_POWER_STATE;
        cmd.length = 1;
        push_command(cmd);
+
+       cmd.command[0] = MFX_SET_STATION_ID;
+       cmd.command[1] = 'R';
+       cmd.command[2] = '2';
+       cmd.command[3] = 'C';
+       cmd.command[4] = '2';
+       cmd.length = 5;
+       push_command(cmd);
 }
 
 ArduControl::~ArduControl()
@@ -55,6 +66,8 @@ const char *ArduControl::enumerate_protocols(unsigned i) const
 {
        if(i==0)
                return "MM";
+       else if(i==1)
+               return "MFX";
        else
                return 0;
 }
@@ -132,7 +145,7 @@ void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
        unsigned mask = 1<<func;
        if(loco.funcs.set((loco.funcs&~mask)|(mask*state)))
        {
-               if(func>0)
+               if(func>0 || loco.proto!=MM)
                {
                        QueuedCommand cmd(loco, Locomotive::FUNCTIONS, func);
                        push_command(cmd);
@@ -492,6 +505,14 @@ unsigned ArduControl::Locomotive::create_speed_dir_command(char *buffer) const
                buffer[3] = speed.pending+reverse.pending*0x80;
                return 4;
        }
+       else if(proto==MFX)
+       {
+               buffer[0] = MFX_SPEED;
+               buffer[1] = address>>8;
+               buffer[2] = address;
+               buffer[3] = speed.pending+reverse.pending*0x80;
+               return 4;
+       }
        else
                return 0;
 }
@@ -509,6 +530,25 @@ unsigned ArduControl::Locomotive::create_speed_func_command(unsigned f, char *bu
                buffer[3] = speed.pending;
                return 4;
        }
+       else if(proto==MFX)
+       {
+               bool f16 = (funcs.pending>0xFF);
+               buffer[0] = (f16 ? MFX_SPEED_FUNCS16 : MFX_SPEED_FUNCS8);
+               buffer[1] = address>>8;
+               buffer[2] = address;
+               buffer[3] = speed.pending+reverse.pending*0x80;
+               if(f16)
+               {
+                       buffer[4] = funcs.pending>>8;
+                       buffer[5] = funcs.pending;
+                       return 6;
+               }
+               else
+               {
+                       buffer[4] = funcs.pending;
+                       return 5;
+               }
+       }
        else
                return 0;
 }
@@ -565,8 +605,16 @@ void ArduControl::ControlThread::main()
        Locomotive *loco = 0;
        unsigned phase = 0;
        unsigned s88_octets_remaining = 0;
+
+       Msp::Time::TimeStamp next_mfx_announce;
+       unsigned mfx_search_size = 0;
+       unsigned mfx_search_bits = 0;
+       Msp::Time::TimeStamp next_mfx_search;
+
        while(!done)
        {
+               Time::TimeStamp t = Time::now();
+
                if(!repeat_count)
                {
                        tag = Tag();
@@ -587,6 +635,25 @@ void ArduControl::ControlThread::main()
                                repeat_count = 2;
                                ++phase;
                        }
+                       else if(t>=next_mfx_announce)
+                       {
+                               command[0] = MFX_ANNOUNCE;
+                               command[1] = control.mfx_announce_serial>>8;
+                               command[2] = control.mfx_announce_serial;
+                               length = 3;
+                               next_mfx_announce = t+400*Time::msec;
+                       }
+                       else if(t>=next_mfx_search)
+                       {
+                               command[0] = MFX_SEARCH;
+                               for(unsigned i=0; i<4; ++i)
+                                       command[1+i] = mfx_search_bits>>(24-i*8);
+                               command[5] = mfx_search_size;
+                               length = 6;
+                               next_mfx_search = t+200*Time::msec;
+                               if(control.debug>=1)
+                                       IO::print("Search %08X/%d\n", mfx_search_bits, mfx_search_size);
+                       }
                        else if(!s88_octets_remaining && control.n_s88_octets)
                        {
                                command[0] = S88_READ;
@@ -596,9 +663,17 @@ void ArduControl::ControlThread::main()
                        }
                        else if((loco = control.get_loco_to_refresh()))
                        {
-                               length = loco->create_speed_dir_command(command);
-                               repeat_count = 2;
-                               phase = 0;
+                               if(loco->proto==MM)
+                               {
+                                       length = loco->create_speed_dir_command(command);
+                                       repeat_count = 2;
+                                       phase = 0;
+                               }
+                               else if(loco->proto==MFX)
+                               {
+                                       length = loco->create_speed_func_command(0, command);
+                                       phase = 1;
+                               }
                        }
                        else
                        {
@@ -611,7 +686,7 @@ void ArduControl::ControlThread::main()
                        }
                }
 
-               if(control.debug)
+               if(control.debug>=2)
                {
                        string cmd_hex;
                        for(unsigned i=0; i<length; ++i)
@@ -646,7 +721,7 @@ void ArduControl::ControlThread::main()
                                while(pos<rlength)
                                        pos += control.serial.read(reply+pos, rlength-pos);
 
-                               if(control.debug)
+                               if(control.debug>=2)
                                {
                                        string reply_hex;
                                        for(unsigned i=0; i<rlength; ++i)
@@ -673,6 +748,52 @@ void ArduControl::ControlThread::main()
                                        ptag.serial = control.power.serial;
                                        control.push_completed_tag(ptag);
                                }
+                               else if(type==MFX_FEEDBACK && rlength==2)
+                               {
+                                       if(command[0]==MFX_SEARCH)
+                                       {
+                                               bool finished = true;
+                                               if(reply[1])
+                                               {
+                                                       if(mfx_search_size==32)
+                                                       {
+                                                               if(control.debug>=1)
+                                                                       IO::print("Assigning MFX address %d to decoder %08X\n", control.next_mfx_address, mfx_search_bits);
+
+                                                               QueuedCommand qcmd;
+                                                               qcmd.command[0] = MFX_ASSIGN_ADDRESS;
+                                                               qcmd.command[1] = control.next_mfx_address>>8;
+                                                               qcmd.command[2] = control.next_mfx_address;
+                                                               for(unsigned i=0; i<4; ++i)
+                                                                       qcmd.command[3+i] = mfx_search_bits>>(24-i*8);
+                                                               qcmd.length = 7;
+                                                               control.push_command(qcmd);
+                                                               ++control.next_mfx_address;
+                                                       }
+                                                       else
+                                                       {
+                                                               ++mfx_search_size;
+                                                               finished = false;
+                                                       }
+                                               }
+                                               else if(mfx_search_size>0)
+                                               {
+                                                       unsigned mask = 1<<(32-mfx_search_size);
+                                                       if(!(mfx_search_bits&mask))
+                                                       {
+                                                               mfx_search_bits |= mask;
+                                                               finished = false;
+                                                       }
+                                               }
+
+                                               if(finished)
+                                               {
+                                                       next_mfx_search = t+2*Time::sec;
+                                                       mfx_search_bits = 0;
+                                                       mfx_search_size = 0;
+                                               }
+                                       }
+                               }
                                else if(type==S88_DATA && rlength>2)
                                {
                                        unsigned offset = static_cast<unsigned char>(reply[1]);

diff --git a/source/libr2c2/arducontrol.h b/source/libr2c2/arducontrol.h
index 4e9ef63b248c631821055aecf2ad0bc53ce5fbeb..3a86ceeacc09790bbfc1c7afa04ac6dab29093ff 100644 (file)
--- a/source/libr2c2/arducontrol.h
+++ b/source/libr2c2/arducontrol.h
@@ -26,6 +26,14 @@ private:
                MOTOROLA_SPEED_DIRECTION = 0x13,
                MOTOROLA_SPEED_FUNCTION = 0x14,
                MOTOROLA_SOLENOID = 0x15,
+               MFX_SET_STATION_ID = 0x21,
+               MFX_ANNOUNCE = 0x22,
+               MFX_SEARCH = 0x23,
+               MFX_ASSIGN_ADDRESS = 0x24,
+               MFX_PING = 0x25,
+               MFX_SPEED = 0x28,
+               MFX_SPEED_FUNCS8 = 0x29,
+               MFX_SPEED_FUNCS16 = 0x2A,
                S88_READ = 0x30,
                COMMAND_OK = 0x80,
                RECEIVE_OVERRUN = 0x81,
@@ -37,7 +45,8 @@ private:
                TRACK_CURRENT = 0xC0,
                INPUT_VOLTAGE = 0xC1,
                POWER_STATE = 0xC2,
-               S88_DATA = 0xD0
+               S88_DATA = 0xD0,
+               MFX_FEEDBACK = 0xD1
        };
 
        struct Tag
@@ -68,7 +77,8 @@ private:
 
        enum Protocol
        {
-               MM
+               MM,
+               MFX
        };
 
        struct ProtocolInfo
@@ -189,7 +199,7 @@ private:
        typedef std::map<unsigned, Sensor> SensorMap;
 
        Msp::IO::Serial serial;
-       bool debug;
+       unsigned debug;
 
        ControlledVariable<bool> power;
 
@@ -207,6 +217,9 @@ private:
        SensorMap sensors;
        unsigned n_s88_octets;
 
+       unsigned mfx_announce_serial;
+       unsigned next_mfx_address;
+
        Msp::Mutex mutex;
        ControlThread thread;