{ 0x3FFF, 126, 15 } // MFX
};
-Driver::TelemetryInfo ArduControl::telemetry_info[4] =
+Driver::TelemetryInfo ArduControl::telemetry_info[6] =
{
{ "voltage", "Voltage", "V", 1 },
{ "current", "Current", "A", 2 },
+ { "cmd-rate", "Cmd rate", "/ s", 0 },
{ "cmd-queue-depth", "Cmd queue", "", 0 },
+ { "acc-queue-depth", "Acc queue", "", 0 },
{ "s88-latency", "S88 latency", "ms", 0 }
};
return i;
}
+ArduControl::MfxInfo *ArduControl::find_mfx_info(unsigned id)
+{
+ for(MfxInfoArray::iterator i=mfx_info.begin(); i!=mfx_info.end(); ++i)
+ if(i->id==id)
+ return &*i;
+ return 0;
+}
+
void ArduControl::remove_loco(unsigned id)
{
Locomotive &loco = get_item(locomotives, id);
const Driver::TelemetryInfo *ArduControl::enumerate_telemetry(unsigned i) const
{
- if(i<4)
+ if(i<6)
return telemetry_info+i;
else
return 0;
else if(name==telemetry_info[1].name)
return monitor.get_current();
else if(name==telemetry_info[2].name)
- return command_queue.size();
+ return thread.get_command_rate();
else if(name==telemetry_info[3].name)
+ return command_queue.size();
+ else if(name==telemetry_info[4].name)
+ return accessory_queue.size();
+ else if(name==telemetry_info[5].name)
return s88.get_latency()/Time::msec;
else
throw key_error(name);
next_address(1),
size(0),
bits(0),
- misses(0)
+ misses(0),
+ pending_info(0),
+ read_array(0),
+ read_offset(0),
+ read_length(0),
+ block_size(0)
{ }
bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
{
+ if(read_length>0)
+ {
+ cmd.command[0] = MFX_READ;
+ cmd.command[1] = pending_info->address>>8;
+ cmd.command[2] = pending_info->address;
+ unsigned index = read_array*0x40+read_offset;
+ cmd.command[3] = index>>8;
+ cmd.command[4] = index;
+ unsigned length = (read_length>=4 ? 4 : read_length>=2 ? 2 : 1);
+ cmd.command[5] = length;
+ cmd.length = 6;
+
+ sleep(100*Time::msec);
+
+ return true;
+ }
+ else if(pending_info)
+ {
+ queue.push(*pending_info);
+ Tag tag;
+ tag.type = Tag::GENERAL;
+ tag.command = NEW_LOCO;
+ tag.id = pending_info->id;
+ control.completed_commands.push(tag);
+
+ if(control.debug>=1)
+ IO::print("Completed processing locomotive %s at address %d\n", pending_info->name, pending_info->address);
+
+ delete pending_info;
+ pending_info = 0;
+ }
+
if(size>32)
{
+ unsigned address = 0;
+ if(MfxInfo *existing = control.find_mfx_info(bits))
+ address = existing->address;
+ else
+ address = next_address++;
+
if(control.debug>=1)
- IO::print("Assigning MFX address %d to decoder %08X\n", next_address, bits);
+ IO::print("Assigning MFX address %d to decoder %08X\n", address, bits);
- MfxInfo info;
- info.protocol = "MFX";
- info.address = next_address;
- info.name = format("%08X", bits);
- info.id = bits;
- queue.push(info);
+ pending_info = new MfxInfo;
+ pending_info->protocol = "MFX";
+ pending_info->address = address;
+ pending_info->name = format("%08X", bits);
+ pending_info->id = bits;
cmd.command[0] = MFX_ASSIGN_ADDRESS;
- cmd.command[1] = next_address>>8;
- cmd.command[2] = next_address;
+ cmd.command[1] = address>>8;
+ cmd.command[2] = address;
for(unsigned i=0; i<4; ++i)
cmd.command[3+i] = bits>>(24-i*8);
cmd.length = 7;
- cmd.tag.type = Tag::GENERAL;
- cmd.tag.command = NEW_LOCO;
- cmd.tag.id = bits;
-
size = 0;
bits = 0;
- ++next_address;
+ misses = 0;
+
+ read_array = 0;
+ read_offset = 0;
+ read_length = 6;
return true;
}
cmd.command[5] = size;
cmd.length = 6;
- sleep(200*Time::msec);
+ sleep(100*Time::msec);
if(control.debug>=1)
IO::print("Search %08X/%d\n", bits, size);
misses = 0;
}
}
+ else if(type==MFX_READ_FEEDBACK && length>=3)
+ {
+ if(reply[1])
+ {
+ misses = 0;
+
+ for(unsigned i=2; i<length; ++i)
+ read_data[read_offset+i-2] = reply[i];
+ read_offset += length-2;
+ read_length -= length-2;
+
+ if(!read_length)
+ {
+ if(read_array==0)
+ block_size = static_cast<unsigned char>(read_data[4])*static_cast<unsigned char>(read_data[5]);
+
+ bool array_handled = false;
+ if(read_data[0]==0x18)
+ {
+ for(unsigned i=1; i<read_offset; ++i)
+ if(!read_data[i])
+ {
+ pending_info->name = string(read_data+1, i-1);
+ array_handled = true;
+ break;
+ }
+
+ if(!array_handled)
+ read_length = 4;
+ }
+ else
+ array_handled = true;
+
+ if(array_handled && control.debug>=1)
+ {
+ IO::print("MFX CA %03X:", read_array);
+ for(unsigned i=0; i<read_offset; ++i)
+ IO::print(" %02X", static_cast<unsigned char>(read_data[i]));
+ IO::print("\n");
+ }
+
+ if(array_handled && read_array<block_size)
+ {
+ ++read_array;
+ read_offset = 0;
+ read_length = 1;
+ }
+ }
+ }
+ else
+ {
+ ++misses;
+ if(misses>=10)
+ {
+ if(control.debug>=1)
+ IO::print("Failed to read MFX configuration from %d\n", pending_info->address);
+ read_length = 0;
+ }
+ }
+ }
}
void ArduControl::MfxSearchTask::set_next_address(unsigned a)
ArduControl::ControlThread::ControlThread(ArduControl &c):
control(c),
- done(false)
+ done(false),
+ cmd_rate(20),
+ cmd_count(0)
{
tasks.push_back(&control.command_queue);
tasks.push_back(&control.monitor);
void ArduControl::ControlThread::main()
{
init_baud_rate();
+ cmd_rate_start = Time::now();
while(!done)
{
control.command_queue.push(cmd);
}
}
+
+ if(cmd_count>=cmd_rate)
+ {
+ Time::TimeStamp t = Time::now();
+ cmd_rate = cmd_count/((t-cmd_rate_start)/Time::sec);
+ cmd_rate_start = t;
+ cmd_count = 0;
+ }
}
else
Time::sleep(10*Time::msec);
result = r;
}
+ ++cmd_count;
+
return result;
}
projects / r2c2.git / blobdiff