void Interpreter::execute(QString command)
{
if (m_running==true)
queueCommand(STOP);
if (m_localProgramRunning)
queueCommand(STOP_LOCAL);
command.remove(QRegExp("^\\s+")); // remove leading whitespace
if (command!="")
emit consoleCommand(command);
}
void Interpreter::runOrStopProgram()
{
unwait(); // unhang ourselves if we're waiting
if (m_localProgramRunning)
m_localProgramRunning = false;
else if (m_running==false)
queueCommand(RUN);
else if (m_running==true)
queueCommand(STOP);
// no case to run local program because this is sort of an undocumented feature for now
}
void CanvasTexture::del()
{
if (!invalidated() && m_textureId) {
if (m_quickItem) {
m_context->quickItemToTextureMap().remove(m_quickItem);
m_quickItem = 0;
queueCommand(CanvasGlCommandQueue::internalClearQuickItemAsTexture, m_textureId);
} else {
queueCommand(CanvasGlCommandQueue::glDeleteTextures, m_textureId);
}
}
m_textureId = 0;
}
void CanvasFrameBuffer::del()
{
if (m_framebufferId) {
queueCommand(CanvasGlCommandQueue::glDeleteFramebuffers, m_framebufferId);
m_framebufferId = 0;
}
}
QT_BEGIN_NAMESPACE
QT_CANVAS3D_BEGIN_NAMESPACE
/*!
* \qmltype Canvas3DTexture
* \since QtCanvas3D 1.0
* \inqmlmodule QtCanvas3D
* \inherits Canvas3DAbstractObject
* \brief Contains an OpenGL texture.
*
* An uncreatable QML type that contains an OpenGL texture. You can get it by calling
* the \l{Context3D::createTexture()}{Context3D.createTexture()} method.
*/
CanvasTexture::CanvasTexture(CanvasGlCommandQueue *queue, CanvasContext *context,
QQuickItem *quickItem) :
CanvasAbstractObject(queue, context),
m_textureId(queue->createResourceId()),
m_isAlive(true),
m_context(context),
m_quickItem(quickItem)
{
if (m_quickItem)
connect(m_quickItem, &QObject::destroyed, this, &CanvasTexture::handleItemDestroyed);
else
queueCommand(CanvasGlCommandQueue::glGenTextures, m_textureId);
}
//--------------------------------------------------------------
void Controller::exitBootstrap() {
std::cout<<"ofxARDrone::Controller::exitBootstrap";
Command configCommand;
configCommand.setName("CONFIG");
configCommand.addString("general:navdata_demo");
configCommand.addString("TRUE");
queueCommand(configCommand);
// queueCommand("AT*CONFIG="+toString(drone->sequenceNumber++)+",\"general:navdata_demo\",\"TRUE\"");
Command command;
command.setName("CTRL");
command.addInt(5);
queueCommand(command);
}
//--------------------------------------------------------------
void Controller::queueCommand(Command &command) {
if(drone == NULL) { std::cerr<<" Drone is NULL"; return; }
string commandString = command.getString(drone->sequenceNumber);
queueCommand(commandString);
drone->sequenceNumber++;
}
void Interpreter::execute(const QString &command)
{
QStringList argv = command.split(QRegExp("[\\s(),\\t]"), QString::SkipEmptyParts);
unwait(); // unhang ourselves if we're waiting for input
m_mutexProg.lock();
m_argv = argv;
m_externalCommand = command; // save command so we can print. This variable also indicates that we're not a human typing a command
m_mutexProg.unlock();
if (m_running==true)
queueCommand(STOP);
}
GlCommand &CanvasGlCommandQueue::queueCommand(CanvasGlCommandQueue::GlCommandId id, GLfloat p1,
GLfloat p2, GLfloat p3, GLfloat p4)
{
GlCommand &command = queueCommand(id);
command.f1 = p1;
command.f2 = p2;
command.f3 = p3;
command.f4 = p4;
return command;
}
void Interpreter::handleLocalProgram()
{
int res;
if (m_program.size()>0 && m_localProgramRunning)
{
if (m_pc>=m_program.size())
m_pc = 0;
res = m_chirp->execute(m_program[m_pc]);
if (res<0)
queueCommand(STOP_LOCAL);
m_pc++;
}
}
void Interpreter::command(const QString &command)
{
QMutexLocker locker(&m_mutexInput);
if (m_localProgramRunning)
return;
QStringList words = command.split(QRegExp("[\\s(),\\t]"), QString::SkipEmptyParts);
if (m_waiting)
{
m_command = command;
m_command.remove(QRegExp("[(),\\t]"));
m_key = (Qt::Key)0;
m_selection = RectA(0, 0, 0, 0);
m_waitInput.wakeAll();
goto end;
}
if (words.size()==0)
goto end;
if (words[0]=="do")
{
clearLocalProgram();
beginLocalProgram();
}
else if (words[0]=="done")
{
endLocalProgram();
locker.unlock();
runOrStopProgram(true);
locker.relock();
}
else if (words[0]=="list")
listProgram();
else if (words[0].left(4)=="cont")
{
locker.unlock();
runOrStopProgram(true);
locker.relock();
}
else if (words[0]=="close")
queueCommand(CLOSE);
else
handleCall(words);
end:
prompt();
}
GlCommand &CanvasGlCommandQueue::queueCommand(CanvasGlCommandQueue::GlCommandId id, GLint p1,
GLint p2, GLint p3, GLint p4, GLint p5, GLint p6,
GLint p7, GLint p8)
{
GlCommand &command = queueCommand(id);
command.i1 = p1;
command.i2 = p2;
command.i3 = p3;
command.i4 = p4;
command.i5 = p5;
command.i6 = p6;
command.i7 = p7;
command.i8 = p8;
return command;
}
//--------------------------------------------------------------
void Controller::emergency(int on) {
std::cout<<"ofxARDrone::Controller::emergency - + toString(on)";
Command command;
command.setName("REF");
msa::BitField32 bitField;
bitField.clearAll();
bitField.setBit(18);
bitField.setBit(20);
bitField.setBit(22);
bitField.setBit(24);
bitField.setBit(28);
bitField.setBit(8, on);
command.addInt(bitField);
queueCommand(command);
}
QT_BEGIN_NAMESPACE
QT_CANVAS3D_BEGIN_NAMESPACE
/*!
* \qmltype Canvas3DFrameBuffer
* \since QtCanvas3D 1.0
* \inqmlmodule QtCanvas3D
* \inherits Canvas3DAbstractObject
* \brief Contains an OpenGL framebuffer.
*
* An uncreatable QML type that contains an OpenGL framebuffer object. You can get it by calling the
* \l{Context3D::createFramebuffer()}{Context3D.createFramebuffer()} method.
*/
CanvasFrameBuffer::CanvasFrameBuffer(CanvasGlCommandQueue *queue, QObject *parent) :
CanvasAbstractObject(queue, parent),
m_framebufferId(queue->createResourceId()),
m_texture(0)
{
queueCommand(CanvasGlCommandQueue::glGenFramebuffers, m_framebufferId);
}
exitCode CommandDispatcher::dispatch(Command * command, bool clean)
throw (exceptions::OutOfMemoryException) {
LOG4CPP_DEBUG(log, "CommandDispatcher::dispatch(Command * command)");
// If disabled...
if ( d_suspended ) {
// Queuing command for handler notify
LOG4CPP_INFO(log, "Disaptcher suspended; queuing new Command for delayed dispatching");
queueCommand(command);
return DIS_SUSPENDED;
}
// Dispatching command immediatly
d_handler->notify(command);
if ( clean ) {
delete command;
}
return OK;
}
//==============================================================================
ObjectHandle CompoundCommand::execute(const ObjectHandle& arguments) const
{
auto cmdSysProvider = getCommandSystemProvider();
assert(cmdSysProvider != nullptr);
MacroEditObject* ccArgs = arguments.getBase<MacroEditObject>();
assert(ccArgs);
std::vector<CommandInstance*> subInstances;
subInstances.reserve(subCommands_.size());
CommandInstancePtr instance;
for (SubCommandCollection::size_type i = 0; i < subCommands_.size(); ++i)
{
ccArgs->resolveDependecy(i, subInstances);
instance = cmdSysProvider->queueCommand(subCommands_[i].first.c_str(), ccArgs->getCommandArgument(i));
assert(instance != nullptr);
cmdSysProvider->waitForInstance(instance);
subInstances.push_back(instance.get());
}
return instance->getReturnValue();
}
void Interpreter::runOrStopProgram(bool local)
{
unwait(); // unhang ourselves if we're waiting
if (local)
{
if (m_running)
queueCommand(STOP);
else if (m_localProgramRunning)
queueCommand(STOP_LOCAL);
else
queueCommand(RUN_LOCAL);
}
else
{
if (m_localProgramRunning)
queueCommand(STOP_LOCAL);
else if (m_running==false)
queueCommand(RUN);
else if (m_running==true)
queueCommand(STOP);
// note m_running has 3 states...
}
}
//--------------------------------------------------------------
void Controller::update() {
if(drone == NULL) { std::cerr<<" Drone is NULL"; return; }
State &state = drone->state;
if(state.isCalibratingHorizontal()) {
if(state.isFlying()) {
std::cerr<<"ofxARDrone::Controller::update - shouldn't calibrate horizontal while flying";
state.setCalibratingHorizontal(false, 0);
} else {
Command command;
command.setName("FTRIM");
queueCommand(command);
}
}
if(state.isCalibratingMagnetometer()) {
if(!state.isFlying()) {
std::cerr<<"ofxARDrone::Controller::update - should only calibrate magnetometer while flying";
state.setCalibratingMagnetometer(false, 0);
} else {
Command command;
command.setName("CALIB");
queueCommand(command);
}
}
if(state.isTakingOff() || state.isLanding()) {
Command command;
command.setName("REF");
msa::BitField32 bitField;
bitField.clearAll();
bitField.setBit(18);
bitField.setBit(20);
bitField.setBit(22);
bitField.setBit(24);
bitField.setBit(28);
bitField.setBit(9, state.isTakingOff()); // keep sending until navdata says it's taken off, or landed
command.addInt(bitField);
queueCommand(command);
}
rollAmount = cinder::math<float>::clamp(rollAmount, -1, 1);
pitchAmount = cinder::math<float>::clamp(pitchAmount, -1, 1);
liftSpeed = cinder::math<float>::clamp(liftSpeed, -1, 1);
spinSpeed = cinder::math<float>::clamp(spinSpeed, -1, 1);
if(state.isFlying()) {
Command command;
command.setName("PCMD");
command.addInt(1);
command.addFloat(rollAmount);
command.addFloat(pitchAmount);
command.addFloat(liftSpeed);
command.addFloat(spinSpeed);
queueCommand(command);
}
if(commandString.empty() && getMillisSinceLastSend() > 50) resetCommunicationWatchdog();
if(commandString.empty() == false) {
// udpSender.Send(commandString.c_str(), commandString.length());
commandHistory.push_front(commandString);
// millisOfLastSend = ofGetElapsedTimeMillis();
this->commandString = "";
}
}
void Interpreter::getAction(int index)
{
queueCommand(GET_ACTION, index);
}
//--------------------------------------------------------------
void Controller::resetCommunicationWatchdog() {
// std::cout<<"ofxARDrone::Controller::resetCommunicationWatchdog");
queueCommand("AT*COMWDG");
}
//--------------------------------------------------------------
void Controller::sendAck() {
std::cout<<"ofxARDrone::Controller::sendAck";
queueCommand("AT*CTRL=0");
}
void Interpreter::saveParams()
{
queueCommand(SAVE_PARAMS);
}
void Interpreter::updateParam()
{
queueCommand(UPDATE_PARAM);
}
void CanvasTexture::bind(CanvasContext::glEnums target)
{
if (m_textureId)
queueCommand(CanvasGlCommandQueue::glBindTexture, GLint(target), m_textureId);
}
void Interpreter::loadParams()
{
queueCommand(LOAD_PARAMS);
}
void Interpreter::saveParams(bool reject)
{
queueCommand(SAVE_PARAMS, reject);
}
/** Adds a sound effect command with a single floating point parameter to the
* queue of the sfx manager. Openal commands can sometimes cause a 5ms delay,
* so it is done in a separate thread.
* \param command The command to execute.
* \param sfx The sound effect to be started.
* \param f Floating point parameter for the command.
*/
void SFXManager::queue(SFXCommands command, SFXBase *sfx, float f)
{
SFXCommand *sfx_command = new SFXCommand(command, sfx, f);
queueCommand(sfx_command);
} // queue(float)
/** Queues a command for the music manager that takes a floating point value
* (e.g. setTemporaryVolume).
* \param mi The music for which the command is.
* \param f The floating point parameter.
*/
void SFXManager::queue(SFXCommands command, MusicInformation *mi, float f)
{
SFXCommand *sfx_command = new SFXCommand(command, mi, f);
queueCommand(sfx_command);
} // queue(MusicInformation)
/** Adds a sound effect command with a Vec3 parameter to the queue of the sfx
* manager. Openal commands can sometimes cause a 5ms delay, so it is done in
* a separate thread.
* \param command The command to execute.
* \param sfx The sound effect to be started.
* \param p A Vec3 parameter for the command.
*/
void SFXManager::queue(SFXCommands command, SFXBase *sfx, const Vec3 &p)
{
SFXCommand *sfx_command = new SFXCommand(command, sfx, p);
queueCommand(sfx_command);
} // queue (Vec3)
