+ string cmd_hex;
+ for(unsigned i=0; i<cmd.length; ++i)
+ cmd_hex += format(" %02X", static_cast<unsigned char>(cmd.command[i]));
+ IO::print("< %02X%s\n", cmd.length^0xFF, cmd_hex);
+ }
+
+ control.serial.put(cmd.length^0xFF);
+ control.serial.write(cmd.command, cmd.length);
+
+ unsigned result = 0;
+ while(1)
+ {
+ bool got_data;
+ if(result)
+ got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero);
+ else
+ got_data = IO::poll(control.serial, IO::P_INPUT);
+
+ if(!got_data)
+ break;
+
+ unsigned rlength = control.serial.get()^0xFF;
+ if(rlength>15)
+ {
+ IO::print("Invalid length %02X\n", rlength);
+ continue;
+ }
+
+ char reply[15];
+ unsigned pos = 0;
+ while(pos<rlength)
+ pos += control.serial.read(reply+pos, rlength-pos);
+
+ if(control.debug>=2)
+ {
+ string reply_hex;
+ for(unsigned i=0; i<rlength; ++i)
+ reply_hex += format(" %02X", static_cast<unsigned char>(reply[i]));
+ IO::print("> %02X%s\n", rlength^0xFF, reply_hex);
+ }
+
+ unsigned r = process_reply(reply, rlength);
+ if(r && !result)
+ result = r;
+ }
+
+ return result;
+}
+
+unsigned ArduControl::ControlThread::process_reply(const char *reply, unsigned rlength)
+{
+ unsigned char type = reply[0];
+ if((type&0xE0)==0x80)
+ {
+ if(type!=COMMAND_OK)
+ IO::print("Error %02X\n", type);
+ return type;
+ }
+ else if(type==POWER_STATE && rlength==2)
+ {
+ control.power.set(reply[1]);
+
+ Tag tag;
+ tag.type = Tag::GENERAL;
+ tag.command = POWER;
+ tag.serial = control.power.serial;
+ control.push_completed_tag(tag);