void ControlQueue::rollBack(double time) {
rollbackMode = false;
int rollBackCount = (int) (time / getCycleTime());
int stretchFactor = ceil((double) rollBackCount / (double) rollBackQueue.size());
stretchFactor = max((double) stretchFactor, 1.0);
vec lastCommand(getMovementDegreesOfFreedom());
if(rollBackQueue.size())
lastCommand = rollBackQueue.front();
int newRollBackCount = ceil((double) rollBackCount / (double) stretchFactor);
// fill command queue with last commands (backwards)
for(int i = 0; i < newRollBackCount && rollBackQueue.size(); ++i) {
vec nextCommand = rollBackQueue.front();
// interpolate to stretch the trajectory in case there are not enough measured packets (happens in usage with simulator)
vec diffUnit = (nextCommand - lastCommand) / (double) stretchFactor;
for(int j = 0; j < stretchFactor; ++j) {
move(lastCommand + j * diffUnit);
}
lastCommand = nextCommand;
rollBackQueue.pop_front();
}
// wait until everything has been executed
synchronizeToQueue(1);
rollBackQueue.clear();
}
/* Command reader */
static void JNICALL
reader(jvmtiEnv* jvmti_env, JNIEnv* jni_env, void* arg)
{
jdwpPacket packet;
jdwpCmdPacket *cmd;
jboolean shouldListen = JNI_TRUE;
LOG_MISC(("Begin reader thread"));
while (shouldListen) {
jint rc;
rc = transport_receivePacket(&packet);
/* I/O error or EOF */
if (rc != 0 || (rc == 0 && packet.type.cmd.len == 0)) {
shouldListen = JNI_FALSE;
notifyTransportError();
} else {
cmd = &packet.type.cmd;
LOG_MISC(("Command set %d, command %d", cmd->cmdSet, cmd->cmd));
/*
* FIXME! We need to deal with high priority
* packets and queue flushes!
*/
enqueue(&packet);
shouldListen = !lastCommand(cmd);
}
}
LOG_MISC(("End reader thread"));
}
void dutyminus(uint8_t data) {
if (dutyMain > 10) {
GPIO_write(Board_LED1, Board_LED_OFF);
dutyMain -= 5;
lastCommand(dutyMain);
} else {
GPIO_write(Board_LED1, Board_LED_ON);
dutyMain = 15;
}
}
void dutyplus(uint8_t data) {
if (dutyMain < 91) {
GPIO_write(Board_LED0, Board_LED_OFF);
dutyMain += 5;
lastCommand(dutyMain);
} else {
GPIO_write(Board_LED0, Board_LED_ON);
dutyMain = 90;
}
}
void
debugLoop_run(void)
{
jboolean shouldListen;
jdwpPacket p;
jvmtiStartFunction func;
/* Initialize all statics */
/* We may be starting a new connection after an error */
cmdQueue = NULL;
cmdQueueLock = debugMonitorCreate("JDWP Command Queue Lock");
transportError = JNI_FALSE;
shouldListen = JNI_TRUE;
func = &reader;
(void)spawnNewThread(func, NULL, "JDWP Command Reader");
standardHandlers_onConnect();
threadControl_onConnect();
/* Okay, start reading cmds! */
while (shouldListen) {
if (!dequeue(&p)) {
break;
}
if (p.type.cmd.flags & JDWPTRANSPORT_FLAGS_REPLY) {
/*
* Its a reply packet.
*/
continue;
} else {
/*
* Its a cmd packet.
*/
jdwpCmdPacket *cmd = &p.type.cmd;
PacketInputStream in;
PacketOutputStream out;
CommandHandler func;
/* Should reply be sent to sender.
* For error handling, assume yes, since
* only VM/exit does not reply
*/
jboolean replyToSender = JNI_TRUE;
/*
* For VirtualMachine.Resume commands we hold the resumeLock
* while executing and replying to the command. This ensures
* that a Resume after VM_DEATH will be allowed to complete
* before the thread posting the VM_DEATH continues VM
* termination.
*/
if (resumeCommand(cmd)) {
debugMonitorEnter(resumeLock);
}
/* Initialize the input and output streams */
inStream_init(&in, p);
outStream_initReply(&out, inStream_id(&in));
LOG_MISC(("Command set %d, command %d", cmd->cmdSet, cmd->cmd));
func = debugDispatch_getHandler(cmd->cmdSet,cmd->cmd);
if (func == NULL) {
/* we've never heard of this, so I guess we
* haven't implemented it.
* Handle gracefully for future expansion
* and platform / vendor expansion.
*/
outStream_setError(&out, JDWP_ERROR(NOT_IMPLEMENTED));
} else if (gdata->vmDead &&
((cmd->cmdSet) != JDWP_COMMAND_SET(VirtualMachine))) {
/* Protect the VM from calls while dead.
* VirtualMachine cmdSet quietly ignores some cmds
* after VM death, so, it sends it's own errors.
*/
outStream_setError(&out, JDWP_ERROR(VM_DEAD));
} else {
/* Call the command handler */
replyToSender = func(&in, &out);
}
/* Reply to the sender */
if (replyToSender) {
if (inStream_error(&in)) {
outStream_setError(&out, inStream_error(&in));
}
outStream_sendReply(&out);
}
/*
* Release the resumeLock as the reply has been posted.
*/
if (resumeCommand(cmd)) {
debugMonitorExit(resumeLock);
}
inStream_destroy(&in);
outStream_destroy(&out);
shouldListen = !lastCommand(cmd);
}
}
threadControl_onDisconnect();
standardHandlers_onDisconnect();
/*
* Cut off the transport immediately. This has the effect of
* cutting off any events that the eventHelper thread might
* be trying to send.
*/
transport_close();
debugMonitorDestroy(cmdQueueLock);
/* Reset for a new connection to this VM if it's still alive */
if ( ! gdata->vmDead ) {
debugInit_reset(getEnv());
}
}
