void Interpreter::handlePendingCommand()
{
QMutexLocker locker(&m_mutexQueue);
if (m_commandQueue.empty())
return;
const Command command = m_commandQueue.front();
m_commandQueue.pop();
locker.unlock();
switch (command.first)
{
case STOP:
sendStop();
break;
case RUN:
sendRun();
break;
case GET_ACTION:
sendGetAction(command.second.toInt());
break;
case LOAD_PARAMS:
handleLoadParams();
break;
case SAVE_PARAMS:
handleSaveParams();
break;
}
}
void RiscComm::onConsoleInput(QString input)
{
m_sp->readAll(); // flush any residual crap out (from FPGA reset for example)
if (input.compare("s") == 0) {
sendStep();
doScan();
}
else if (input.compare("sc") == 0)
doScan();
else if (input.compare("q") == 0)
QCoreApplication::exit();
else if (input.compare("r") == 0)
sendRun();
else if (input.compare("rs") == 0)
sendReset();
else if (input.compare("st") == 0) {
sendStop();
doScan();
} else if (input.startsWith("wp")) {
QStringList args = input.split(" ");
if (args.length() == 2) { // upload a file
sendProgram(args.at(1));
}
} else if (input.compare("clrmem") == 0) {
QByteArray zeros;
zeros.fill(0, 256);
writeMem(zeros, 0, true);
} else if (input.compare("dm") == 0) {
dumpMem();
}
else
qDebug() << "Unknown command:" << input;
}
void Interpreter::handleSaveParams(bool reject)
{
bool running;
int32_t response;
// if we're running, stop so this doesn't take too long....
// (ie it would proceed with 1 property to returned frame, which could take 1 second or 2)
running = m_running;
if (running==1) // only if we're running and not in forced state (running==2)
sendStop();
// notify monmodules
sendMonModulesParamChange();
// save them in pixy
if (!reject)
handlePixySaveParams(false);
// reset the shadow parameters because we've saved all params
m_chirp->callSync(m_reset_shadows, END_OUT_ARGS, &response, END_IN_ARGS);
// if we're running, we've stopped, now resume
if (running==1)
{
sendRun();
m_fastPoll = false; // turn off fast polling...
}
}
//Send a block of data to the FPGA, raise the execution signal, wait for the execution
// signal to be lowered, then read back up to values of results
// startAddress: local address on FPGA input buffer to begin writing at
// length: # of bytes to write
// inData: data to be sent to FPGA
// maxWaitTime: # of seconds to wait until execution timeout
// outData: readback data buffer (if function returns successfully)
// maxOutLength: maximum length of outData buffer provided
// outputLength: number of bytes actually returned (if function returns successfully)
// Returns true if entire process is successful.
// If function fails for any reason, returns false.
// Check error code with getLastError().
// error == FAILCAPACITY: The output was larger than provided buffer. Rather than the number of
// bytes actually returned, the outputLength variable will contain the TOTAL number bytes the
// function wanted to return (the number of bytes actually returned will be maxOutLength).
// If this occurs, user should read back bytes {maxOutLength, outputLength - 1} manually
// with a subsequent sendRead command.
// error == FAILREADACK: The write and execution phases completed correctly, but we retried
// the readback phase too many times. In this case, like the FAILCAPICITY error, outputLength
// will contain the TOTAL number bytes the function wanted to return. The state of outData is unknown,
// but some data has been partially written. The user could try calling sendRead
// from {0, outputLength-1} manually if re-calling sendWriteAndRun is not easy
// (for example, if inData and outData overlapped).
// error == anything else: see normal error list
BOOL PICO_SIRC::sendWriteAndRun(uint32_t startAddress, uint32_t inLength, uint8_t *inData,
uint32_t maxWaitTimeInMsec, uint8_t *outData, uint32_t maxOutLength,
uint32_t *outputLength)
{
setLastError( 0);
//Check the input parameters
if(!inData){
setLastError( INVALIDBUFFER);
return false;
}
if(startAddress > OUTPUT_OFFSET){
setLastError( INVALIDADDRESS);
return false;
}
if(inLength == 0 || startAddress + inLength > OUTPUT_OFFSET){
setLastError( INVALIDLENGTH);
return false;
}
//Check the output parameters
if(!outData){
setLastError( INVALIDBUFFER);
return false;
}
if(maxOutLength == 0 || maxOutLength > OUTPUT_OFFSET){
setLastError( INVALIDLENGTH);
return false;
}
//Send the data to the FPGA
if (!sendWrite(startAddress,inLength,inData)){
return false;
}
//Send the run cmd
if (!sendRun()){
return false;
}
//Wait till done
if (!waitDone( maxWaitTimeInMsec)){
return false;
}
//Read back data
//BUGBUG what about partial results??
if (!sendRead(0,maxOutLength,outData)){
return false;
}
*outputLength = maxOutLength;
//and done
return true;
}
void Interpreter::handlePendingCommand()
{
QMutexLocker locker(&m_mutexQueue);
if (m_commandQueue.empty())
return;
const Command command = m_commandQueue.front();
m_commandQueue.pop_front();
locker.unlock();
switch (command.m_type)
{
case STOP:
sendStop();
break;
case RUN:
sendRun();
break;
case STOP_LOCAL:
m_localProgramRunning = false;
emit runState(false);
break;
case RUN_LOCAL:
if (m_program.size()>0)
{
m_localProgramRunning = true;
emit runState(true);
emit enableConsole(false);
m_pc = 0;
}
break;
case LOAD_PARAMS:
handleLoadParams();
break;
case SAVE_PARAMS:
handleSaveParams(command.m_arg0.toBool());
break;
case UPDATE_PARAM:
handleUpdateParam();
break;
case CLOSE:
emit runState(-1);
break;
}
}
void Interpreter::handlePendingCommand()
{
switch (m_pendingCommand)
{
case NONE:
break;
case STOP:
sendStop();
break;
case RUN:
sendRun();
break;
}
m_pendingCommand = NONE;
}
void Interpreter::handleLoadParams()
{
DBG("loading...");
uint i;
char *id, *desc;
uint32_t len;
uint32_t flags;
int response, res;
uint8_t *data, *argList;
int running;
// if we're running, stop so this doesn't take too long....
// (ie it would proceed with 1 property to returned frame, which could take 1 second or 2)
running = m_running;
if (running==1) // only if we're running and not in forced state (running==2)
sendStop();
for (i=0; true; i++)
{
QString category;
res = m_chirp->callSync(m_getAll_param, UINT16(i), END_OUT_ARGS, &response, &flags, &argList, &id, &desc, &len, &data, END_IN_ARGS);
if (res<0)
break;
if (response<0)
break;
QString sdesc(desc);
Parameter parameter(id, (PType)argList[0]);
parameter.setProperty(PP_FLAGS, flags);
handleProperties(argList, ¶meter, &sdesc);
parameter.setHelp(sdesc);
// deal with param category
if (strlen((char *)argList)>1)
{
QByteArray a((char *)data, len);
parameter.set(a);
}
else
{
if (argList[0]==CRP_INT8 || argList[0]==CRP_INT16 || argList[0]==CRP_INT32)
{
int32_t val = 0;
Chirp::deserialize(data, len, &val, END);
parameter.set(val);
}
else if (argList[0]==CRP_FLT32)
{
float val;
Chirp::deserialize(data, len, &val, END);
parameter.set(val);
}
else // not sure what to do with it, so we'll save it as binary
{
QByteArray a((char *)data, len);
parameter.set(a);
}
}
// it's changed! (ie, it's been loaded)
parameter.setDirty(true);
m_pixyParameters.add(parameter);
}
// if we're running, we've stopped, now resume
if (running==1)
{
sendRun();
m_fastPoll = false; // turn off fast polling...
}
DBG("loaded");
emit paramLoaded();
sendMonModulesParamChange();
m_pixyParameters.clean();
}
//#pragma code highPriorityInterruptAddress=0x0008
//void high_interrupt(void)
//{
// _asm GOTO highPriorityIsr _endasm
//}
//
//#pragma code
void main(void){
//Default
TRISD = 0x00; //set port D signals to output
menu_ref_1=1;
menu_ref_2=0;
/*zone of disgust*/
values[MAXSPEED]=100; //Max speed
values[PIDGAINS]=100; //PID Gains
values[MAXYAW]=100; //Max Yaw
values[IRSAMPE]=10; //ir_samp_e;
values[IRSAMPR]=20; //ir_samp_r;
values[IRRAW]=60; //ir_raw
values[IRAVG]=10; //ir_avg;
/*****************/
setupSerial();
Lcd_Init();
ADC_setup();
Button_Setup();
welcome();
LCD_disp(menu_ref_1, menu_ref_2);
RUN=0;
while(1){
//NORMAL OPERATION
while(RUN==0){
if(PORTDbits.RD0 == 0)
{
mode_button = 1;
}
if(PORTDbits.RD1 == 0)
{
motor_button = 1;
}
if (mode_button==1){ //Mode change sequence
checkMode(menu);
menu_ref_2=0; //Default menu ref, should only be SPEED
delayms(200);
mode_button=0;
LCD_disp(menu_ref_1, menu_ref_2);
}
delayms(200); //Used to prevent double tapping
//possibly new function
switchChannels(0);
joy_x = doADC();
switchChannels(1);
joy_y = doADC();
if(menu_ref_1==MANUAL) //IF IN MANUAL
{
if(joy_x<=LEFT){ //User pushes right joystick left
if(values[menu_ref_2]>0){
values[menu_ref_2]=values[menu_ref_2]-5; //decrement value by 5%
GLOBAL_MAX_SPEED = values[menu_ref_2];
sendMaxSpeed();
}
LCD_disp(menu_ref_1, menu_ref_2);
delayms(200); //Delay to prevent flickering, and also to prevent 100-0 increments
}
else if (joy_x>=RIGHT){ //User pushes right joystick right
if(values[menu_ref_2]<100){
values[menu_ref_2]=values[menu_ref_2]+5; //increment value by 5%
GLOBAL_MAX_SPEED = values[menu_ref_2];
sendMaxSpeed();
}
LCD_disp(menu_ref_1, menu_ref_2);
delayms(200);
}
}
else if(menu_ref_1==FACTORY){ //IF IN FACTORY MODE
if (joy_y>=UP){ //User pushes left joystick UP
//Circular selection
if(menu_ref_2==0){
menu_ref_2=4;
}
else{
menu_ref_2--; //switched the order of this to prevent overflow
}
LCD_disp(menu_ref_1, menu_ref_2);
delayms(250); //Arbitrary delay of 250 milliseconds
}
else if (joy_y<=DOWN){ //User pushes left joystick DOWN
menu_ref_2++;
//Circular selection
if(menu_ref_2>=7){
menu_ref_2=1;
}
LCD_disp(menu_ref_1, menu_ref_2);
delayms(250);
}
else if (joy_x<=LEFT){ //User pushes right joystick left
if(values[menu_ref_2]>0){
values[menu_ref_2]=values[menu_ref_2]-5; //decrement value by 5%
}
LCD_disp(menu_ref_1, menu_ref_2);
delayms(255);
delayms(200);
}
else if (joy_x>=RIGHT){ //User pushes right joystick right
if(values[menu_ref_2]<100){
values[menu_ref_2]=values[menu_ref_2]+5; //decrement value by 5%
}
LCD_disp(menu_ref_1, menu_ref_2);
delayms(255);
delayms(200);
}
}
//All other menu_ref_1's do not have values displayed
if (motor_button==1){
motor_button=0;
RUN=1;
}
//send here
}
/*Menu on setup*/
if (RUN==1){
on_setup();
GLOBAL_RUN = RUN;
sendRun();
//MOTOR ON GOES AFTER
motor_button=0;
}
/*MOTOR ON BEHAVIOUR*/
while(RUN==1){
if(PORTDbits.RD1 == 0)
{
motor_button = 1;
}
if (motor_button==1){ //At interrupt, stop motor and return to menu
motor_button=0;
RUN=0;
GLOBAL_RUN = RUN;
sendRun();
}
switchChannels(0);
//GLOBAL_VELOCITY = doADC(); GLOBAL_VELOCITY = doADC() //Get joystick values
joy_y = doADC();
GLOBAL_VELOCITY = joy_y;
switchChannels(1);
joy_x = doADC();
GLOBAL_OMEGA = joy_x;
sendVelocity();
sendOmega();
//send command here
}
/*MOTOR OFF MESSAGE*/
RUN=0;
//MOTOR OFF FUNCTION GOES BEFORE MESSAGE
LCD_title(t5); //"Whoa!"
delays(2); //safety delay
LCD_disp(menu_ref_1, menu_ref_2); //Return to normal display
}
}
void Interpreter::handleSaveParams()
{
int i;
int res, response;
bool dirty, running;
// if we're running, stop so this doesn't take too long....
// (ie it would proceed with 1 property to returned frame, which could take 1 second or 2)
running = m_running;
if (running==1) // only if we're running and not in forced state (running==2)
sendStop();
Parameters ¶meters = m_pixyParameters.parameters();
for (i=0, dirty=false; i<parameters.size(); i++)
{
uint8_t buf[0x100];
if (parameters[i].dirty())
{
int len;
QByteArray str = parameters[i].id().toUtf8();
const char *id = str.constData();
PType type = parameters[i].type();
parameters[i].setDirty(false); // reset
dirty = true; // keep track for sending signal
qDebug() << id;
if (type==PT_INT8 || type==PT_INT16 || type==PT_INT32)
{
int val = parameters[i].value().toInt();
len = Chirp::serialize(NULL, buf, 0x100, type, val, END);
}
else if (type==PT_FLT32)
{
float val = parameters[i].value().toFloat();
len = Chirp::serialize(NULL, buf, 0x100, type, val, END);
}
else if (type==PT_INTS8)
{
QByteArray a = parameters[i].value().toByteArray();
len = a.size();
memcpy(buf, a.constData(), len);
}
else
continue; // don't know what to do!
res = m_chirp->callSync(m_set_param, STRING(id), UINTS8(len, buf), END_OUT_ARGS, &response, END_IN_ARGS);
if (res<0 || response<0)
{
emit error("There was a problem setting a parameter.");
break;
}
}
}
// if we're running, we've stopped, now resume
if (running==1)
{
sendRun();
m_fastPoll = false; // turn off fast polling...
}
if (dirty) // if we updated any parameters, output paramChange signal
emit paramChange();
}
void Interpreter::handleLoadParams()
{
qDebug("loading...");
uint i;
char *id, *desc;
uint32_t len;
uint32_t flags;
int response, res;
uint8_t *data, *argList;
int running;
// if we're running, stop so this doesn't take too long....
// (ie it would proceed with 1 property to returned frame, which could take 1 second or 2)
running = m_running;
if (running==1) // only if we're running and not in forced state (running==2)
sendStop();
for (i=0; true; i++)
{
QString category;
res = m_chirp->callSync(m_getAll_param, UINT16(i), END_OUT_ARGS, &response, &flags, &argList, &id, &desc, &len, &data, END_IN_ARGS);
if (res<0)
break;
if (response<0)
break;
QString sdesc(desc);
// deal with param category
QStringList words = QString(desc).split(QRegExp("\\s+"));
int i = words.indexOf("@c");
if (i>=0 && words.size()>i+1)
{
category = words[i+1];
sdesc = sdesc.remove("@c "); // remove form description
sdesc = sdesc.remove(category + " "); // remove from description
category = category.replace('_', ' '); // make it look prettier
}
else
category = CD_GENERAL;
Parameter parameter(id, (PType)argList[0], "("+printArgType(argList[0], flags)+") "+sdesc);
parameter.setProperty(PP_CATEGORY, category);
parameter.setProperty(PP_FLAGS, flags);
if (strlen((char *)argList)>1)
{
QByteArray a((char *)data, len);
parameter.set(a);
}
else
{
if (argList[0]==CRP_INT8 || argList[0]==CRP_INT16 || argList[0]==CRP_INT32)
{
int32_t val = 0;
Chirp::deserialize(data, len, &val, END);
parameter.set(val);
}
else if (argList[0]==CRP_FLT32)
{
float val;
Chirp::deserialize(data, len, &val, END);
parameter.set(val);
}
else // not sure what to do with it, so we'll save it as binary
{
QByteArray a((char *)data, len);
parameter.set(a);
}
}
m_pixyParameters.add(parameter);
}
// if we're running, we've stopped, now resume
if (running==1)
{
sendRun();
m_fastPoll = false; // turn off fast polling...
}
qDebug("loaded");
emit paramLoaded();
if (m_paramDirty) // emit first time to update any modules waiting to get paramter info
{
m_paramDirty = false;
emit paramChange();
}
}
MainWindow::MainWindow(QWidget *parent)
: QMainWindow(parent)
{
ui.setupUi(this);
connect(ui.actionExit, SIGNAL(triggered()), this, SLOT(close()));
//connect(ui.actionExport_Accessed_Files, SIGNAL(triggered()), this, SLOT(exportAccessedFiles()));
connect(ui.actionRun, SIGNAL(triggered()), this, SLOT(sendRun()));
connect(ui.actionRotate_Left, SIGNAL(triggered()), this, SLOT(rotateLeft()));
connect(ui.actionRotate_Right, SIGNAL(triggered()), this, SLOT(rotateRight()));
connect(ui.actionPortrait, SIGNAL(triggered()), this, SLOT(portrait()));
connect(ui.actionPortrait_Upside_Down, SIGNAL(triggered()), this, SLOT(portraitUpsideDown()));
connect(ui.actionLandscape_Left, SIGNAL(triggered()), this, SLOT(landscapeLeft()));
connect(ui.actionLandscape_Right, SIGNAL(triggered()), this, SLOT(landscapeRight()));
connect(ui.action15_fps, SIGNAL(triggered()), this, SLOT(action15_fps()));
connect(ui.action30_fps, SIGNAL(triggered()), this, SLOT(action30_fps()));
connect(ui.action60_fps, SIGNAL(triggered()), this, SLOT(action60_fps()));
connect(ui.actionUnlimited, SIGNAL(triggered()), this, SLOT(actionUnlimited()));
connect(ui.action320x480, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action768x1024, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action640x960, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action1536x2048, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action320x568, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action640x1136, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action480x800, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action240x320, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action540x960, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action480x854, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action240x400, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action360x640, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action800x1280, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action600x1024, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action600x800, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action768x1366, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action720x1280, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.action1080x1920, SIGNAL(triggered()), this, SLOT(actionResolution()));
connect(ui.actionQuarter, SIGNAL(triggered()), this, SLOT(actionScale()));
connect(ui.actionHalf, SIGNAL(triggered()), this, SLOT(actionScale()));
connect(ui.actionOriginal, SIGNAL(triggered()), this, SLOT(actionScale()));
orientationGroup_ = new QActionGroup(this);
orientationGroup_->addAction(ui.actionPortrait);
orientationGroup_->addAction(ui.actionPortrait_Upside_Down);
orientationGroup_->addAction(ui.actionLandscape_Left);
orientationGroup_->addAction(ui.actionLandscape_Right);
resolutionGroup_ = new QActionGroup(this);
resolutionGroup_->addAction(ui.action320x480);
resolutionGroup_->addAction(ui.action768x1024);
resolutionGroup_->addAction(ui.action640x960);
resolutionGroup_->addAction(ui.action1536x2048);
resolutionGroup_->addAction(ui.action320x568);
resolutionGroup_->addAction(ui.action640x1136);
resolutionGroup_->addAction(ui.action480x800);
resolutionGroup_->addAction(ui.action240x320);
resolutionGroup_->addAction(ui.action540x960);
resolutionGroup_->addAction(ui.action480x854);
resolutionGroup_->addAction(ui.action240x400);
resolutionGroup_->addAction(ui.action360x640);
resolutionGroup_->addAction(ui.action800x1280);
resolutionGroup_->addAction(ui.action600x1024);
resolutionGroup_->addAction(ui.action600x800);
resolutionGroup_->addAction(ui.action768x1366);
resolutionGroup_->addAction(ui.action720x1280);
resolutionGroup_->addAction(ui.action1080x1920);
zoomGroup_ = new QActionGroup(this);
zoomGroup_->addAction(ui.actionQuarter);
zoomGroup_->addAction(ui.actionHalf);
zoomGroup_->addAction(ui.actionOriginal);
connect(ui.actionAlways_on_Top, SIGNAL(triggered(bool)), this, SLOT(alwaysOnTop(bool)));
QSettings settings;
QPoint pos = settings.value("pos", QPoint(50, 50)).toPoint();
QSize size = settings.value("size", QSize(1, 1)).toSize();
resize(size);
move(pos);
bool alwaysOnTop = settings.value("alwaysOnTop").toBool();
if (alwaysOnTop)
{
setWindowFlags(windowFlags() | Qt::WindowStaysOnTopHint);
show();
}
ui.actionAlways_on_Top->setChecked(alwaysOnTop);
QTimer::singleShot(0, this, SLOT(afterInitialization()));
int resolution = settings.value("resolution", 320).toInt();
switch (resolution)
{
case 320:
ui.action320x480->setChecked(true);
break;
case 768:
ui.action768x1024->setChecked(true);
break;
case 640:
ui.action640x960->setChecked(true);
break;
case 1536:
ui.action1536x2048->setChecked(true);
break;
case 320+1:
ui.action320x568->setChecked(true);
break;
case 640+1:
ui.action640x1136->setChecked(true);
break;
case 480:
ui.action480x800->setChecked(true);
break;
case 240:
ui.action240x320->setChecked(true);
break;
case 540:
ui.action540x960->setChecked(true);
break;
case 480+1:
ui.action480x854->setChecked(true);
break;
case 240+1:
ui.action240x400->setChecked(true);
break;
case 360:
ui.action360x640->setChecked(true);
break;
case 800:
ui.action800x1280->setChecked(true);
break;
case 600:
ui.action600x1024->setChecked(true);
break;
case 600+1:
ui.action600x800->setChecked(true);
break;
case 768+1:
ui.action768x1366->setChecked(true);
break;
case 720:
ui.action720x1280->setChecked(true);
break;
case 1080:
ui.action1080x1920->setChecked(true);
break;
}
switch (settings.value("scale", 1).toInt())
{
case 1:
ui.actionOriginal->setChecked(true);
break;
case 2:
ui.actionHalf->setChecked(true);
break;
case 4:
ui.actionQuarter->setChecked(true);
break;
}
ui.glCanvas->setScale(scale());
ui.glCanvas->setFixedSize(hardwareWidth()/scale(), hardwareHeight()/scale());
ui.glCanvas->setResolution(hardwareWidth(), hardwareHeight());
Orientation orientation = static_cast<Orientation>(settings.value("orientation", ePortrait).toInt());
switch (orientation)
{
case ePortrait:
portrait();
ui.actionPortrait->setChecked(true);
break;
case eLandscapeLeft:
landscapeLeft();
ui.actionLandscape_Left->setChecked(true);
break;
case ePortraitUpsideDown:
portraitUpsideDown();
ui.actionPortrait_Upside_Down->setChecked(true);
break;
case eLandscapeRight:
landscapeRight();
ui.actionLandscape_Right->setChecked(true);
break;
}
int fps = settings.value("fps2", 60).toInt();
if (fps == 15)
action15_fps();
else if (fps == 30)
action30_fps();
else if (fps == 60)
action60_fps();
else
actionUnlimited();
connect(ui.glCanvas, SIGNAL(projectNameChanged(const QString&)), this, SLOT(projectNameChanged(const QString&)));
}
