namespace R2C2 {
+ArduControl::ProtocolInfo ArduControl::protocol_info[2] =
+{
+ { 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()
void ArduControl::set_power(bool p)
{
- if(p==power.pending)
- return;
-
- power.pending = p;
- ++power.serial;
-
- QueuedCommand cmd(POWER);
- cmd.tag.serial = power.serial;
- cmd.command[0] = (p ? POWER_ON : POWER_OFF);
- cmd.length = 1;
- push_command(cmd);
+ if(power.set(p))
+ {
+ QueuedCommand cmd(POWER);
+ cmd.tag.serial = power.serial;
+ cmd.command[0] = (p ? POWER_ON : POWER_OFF);
+ cmd.length = 1;
+ push_command(cmd);
+ }
}
void ArduControl::halt(bool)
{
if(i==0)
return "MM";
+ else if(i==1)
+ return "MFX";
else
return 0;
}
{
if(proto_name=="MM")
return MM;
+ else if(proto_name=="MFX")
+ return MFX;
else
throw invalid_argument("ArduControl::map_protocol");
}
unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const
{
- Protocol proto = map_protocol(proto_name);
- if(proto==MM)
- return 14;
- else
- return 0;
+ return protocol_info[map_protocol(proto_name)].max_speed;
}
-void ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
+unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
{
if(!addr)
throw invalid_argument("ArduControl::add_loco");
Protocol proto = map_protocol(proto_name);
+ if(addr>protocol_info[proto].max_address)
+ throw invalid_argument("ArduControl::add_loco");
- if(proto==MM)
- {
- if(addr>=80)
- throw invalid_argument("ArduControl::add_loco");
- }
+ Locomotive loco(proto, addr);
+ insert_unique(locomotives, loco.id, loco);
- insert_unique(locomotives, addr, Locomotive(proto, addr));
+ return loco.id;
}
-void ArduControl::remove_loco(unsigned addr)
+void ArduControl::remove_loco(unsigned id)
{
- Locomotive &loco = get_item(locomotives, addr);
+ Locomotive &loco = get_item(locomotives, id);
remove_loco_from_refresh(loco);
- locomotives.erase(addr);
+ locomotives.erase(id);
}
-void ArduControl::set_loco_speed(unsigned addr, unsigned speed)
+void ArduControl::set_loco_speed(unsigned id, unsigned speed)
{
- Locomotive &loco = get_item(locomotives, addr);
+ Locomotive &loco = get_item(locomotives, id);
+ if(speed>protocol_info[loco.proto].max_speed)
+ throw invalid_argument("ArduControl::set_loco_speed");
+
if(loco.speed.set(speed))
{
QueuedCommand cmd(loco, Locomotive::SPEED);
}
}
-void ArduControl::set_loco_reverse(unsigned addr, bool rev)
+void ArduControl::set_loco_reverse(unsigned id, bool rev)
{
- Locomotive &loco = get_item(locomotives, addr);
+ Locomotive &loco = get_item(locomotives, id);
if(loco.reverse.set(rev))
{
QueuedCommand cmd(loco, Locomotive::REVERSE);
}
}
-void ArduControl::set_loco_function(unsigned addr, unsigned func, bool state)
+void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
{
- Locomotive &loco = get_item(locomotives, addr);
+ Locomotive &loco = get_item(locomotives, id);
+ if(func>protocol_info[loco.proto].max_func)
+ throw invalid_argument("ArduControl::set_loco_function");
+
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);
}
}
-void ArduControl::add_turnout(unsigned addr, const TrackType &type)
+unsigned ArduControl::add_turnout(unsigned addr, const TrackType &type)
{
if(!addr || !type.is_turnout())
throw invalid_argument("ArduControl::add_turnout");
- add_accessory(Accessory::TURNOUT, addr, type.get_state_bits());
+ return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits());
}
void ArduControl::remove_turnout(unsigned addr)
return get_accessory(Accessory::TURNOUT, addr);
}
-void ArduControl::add_signal(unsigned addr, const SignalType &)
+unsigned ArduControl::add_signal(unsigned addr, const SignalType &)
{
- add_accessory(Accessory::SIGNAL, addr, 1);
+ return add_accessory(Accessory::SIGNAL, addr, 1);
}
void ArduControl::remove_signal(unsigned addr)
return get_accessory(Accessory::SIGNAL, addr);
}
-void ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits)
+unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits)
{
AccessoryMap::iterator i = accessories.lower_bound(addr);
AccessoryMap::iterator j = accessories.upper_bound(addr+bits-1);
}
insert_unique(accessories, addr, Accessory(kind, addr, bits));
+ return addr;
}
void ArduControl::remove_accessory(Accessory::Kind kind, unsigned addr)
return acc.state;
}
-void ArduControl::add_sensor(unsigned addr)
+unsigned ArduControl::add_sensor(unsigned addr)
{
if(!addr)
throw invalid_argument("ArduControl::add_sensor");
unsigned octet_index = (addr-1)/8;
if(octet_index>=n_s88_octets)
n_s88_octets = octet_index+1;
+
+ return addr;
}
void ArduControl::remove_sensor(unsigned addr)
}
else if(tag.type==Tag::LOCOMOTIVE)
{
- LocomotiveMap::iterator i = locomotives.find(tag.address);
+ LocomotiveMap::iterator i = locomotives.find(tag.id);
if(i==locomotives.end())
continue;
if(tag.command==Locomotive::SPEED)
{
if(loco.speed.commit(tag.serial))
- signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
+ signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
}
else if(tag.command==Locomotive::REVERSE)
{
if(loco.reverse.commit(tag.serial))
- signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
+ signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
}
else if(tag.command==Locomotive::FUNCTIONS)
{
unsigned changed = old^loco.funcs;
for(unsigned j=0; changed>>j; ++j)
if((changed>>j)&1)
- signal_loco_function.emit(loco.address, j, (loco.funcs>>j)&1);
+ signal_loco_function.emit(loco.id, j, (loco.funcs>>j)&1);
}
}
}
else if(tag.type==Tag::ACCESSORY)
{
- AccessoryMap::iterator i = accessories.find(tag.address);
+ AccessoryMap::iterator i = accessories.find(tag.id);
if(i==accessories.end())
continue;
}
else if(tag.type==Tag::SENSOR)
{
- SensorMap::iterator i = sensors.find(tag.address);
+ SensorMap::iterator i = sensors.find(tag.id);
if(i==sensors.end())
continue;
type(NONE),
command(0),
serial(0),
- address(0)
+ id(0)
{ }
ArduControl::Locomotive::Locomotive(Protocol p, unsigned a):
+ id((p<<16)|a),
proto(p),
address(a),
speed(0),
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;
}
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;
}
throw invalid_argument("Accessory::create_state_command");
unsigned a = (address+b+3)*2;
- if((state.pending>>b)&1)
+ if(!((state.pending>>b)&1))
++a;
buffer[0] = MOTOROLA_SOLENOID;
buffer[1] = a>>3;
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();
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;
}
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
{
}
}
- if(control.debug)
+ if(control.debug>=2)
{
string cmd_hex;
for(unsigned i=0; i<length; ++i)
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)
else if(tag && !repeat_count)
control.push_completed_tag(tag);
}
+ else if(type==POWER_STATE && rlength==2)
+ {
+ control.power.set(reply[1]);
+
+ Tag ptag;
+ ptag.type = Tag::GENERAL;
+ ptag.command = POWER;
+ 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]);
stag.type = Tag::SENSOR;
stag.command = Sensor::STATE;
stag.serial = i->second.state.serial;
- stag.address = i->first;
+ stag.id = i->first;
control.push_completed_tag(stag);
}
{
tag.type = Tag::LOCOMOTIVE;
tag.command = cmd;
- tag.address = loco.address;
+ tag.id = loco.id;
if(cmd==Locomotive::SPEED)
{
tag.serial = loco.speed.serial;
{
tag.type = Tag::ACCESSORY;
tag.command = cmd;
- tag.address = acc.address;
+ tag.id = acc.address;
if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
{
tag.serial = acc.state.serial;
projects / r2c2.git / blobdiff