void PlaybackController::nextSequence()
{
if(waitForExporter())
return;
// Play back until either an undopoint or end-of-file is reached
bool loop=true;
while(loop) {
MessageRecord next = m_reader->readNext();
switch(next.status) {
case MessageRecord::OK:
if(next.message->type() == protocol::MSG_INTERVAL) {
// skip intervals
delete next.message;
} else {
emit commandRead(protocol::MessagePtr(next.message));
if(next.message->type() == protocol::MSG_UNDOPOINT)
loop = false;
}
break;
case MessageRecord::INVALID:
qWarning() << "Unrecognized command " << next.error.type << "of length" << next.error.len;
break;
case MessageRecord::END_OF_RECORDING:
emit endOfFileReached();
loop = false;
break;
}
}
if(m_exporter && m_autosave)
exportFrame();
updateIndexPosition();
}
void PlaybackController::jumpTo(int pos)
{
Q_ASSERT(m_indexloader);
if(pos == m_reader->currentIndex())
return;
if(waitForExporter())
return;
// If the target position is behind current position or sufficiently far ahead, jump
// to the closest snapshot point first
if(pos < m_reader->currentIndex() || pos - m_reader->currentIndex() > 500) {
int seIdx=0;
const Index &index = m_indexloader->index();
for(int i=1;i<index.snapshots().size();++i) {
const SnapshotEntry &next = index.snapshots().at(i);
if(int(next.pos) > pos)
break;
seIdx = i;
}
// When jumping forward, don't restore the snapshot if the snapshot is behind
// the current position
if(pos < m_reader->currentIndex() || index.snapshots().at(seIdx).pos > quint32(m_reader->currentIndex()))
jumptToSnapshot(seIdx);
}
// Now the current position is somewhere before the target position: replay commands
while(m_reader->currentIndex() < pos && !m_reader->isEof()) {
MessageRecord next = m_reader->readNext();
switch(next.status) {
case MessageRecord::OK:
if(next.message->type() == protocol::MSG_INTERVAL) {
// skip intervals
delete next.message;
} else {
emit commandRead(protocol::MessagePtr(next.message));
}
break;
case MessageRecord::INVALID:
qWarning() << "Unrecognized command " << next.error.type << "of length" << next.error.len;
break;
case MessageRecord::END_OF_RECORDING:
emit endOfFileReached();
break;
}
}
if(m_exporter && m_autosave)
exportFrame();
updateIndexPosition();
}
void PlaybackController::jumpTo(int pos)
{
if(!m_indexloader) {
qWarning("jumpTo(%d): index not loaded!", pos);
return;
}
if(pos == m_reader->currentIndex())
return;
if(waitForExporter())
return;
// If the target position is behind current position or sufficiently far ahead, jump
// to the closest snapshot point first
if(pos < m_reader->currentIndex() || pos - m_reader->currentIndex() > 500) {
const Index &index = m_indexloader->index();
int snap = index.findClosestSnapshot(pos);
// When jumping forward, don't restore the snapshot if the snapshot is behind
// the current position
if(pos < m_reader->currentIndex() || index.entries().at(snap).pos > quint32(m_reader->currentIndex()))
jumpToSnapshot(snap);
}
// Now the current position is somewhere before the target position: replay commands
while(m_reader->currentIndex() < pos && !m_reader->isEof()) {
MessageRecord next = m_reader->readNext();
switch(next.status) {
case MessageRecord::OK:
if(next.message->type() == protocol::MSG_INTERVAL) {
// skip intervals
delete next.message;
} else {
emit commandRead(protocol::MessagePtr(next.message));
}
break;
case MessageRecord::INVALID:
qWarning("Unrecognized command %d of length %d", next.error.type, next.error.len);
break;
case MessageRecord::END_OF_RECORDING:
emit endOfFileReached();
break;
}
}
if(m_exporter && m_autosave)
exportFrame();
updateIndexPosition();
}
void Communications(void * pvParameters){
msg.Payload = msgBuff;
vSemaphoreCreateBinary(DataSmphr);
if (DataSmphr==NULL){
while(1);
}
qUART_Register_RBR_Callback(UART_GROUNDCOMM, UART_Rx_Handler);
qUART_EnableRx(UART_GROUNDCOMM);
comms_trcLabel = xTraceOpenLabel("Comms task");
for (;;){
if (pdTRUE == xSemaphoreTake(DataSmphr,500/portTICK_RATE_MS)){
vTracePrintF(comms_trcLabel,"Got joystick package");
switch (msg.Type){
case MSG_TYPE_CONTROL:
memcpy(&quadrotor.joystick,msg.Payload,10);
case MSG_TYPE_DEBUG:
break;
case MSG_TYPE_SYSTEM:
switch (msg.Payload[0]) {
case COMMAND_READ:
commandRead(&msg.Payload[1]);
break;
case COMMAND_WRITE:
commandWrite(&msg.Payload[1]);
break;
default:
break;
}
break;
default:
break;
}
}else{
// Timeout to get a new joystick commands, values to 0
vTracePrintF(comms_trcLabel,"Joystick package timeout");
memset(&quadrotor.joystick,0,sizeof(quadrotor.joystick));
}
}
}
int ofxInFocusSerial::getSystemState() {
commandRead("SYS");
return getResponseInt();
}
string ofxInFocusSerial::getFirmwareVersion() {
commandRead("FVS");
return getResponseString();
}
bool ofxInFocusSerial::isLampEco() {
commandRead("IPM");
return getResponseInt() == 1;
}
int ofxInFocusSerial::getLampHoursEco() {
commandRead("LME");
return getResponseInt();
}
int ofxInFocusSerial::getLampHoursNormal() {
commandRead("LMO");
return getResponseInt();
}
bool ofxInFocusSerial::isPowerOn() {
commandRead("PWR");
return getResponseInt() == 1;
}
void PlaybackController::nextCommands(int stepCount)
{
Q_ASSERT(stepCount>0);
if(waitForExporter()) {
m_waitedForExporter = true;
return;
}
MessageRecord next = m_reader->readNext();
int writeFrames = 1;
switch(next.status) {
case MessageRecord::OK: {
protocol::MessagePtr msg(next.message);
if(msg->type() == protocol::MSG_INTERVAL) {
if(m_play) {
// Autoplay mode: pause for the given interval
int interval = msg.cast<protocol::Interval>().milliseconds();
int maxinterval = m_maxInterval * 1000;
m_timer->start(qMin(maxinterval, int(interval / m_speedFactor)));
if(m_exporter && m_autosave) {
int pauseframes = qRound(qMin(interval, maxinterval) / 1000.0 * m_exporter->fps());
if(pauseframes>0)
writeFrames = pauseframes;
}
} else {
// Manual mode: skip interval
nextCommands(1);
return;
}
} else {
if(m_play) {
if(msg->type() == protocol::MSG_MARKER && m_stopOnMarkers) {
setPlaying(false);
notification::playSound(notification::Event::MARKER);
} else {
if(stepCount==1)
m_timer->start(int(qMax(1.0, 33.0 / m_speedFactor) + 0.5));
}
}
emit commandRead(msg);
}
break;
}
case MessageRecord::INVALID:
qWarning() << "Unrecognized command " << next.error.type << "of length" << next.error.len;
if(m_play)
m_timer->start(1);
break;
case MessageRecord::END_OF_RECORDING:
emit endOfFileReached();
break;
}
if(stepCount>1) {
nextCommands(stepCount-1);
} else {
if(m_exporter && m_autosave)
exportFrame(writeFrames);
updateIndexPosition();
}
}
void serialEvent()
{
while (Serial.available())
{
char inChar = Serial.read();
switch (comState)
{
case COMSTATE_COM:
command = inChar;
comState = COMSTATE_REG;
break;
case COMSTATE_REG:
reg = inChar;
count = 0;
comState = COMSTATE_COM;
if (command == 'r')
{
commandRead(reg, buffer);
Serial.write(buffer, BURST);
comState = COMSTATE_WAITACK;
}
else if (command == 'w')
{
comState = COMSTATE_WRITE;
}
else
{
// wat
comState = COMSTATE_COM;
}
break;
case COMSTATE_WAITACK:
if (inChar == ACK)
{
// acked
}
else if (inChar == NACK)
{
// nacked
}
else
{
// this is really bad
}
comState = COMSTATE_COM;
break;
case COMSTATE_WRITE:
buffer[count] = inChar;
if (++count == BURST)
{
if (commandWrite(reg, buffer))
Serial.write(ACK);
else
Serial.write(NACK);
comState = COMSTATE_COM;
}
break;
default:
comState = COMSTATE_COM;
break;
}
}
}
