1 #include <msp/core/maputils.h>
2 #include <msp/datafile/writer.h>
3 #include <msp/fs/redirectedpath.h>
4 #include <msp/fs/stat.h>
5 #include <msp/io/print.h>
6 #include <msp/time/utils.h>
7 #include "arducontrol.h"
15 ArduControl::ProtocolInfo ArduControl::protocol_info[2] =
18 { 0x3FFF, 126, 15 } // MFX
21 Driver::TelemetryInfo ArduControl::telemetry_info[4] =
23 { "voltage", "Voltage", "V", 1 },
24 { "current", "Current", "A", 2 },
25 { "cmd-queue-depth", "Cmd queue", "", 0 },
26 { "s88-latency", "S88 latency", "ms", 0 }
29 ArduControl::ArduControl(const Options &opts):
30 serial(opts.get<string>(string(), "ttyUSB0")),
31 debug(opts.get<unsigned>("debug")),
32 state_file("arducontrol.state"),
36 command_timeout(200*Time::msec),
41 if(FS::exists(state_file))
42 DataFile::load(*this, state_file.str());
44 unsigned max_address = 0;
45 for(MfxInfoArray::const_iterator i=mfx_info.begin(); i!=mfx_info.end(); ++i)
46 max_address = max(max_address, i->address);
47 mfx_search.set_next_address(max_address+1);
50 cmd.command[0] = READ_POWER_STATE;
52 command_queue.push(cmd);
54 cmd.command[0] = MFX_SET_STATION_ID;
60 command_queue.push(cmd);
63 ArduControl::~ArduControl()
68 void ArduControl::set_power(bool p)
72 PendingCommand cmd(POWER);
73 cmd.tag.serial = power.serial;
74 cmd.command[0] = (p ? POWER_ON : POWER_OFF);
76 command_queue.push(cmd);
80 void ArduControl::halt(bool h)
88 for(LocomotiveMap::const_iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
89 set_loco_speed(i->first, 0);
92 signal_halt.emit(halted);
95 const char *ArduControl::enumerate_protocols(unsigned i) const
105 ArduControl::Protocol ArduControl::map_protocol(const string &proto_name)
109 else if(proto_name=="MFX")
112 throw invalid_argument("ArduControl::map_protocol");
115 unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const
117 return protocol_info[map_protocol(proto_name)].max_speed;
120 const Driver::DetectedLocomotive *ArduControl::enumerate_detected_locos(unsigned i) const
122 if(i>=mfx_info.size())
128 unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
131 throw invalid_argument("ArduControl::add_loco");
133 Protocol proto = map_protocol(proto_name);
134 if(addr>protocol_info[proto].max_address)
135 throw invalid_argument("ArduControl::add_loco");
137 Locomotive loco(proto, addr);
138 insert_unique(locomotives, loco.id, loco);
143 ArduControl::MfxInfoArray::iterator ArduControl::add_mfx_info(const MfxInfo &info)
145 MfxInfoArray::iterator i;
146 for(i=mfx_info.begin(); (i!=mfx_info.end() && i->id!=info.id); ++i) ;
147 if(i==mfx_info.end())
149 mfx_info.push_back(info);
150 i = --mfx_info.end();
157 ArduControl::MfxInfo *ArduControl::find_mfx_info(unsigned id)
159 for(MfxInfoArray::iterator i=mfx_info.begin(); i!=mfx_info.end(); ++i)
165 void ArduControl::remove_loco(unsigned id)
167 Locomotive &loco = get_item(locomotives, id);
168 refresh.remove_loco(loco);
169 locomotives.erase(id);
172 void ArduControl::set_loco_speed(unsigned id, unsigned speed)
174 Locomotive &loco = get_item(locomotives, id);
175 if(speed>protocol_info[loco.proto].max_speed)
176 throw invalid_argument("ArduControl::set_loco_speed");
181 if(loco.speed.set(speed))
183 PendingCommand cmd(loco, Locomotive::SPEED);
184 command_queue.push(cmd);
186 refresh.add_loco(loco);
190 void ArduControl::set_loco_reverse(unsigned id, bool rev)
192 Locomotive &loco = get_item(locomotives, id);
193 if(loco.reverse.set(rev))
195 PendingCommand cmd(loco, Locomotive::REVERSE);
196 command_queue.push(cmd);
198 refresh.add_loco(loco);
202 void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
204 Locomotive &loco = get_item(locomotives, id);
205 if(func>protocol_info[loco.proto].max_func)
206 throw invalid_argument("ArduControl::set_loco_function");
208 unsigned mask = 1<<func;
209 if(loco.funcs.set((loco.funcs&~mask)|(mask*state)))
211 if(func>0 || loco.proto!=MM)
213 PendingCommand cmd(loco, Locomotive::FUNCTIONS, func);
214 command_queue.push(cmd);
217 refresh.add_loco(loco);
221 unsigned ArduControl::add_turnout(unsigned addr, const TrackType &type)
223 if(!addr || !type.is_turnout())
224 throw invalid_argument("ArduControl::add_turnout");
226 return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits(), type.get_paths());
229 void ArduControl::remove_turnout(unsigned addr)
231 remove_accessory(Accessory::TURNOUT, addr);
234 void ArduControl::set_turnout(unsigned addr, unsigned state)
236 set_accessory(Accessory::TURNOUT, addr, state);
239 unsigned ArduControl::get_turnout(unsigned addr) const
241 return get_accessory(Accessory::TURNOUT, addr);
244 unsigned ArduControl::add_signal(unsigned addr, const SignalType &)
246 return add_accessory(Accessory::SIGNAL, addr, 1, 3);
249 void ArduControl::remove_signal(unsigned addr)
251 remove_accessory(Accessory::SIGNAL, addr);
254 void ArduControl::set_signal(unsigned addr, unsigned state)
256 set_accessory(Accessory::SIGNAL, addr, state);
259 unsigned ArduControl::get_signal(unsigned addr) const
261 return get_accessory(Accessory::SIGNAL, addr);
264 unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits, unsigned states)
266 AccessoryMap::iterator i = accessories.lower_bound(addr);
267 AccessoryMap::iterator j = accessories.upper_bound(addr+bits-1);
269 throw key_error(addr);
270 if(i!=accessories.begin())
273 if(i->first+i->second.bits>addr)
274 throw key_error(addr);
277 insert_unique(accessories, addr, Accessory(kind, addr, bits, states));
281 void ArduControl::remove_accessory(Accessory::Kind kind, unsigned addr)
283 Accessory &acc = get_item(accessories, addr);
285 throw key_error(addr);
286 accessories.erase(addr);
289 void ArduControl::set_accessory(Accessory::Kind kind, unsigned addr, unsigned state)
291 Accessory &acc = get_item(accessories, addr);
293 throw key_error(addr);
295 if(state!=acc.target || acc.uncertain)
298 accessory_queue.push_back(&acc);
302 unsigned ArduControl::get_accessory(Accessory::Kind kind, unsigned addr) const
304 const Accessory &acc = get_item(accessories, addr);
306 throw key_error(addr);
310 void ArduControl::activate_accessory_by_mask(Accessory &acc, unsigned mask)
312 unsigned bit = mask&~(mask-1);
313 for(active_index=0; (bit>>active_index)>1; ++active_index) ;
314 acc.state.set((acc.state&~bit)|(acc.target&bit));
316 IO::print("Setting accessory %d bit %d, state=%d\n", acc.address, active_index, acc.state.pending);
317 PendingCommand cmd(acc, Accessory::ACTIVATE, active_index);
318 command_queue.push(cmd);
319 active_accessory = &acc;
321 monitor.reset_peak();
324 unsigned ArduControl::add_sensor(unsigned addr)
327 throw invalid_argument("ArduControl::add_sensor");
329 insert_unique(sensors, addr, Sensor(addr));
330 s88.grow_n_octets((addr+7)/8);
335 void ArduControl::remove_sensor(unsigned addr)
337 remove_existing(sensors, addr);
338 // TODO update s88.n_octets
341 bool ArduControl::get_sensor(unsigned addr) const
343 return get_item(sensors, addr).state;
346 const Driver::TelemetryInfo *ArduControl::enumerate_telemetry(unsigned i) const
349 return telemetry_info+i;
354 float ArduControl::get_telemetry_value(const string &name) const
356 if(name==telemetry_info[0].name)
357 return monitor.get_voltage();
358 else if(name==telemetry_info[1].name)
359 return monitor.get_current();
360 else if(name==telemetry_info[2].name)
361 return command_queue.size();
362 else if(name==telemetry_info[3].name)
363 return s88.get_latency()/Time::msec;
365 throw key_error(name);
368 void ArduControl::tick()
371 while(completed_commands.pop(tag))
373 if(tag.type==Tag::GENERAL)
375 if(tag.command==POWER)
377 if(power.commit(tag.serial))
378 signal_power.emit(power.current);
380 else if(tag.command==NEW_LOCO)
383 if(mfx_search.pop_info(info))
385 MfxInfoArray::iterator i = add_mfx_info(info);
387 signal_locomotive_detected.emit(*i);
391 else if(tag.type==Tag::LOCOMOTIVE)
393 LocomotiveMap::iterator i = locomotives.find(tag.id);
394 if(i==locomotives.end())
397 Locomotive &loco = i->second;
398 if(tag.command==Locomotive::SPEED)
400 if(loco.speed.commit(tag.serial))
401 signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
403 else if(tag.command==Locomotive::REVERSE)
405 if(loco.reverse.commit(tag.serial))
406 signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
408 else if(tag.command==Locomotive::FUNCTIONS)
410 unsigned old = loco.funcs;
411 if(loco.funcs.commit(tag.serial))
413 unsigned changed = old^loco.funcs;
414 for(unsigned j=0; changed>>j; ++j)
416 signal_loco_function.emit(loco.id, j, (loco.funcs>>j)&1);
420 else if(tag.type==Tag::ACCESSORY)
422 AccessoryMap::iterator i = accessories.find(tag.id);
423 if(i==accessories.end())
426 Accessory &acc = i->second;
427 if(tag.command==Accessory::ACTIVATE)
428 off_timeout = Time::now()+acc.active_time;
429 else if(tag.command==Accessory::DEACTIVATE)
431 if(acc.state.commit(tag.serial))
433 if(&acc==active_accessory)
434 active_accessory = 0;
438 else if(tag.type==Tag::SENSOR)
440 SensorMap::iterator i = sensors.find(tag.id);
444 Sensor &sensor = i->second;
445 if(tag.command==Sensor::STATE)
447 if(sensor.state.commit(tag.serial))
448 signal_sensor.emit(sensor.address, sensor.state);
453 while(power && !active_accessory && !accessory_queue.empty())
455 Accessory &acc = *accessory_queue.front();
459 unsigned zeroes = acc.uncertain&~acc.target;
461 activate_accessory_by_mask(acc, zeroes);
463 activate_accessory_by_mask(acc, acc.uncertain);
465 else if(acc.state!=acc.target)
467 unsigned changes = acc.state^acc.target;
468 if(!(changes&((1<<acc.bits)-1)))
470 // All remaining changes are in non-physical bits
471 acc.state.set(acc.state^changes);
472 acc.state.commit(acc.state.serial);
476 unsigned toggle_bit = 0;
477 for(unsigned bit=1; (!toggle_bit && bit<=changes); bit<<=1)
478 if((changes&bit) && (acc.valid_states&(1<<(acc.state^bit))))
481 activate_accessory_by_mask(acc, toggle_bit);
486 accessory_queue.pop_front();
488 if(acc.state==acc.target)
490 if(acc.kind==Accessory::TURNOUT)
491 signal_turnout.emit(acc.address, acc.state);
492 else if(acc.kind==Accessory::SIGNAL)
493 signal_signal.emit(acc.address, acc.state);
498 if(active_accessory && off_timeout)
500 bool success = (monitor.get_peak()>0.35f && monitor.get_current()<monitor.get_peak()-0.2f);
501 Time::TimeStamp t = Time::now();
502 if(t>off_timeout || success)
504 Accessory &acc = *active_accessory;
506 unsigned bit = 1<<active_index;
508 // Assume success if we were uncertain of the physical setting
509 if(acc.uncertain&bit)
510 acc.uncertain &= ~bit;
511 else if(acc.kind==Accessory::TURNOUT && !success)
514 IO::print("Peak current only %.2f A\n", monitor.get_peak());
515 signal_turnout_failed.emit(acc.address);
516 acc.state.rollback();
517 if(acc.valid_states&(1<<(acc.target^bit)))
520 acc.target = acc.state;
523 off_timeout = Time::TimeStamp();
524 PendingCommand cmd(acc, Accessory::DEACTIVATE, active_index);
525 command_queue.push(cmd);
530 void ArduControl::flush()
532 while(!command_queue.empty() || (power && !accessory_queue.empty()))
536 void ArduControl::save_state() const
538 FS::RedirectedPath tmp_file(state_file);
539 IO::BufferedFile out(tmp_file.str(), IO::M_WRITE);
540 DataFile::Writer writer(out);
542 writer.write((DataFile::Statement("mfx_announce_serial"), mfx_announce.get_serial()));
543 for(MfxInfoArray::const_iterator i=mfx_info.begin(); i!=mfx_info.end(); ++i)
545 DataFile::Statement st("mfx_locomotive");
547 st.sub.push_back((DataFile::Statement("address"), i->address));
548 st.sub.push_back((DataFile::Statement("name"), i->name));
554 ArduControl::Tag::Tag():
562 ArduControl::Locomotive::Locomotive(Protocol p, unsigned a):
572 unsigned ArduControl::Locomotive::create_speed_dir_command(char *buffer) const
576 buffer[0] = MOTOROLA_SPEED_DIRECTION;
578 buffer[2] = funcs.pending&1;
579 buffer[3] = speed.pending+reverse.pending*0x80;
584 buffer[0] = MFX_SPEED;
585 buffer[1] = address>>8;
587 buffer[3] = speed.pending+reverse.pending*0x80;
594 unsigned ArduControl::Locomotive::create_speed_func_command(unsigned f, char *buffer) const
599 throw invalid_argument("Locomotive::create_speed_func_command");
601 buffer[0] = MOTOROLA_SPEED_FUNCTION;
603 buffer[2] = (f<<4)|(((funcs.pending>>f)&1)<<1)|(funcs.pending&1);
604 buffer[3] = speed.pending;
609 bool f16 = (funcs.pending>0xFF);
610 buffer[0] = (f16 ? MFX_SPEED_FUNCS16 : MFX_SPEED_FUNCS8);
611 buffer[1] = address>>8;
613 buffer[3] = speed.pending+reverse.pending*0x80;
616 buffer[4] = funcs.pending>>8;
617 buffer[5] = funcs.pending;
622 buffer[4] = funcs.pending;
631 ArduControl::Accessory::Accessory(Kind k, unsigned a, unsigned b, unsigned s):
637 uncertain((1<<bits)-1),
639 active_time((bits*700)*Time::msec)
642 unsigned ArduControl::Accessory::create_state_command(unsigned b, bool c, char *buffer) const
645 throw invalid_argument("Accessory::create_state_command");
647 unsigned a = (address+b+3)*2;
648 if(!((state.pending>>b)&1))
650 buffer[0] = MOTOROLA_SOLENOID;
652 buffer[2] = ((a&7)<<4)|c;
657 ArduControl::Sensor::Sensor(unsigned a):
663 ArduControl::PendingCommand::PendingCommand():
668 ArduControl::PendingCommand::PendingCommand(GeneralCommand cmd):
672 tag.type = Tag::GENERAL;
676 ArduControl::PendingCommand::PendingCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index):
679 tag.type = Tag::LOCOMOTIVE;
682 if(cmd==Locomotive::SPEED)
684 tag.serial = loco.speed.serial;
685 length = loco.create_speed_dir_command(command);
687 else if(cmd==Locomotive::REVERSE)
689 tag.serial = loco.reverse.serial;
690 length = loco.create_speed_dir_command(command);
692 else if(cmd==Locomotive::FUNCTIONS)
694 tag.serial = loco.funcs.serial;
695 length = loco.create_speed_func_command(index, command);
698 throw invalid_argument("PendingCommand");
701 ArduControl::PendingCommand::PendingCommand(Accessory &acc, Accessory::Command cmd, unsigned index):
704 tag.type = Tag::ACCESSORY;
706 tag.id = acc.address;
707 if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
709 tag.serial = acc.state.serial;
710 length = acc.create_state_command(index, (cmd==Accessory::ACTIVATE), command);
713 throw invalid_argument("PendingCommand");
718 void ArduControl::Queue<T>::push(const T &item)
720 MutexLock lock(mutex);
721 items.push_back(item);
725 bool ArduControl::Queue<T>::pop(T &item)
727 MutexLock lock(mutex);
731 item = items.front();
737 unsigned ArduControl::Queue<T>::size() const
743 bool ArduControl::Queue<T>::empty() const
745 return items.empty();
749 bool ArduControl::CommandQueueTask::get_work(PendingCommand &cmd)
751 return queue.pop(cmd);
754 void ArduControl::CommandQueueTask::push(const PendingCommand &cmd)
760 ArduControl::Task::Task(const string &n, unsigned p):
765 void ArduControl::Task::sleep(const Time::TimeDelta &dt)
767 sleep_timeout = Time::now()+dt;
771 ArduControl::CommandQueueTask::CommandQueueTask():
776 ArduControl::RefreshTask::RefreshTask():
784 bool ArduControl::RefreshTask::get_work(PendingCommand &cmd)
786 if(loco && loco->proto==MM && phase==0)
788 cmd.length = loco->create_speed_func_command(round%4+1, cmd.command);
789 cmd.repeat_count = 2;
794 loco = get_next_loco();
801 cmd.length = loco->create_speed_dir_command(cmd.command);
802 cmd.repeat_count = 2;
804 else if(loco->proto==MFX)
805 cmd.length = loco->create_speed_func_command(0, cmd.command);
812 void ArduControl::RefreshTask::add_loco(Locomotive &l)
814 MutexLock lock(mutex);
818 LocomotivePtrList::iterator oldest = cycle.begin();
819 for(LocomotivePtrList::iterator i=cycle.begin(); ++i!=cycle.end(); )
820 if((*i)->last_change_age>(*oldest)->last_change_age)
826 if(next==cycle.end())
827 next = cycle.begin();
830 void ArduControl::RefreshTask::remove_loco(Locomotive &l)
832 MutexLock lock(mutex);
833 for(LocomotivePtrList::iterator i=cycle.begin(); i!=cycle.end(); ++i)
848 ArduControl::Locomotive *ArduControl::RefreshTask::get_next_loco()
850 MutexLock lock(mutex);
854 Locomotive *l = *next;
859 void ArduControl::RefreshTask::advance()
862 if(next==cycle.end())
864 next = cycle.begin();
870 ArduControl::S88Task::S88Task(ArduControl &c):
877 bool ArduControl::S88Task::get_work(PendingCommand &cmd)
879 if(octets_remaining || !n_octets)
882 Time::TimeStamp t = Time::now();
884 latency = t-last_poll;
887 octets_remaining = n_octets;
888 cmd.command[0] = S88_READ;
889 cmd.command[1] = octets_remaining;
892 sleep(100*Time::msec);
897 void ArduControl::S88Task::process_reply(const char *reply, unsigned length)
899 unsigned char type = reply[0];
900 if(type==S88_DATA && length>2)
902 unsigned offset = static_cast<unsigned char>(reply[1]);
903 unsigned count = length-2;
905 SensorMap::iterator begin = control.sensors.lower_bound(offset*8+1);
906 SensorMap::iterator end = control.sensors.upper_bound((offset+count)*8);
907 for(SensorMap::iterator i=begin; i!=end; ++i)
909 unsigned bit_index = i->first-1-offset*8;
910 bool state = (reply[2+bit_index/8]>>(7-bit_index%8))&1;
911 i->second.state.set(state);
914 tag.type = Tag::SENSOR;
915 tag.command = Sensor::STATE;
916 tag.serial = i->second.state.serial;
918 control.completed_commands.push(tag);
921 if(count>octets_remaining)
922 octets_remaining = 0;
924 octets_remaining -= count;
928 void ArduControl::S88Task::set_n_octets(unsigned n)
933 void ArduControl::S88Task::grow_n_octets(unsigned n)
940 ArduControl::MfxAnnounceTask::MfxAnnounceTask():
941 Task("MfxAnnounce", 1),
945 bool ArduControl::MfxAnnounceTask::get_work(PendingCommand &cmd)
947 cmd.command[0] = MFX_ANNOUNCE;
948 cmd.command[1] = serial>>8;
949 cmd.command[2] = serial;
952 sleep(400*Time::msec);
957 void ArduControl::MfxAnnounceTask::set_serial(unsigned s)
963 ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c):
964 Task("MfxSearch", 1),
977 bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
981 cmd.command[0] = MFX_READ;
982 cmd.command[1] = pending_info->address>>8;
983 cmd.command[2] = pending_info->address;
984 unsigned index = read_array*0x40+read_offset;
985 cmd.command[3] = index>>8;
986 cmd.command[4] = index;
987 unsigned length = (read_length>=4 ? 4 : read_length>=2 ? 2 : 1);
988 cmd.command[5] = length;
991 sleep(100*Time::msec);
995 else if(pending_info)
997 queue.push(*pending_info);
999 tag.type = Tag::GENERAL;
1000 tag.command = NEW_LOCO;
1001 tag.id = pending_info->id;
1002 control.completed_commands.push(tag);
1004 if(control.debug>=1)
1005 IO::print("Completed processing locomotive %s at address %d\n", pending_info->name, pending_info->address);
1007 delete pending_info;
1013 unsigned address = 0;
1014 if(MfxInfo *existing = control.find_mfx_info(bits))
1015 address = existing->address;
1017 address = next_address++;
1019 if(control.debug>=1)
1020 IO::print("Assigning MFX address %d to decoder %08X\n", address, bits);
1022 pending_info = new MfxInfo;
1023 pending_info->protocol = "MFX";
1024 pending_info->address = address;
1025 pending_info->name = format("%08X", bits);
1026 pending_info->id = bits;
1028 cmd.command[0] = MFX_ASSIGN_ADDRESS;
1029 cmd.command[1] = address>>8;
1030 cmd.command[2] = address;
1031 for(unsigned i=0; i<4; ++i)
1032 cmd.command[3+i] = bits>>(24-i*8);
1046 cmd.command[0] = MFX_SEARCH;
1047 for(unsigned i=0; i<4; ++i)
1048 cmd.command[1+i] = bits>>(24-i*8);
1049 cmd.command[5] = size;
1052 sleep(100*Time::msec);
1054 if(control.debug>=1)
1055 IO::print("Search %08X/%d\n", bits, size);
1060 void ArduControl::MfxSearchTask::process_reply(const char *reply, unsigned length)
1062 unsigned char type = reply[0];
1063 if(type==MFX_SEARCH_FEEDBACK && length==2)
1070 else if(size>0 && misses<6)
1073 bits ^= 1<<(32-size);
1083 else if(type==MFX_READ_FEEDBACK && length>=3)
1089 for(unsigned i=2; i<length; ++i)
1090 read_data[read_offset+i-2] = reply[i];
1091 read_offset += length-2;
1092 read_length -= length-2;
1097 block_size = static_cast<unsigned char>(read_data[4])*static_cast<unsigned char>(read_data[5]);
1099 bool array_handled = false;
1100 if(read_data[0]==0x18)
1102 for(unsigned i=1; i<read_offset; ++i)
1105 pending_info->name = string(read_data+1, i-1);
1106 array_handled = true;
1114 array_handled = true;
1116 if(array_handled && control.debug>=1)
1118 IO::print("MFX CA %03X:", read_array);
1119 for(unsigned i=0; i<read_offset; ++i)
1120 IO::print(" %02X", static_cast<unsigned char>(read_data[i]));
1124 if(array_handled && read_array<block_size)
1137 if(control.debug>=1)
1138 IO::print("Failed to read MFX configuration from %d\n", pending_info->address);
1145 void ArduControl::MfxSearchTask::set_next_address(unsigned a)
1150 bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info)
1152 return queue.pop(info);
1156 ArduControl::MonitorTask::MonitorTask():
1165 bool ArduControl::MonitorTask::get_work(PendingCommand &cmd)
1168 cmd.command[0] = READ_INPUT_VOLTAGE;
1170 cmd.command[0] = READ_TRACK_CURRENT;
1173 sleep(200*Time::msec);
1174 next_type = (next_type+1)%5;
1179 void ArduControl::MonitorTask::process_reply(const char *reply, unsigned length)
1181 unsigned char type = reply[0];
1182 if(type==INPUT_VOLTAGE && length==3)
1183 voltage = ((static_cast<unsigned char>(reply[1])<<8) | static_cast<unsigned char>(reply[2]))/1000.0f;
1184 else if(type==TRACK_CURRENT && length==5)
1186 current = ((static_cast<unsigned char>(reply[1])<<8) | static_cast<unsigned char>(reply[2]))/1000.0f;
1187 float peak = ((static_cast<unsigned char>(reply[3])<<8) | static_cast<unsigned char>(reply[4]))/1000.0f;
1188 peak_level = max(peak_level, peak);
1189 base_level = min(base_level, current);
1193 void ArduControl::MonitorTask::reset_peak()
1195 base_level = current;
1196 peak_level = current;
1200 ArduControl::ControlThread::ControlThread(ArduControl &c):
1204 tasks.push_back(&control.command_queue);
1205 tasks.push_back(&control.monitor);
1206 tasks.push_back(&control.mfx_announce);
1207 tasks.push_back(&control.mfx_search);
1208 tasks.push_back(&control.s88);
1209 tasks.push_back(&control.refresh);
1214 void ArduControl::ControlThread::exit()
1220 void ArduControl::ControlThread::main()
1229 bool success = true;
1230 bool resync = false;
1231 for(unsigned i=0; (success && i<cmd.repeat_count); ++i)
1233 unsigned result = do_command(cmd, control.command_timeout);
1234 success = (result==COMMAND_OK);
1235 resync = (result==0);
1238 if(success && cmd.tag)
1239 control.completed_commands.push(cmd.tag);
1243 if(control.debug>=1)
1244 IO::print("Synchronization with ArduControl lost, attempting to recover\n");
1245 for(unsigned i=0; (resync && i<16); ++i)
1247 control.serial.put('\xFF');
1248 while(IO::poll(control.serial, IO::P_INPUT, control.command_timeout))
1249 resync = (control.serial.get()!=0xFF);
1253 if(control.debug>=1)
1254 IO::print("Resynchronization failed, giving up\n");
1259 if(control.debug>=1)
1260 IO::print("Resynchronization successful\n");
1262 control.command_queue.push(cmd);
1267 Time::sleep(10*Time::msec);
1271 void ArduControl::ControlThread::init_baud_rate()
1273 static unsigned rates[] = { 57600, 9600, 19200, 38400, 0 };
1275 control.serial.set_data_bits(8);
1276 control.serial.set_parity(IO::Serial::NONE);
1277 control.serial.set_stop_bits(1);
1278 for(unsigned i=0; rates[i]; ++i)
1280 control.serial.set_baud_rate(rates[i]);
1281 control.serial.put('\xFF');
1282 if(IO::poll(control.serial, IO::P_INPUT, 500*Time::msec))
1284 int c = control.serial.get();
1295 if(control.debug>=1)
1296 IO::print("ArduControl detection failed\n");
1301 if(control.debug>=1)
1302 IO::print("ArduControl detected at %d bits/s\n", rate);
1307 cmd.command[0] = SET_BAUD_RATE;
1308 cmd.command[1] = rates[0]>>8;
1309 cmd.command[2] = rates[0];
1311 if(do_command(cmd, Time::sec)==COMMAND_OK)
1313 control.serial.set_baud_rate(rates[0]);
1314 Time::sleep(Time::sec);
1315 if(do_command(cmd, Time::sec)==COMMAND_OK)
1317 if(control.debug>=1)
1318 IO::print("Rate changed to %d bits/s\n", rates[0]);
1324 bool ArduControl::ControlThread::get_work(PendingCommand &cmd)
1326 Time::TimeStamp t = Time::now();
1329 for(; (count<tasks.size() && tasks[count]->get_sleep_timeout()<=t); ++count) ;
1331 for(; count>0; --count)
1334 for(unsigned j=1; j<count; ++j)
1335 if(tasks[j]->get_priority()<tasks[i]->get_priority())
1338 Task *task = tasks[i];
1339 bool result = task->get_work(cmd);
1341 Time::TimeStamp st = max(task->get_sleep_timeout(), t);
1342 for(; (i+1<tasks.size() && tasks[i+1]->get_sleep_timeout()<=st); ++i)
1343 tasks[i] = tasks[i+1];
1348 if(control.debug>=2)
1349 IO::print("Scheduled task %s\n", task->get_name());
1354 // As fallback, send an idle packet for the MM protocol
1355 cmd.command[0] = MOTOROLA_SPEED;
1356 cmd.command[1] = 80;
1364 unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd, const Time::TimeDelta &timeout)
1366 if(control.debug>=2)
1369 for(unsigned i=0; i<cmd.length; ++i)
1370 cmd_hex += format(" %02X", static_cast<unsigned char>(cmd.command[i]));
1371 IO::print("< %02X%s\n", cmd.length^0xFF, cmd_hex);
1374 control.serial.put(cmd.length^0xFF);
1375 control.serial.write(cmd.command, cmd.length);
1377 unsigned result = 0;
1382 got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero);
1384 got_data = IO::poll(control.serial, IO::P_INPUT, timeout);
1389 unsigned rlength = control.serial.get()^0xFF;
1392 IO::print("Invalid length %02X\n", rlength);
1400 if(!IO::poll(control.serial, IO::P_INPUT, timeout))
1402 pos += control.serial.read(reply+pos, rlength-pos);
1405 if(control.debug>=2)
1408 for(unsigned i=0; i<rlength; ++i)
1409 reply_hex += format(" %02X", static_cast<unsigned char>(reply[i]));
1410 IO::print("> %02X%s\n", rlength^0xFF, reply_hex);
1413 unsigned r = process_reply(reply, rlength);
1421 unsigned ArduControl::ControlThread::process_reply(const char *reply, unsigned rlength)
1423 unsigned char type = reply[0];
1424 if((type&0xE0)==0x80)
1426 if(type!=COMMAND_OK)
1427 IO::print("Error %02X\n", type);
1430 else if(type==POWER_STATE && rlength==2)
1431 set_power(reply[1]);
1432 else if(type==OVERCURRENT)
1435 IO::print("Overcurrent detected!\n");
1439 for(vector<Task *>::iterator i=tasks.begin(); i!=tasks.end(); ++i)
1440 (*i)->process_reply(reply, rlength);
1446 void ArduControl::ControlThread::set_power(bool p)
1448 control.power.set(p);
1451 tag.type = Tag::GENERAL;
1452 tag.command = POWER;
1453 tag.serial = control.power.serial;
1454 control.completed_commands.push(tag);
1458 ArduControl::Loader::Loader(ArduControl &c):
1459 DataFile::ObjectLoader<ArduControl>(c)
1461 add("mfx_announce_serial", &Loader::mfx_announce_serial);
1462 add("mfx_locomotive", &Loader::mfx_locomotive);
1465 void ArduControl::Loader::mfx_announce_serial(unsigned s)
1467 obj.mfx_announce.set_serial(s);
1470 void ArduControl::Loader::mfx_locomotive(unsigned id)
1474 info.protocol = "MFX";
1476 obj.add_mfx_info(info);
1480 ArduControl::MfxInfo::Loader::Loader(MfxInfo &i):
1481 DataFile::ObjectLoader<MfxInfo>(i)
1483 add("address", static_cast<unsigned MfxInfo::*>(&MfxInfo::address));
1484 add("name", static_cast<string MfxInfo::*>(&MfxInfo::name));