void pspParticle::update(){
//debug();
specificUpdate();
updateGains();
if(sendOsc){
createOscMessage();
}
}
void SDRThread::readLoop() {
SDRThreadIQDataQueue* iqDataOutQueue = static_cast<SDRThreadIQDataQueue*>( getOutputQueue("IQDataOutput"));
if (iqDataOutQueue == NULL) {
return;
}
updateGains();
while (!stopping.load()) {
updateSettings();
readStream(iqDataOutQueue);
}
buffers.purge();
}
void Axon200Commander::Panel::doLoad( const Settings::Object::State &s ) {
if (s.loadInteger("Maximized"))
showMaximized();
else if (s.loadInteger("Minimized"))
showMinimized();
if (s.loadInteger("W") != NULL) {
resize(s.loadInteger("W"), s.loadInteger("H"));
parentWidget()->move(s.loadInteger("X"), s.loadInteger("Y"));
}
mainUI->inputChannelSpinBox->setValue( s.loadInteger("Input Channel") );
mainUI->outputChannelSpinBox->setValue( s.loadInteger("Output Channel") );
mainUI->gainComboBox->setCurrentItem( s.loadInteger("Gain") );
mainUI->configComboBox->setCurrentItem( s.loadInteger("Headstage Config") );
mainUI->ampModeButtonGroup->setButton( s.loadInteger("Mode") );
mainUI->autoModeButton->setOn( s.loadInteger("Auto Mode") );
updateChannels();
updateGains();
updateMode( s.loadInteger("Mode") );
}
// Service Server
// This server will receive its request via req.
// req is of type forceController/forceControl:
// ---------------------------------------------------------------------------
// int32 num_ctrls
// string[] type // "force" or "moment"
// geometry_msgs/Vector3[] desired // i.e. desired force or moment values.
// float64 x
// float64 y
// float64 z
// geometry_msgs/Vector3[] gains // if you want to change the default gains.
// float64 x
// float64 y
// float64 z
// ---------------------------------------------------------------------------
// And the result is of type res:
// ---------------------------------------------------------------------------
// float64[] error
// sensor_msgs/JointState update_angles
// std_msgs/Header header
// uint32 seq
// time stamp
// string frame_id
// string[] name // holds baxter joint names: elbow, should, wrist.
// float64[] position // holds a delta joint angle update
// float64[] velocity // not used
// float64[] effort // not used
// ---------------------------------------------------------------------------
bool controller::execute(forceControl::Request &req, forceControl::Response &res)
{
ROS_INFO("Received service call");
bool ok = false;
std::vector<double> error, js;
std::vector<Eigen::VectorXd> dqs;
// Initialize vectors
dqs.clear();
error.clear();
ini();
js.resize(7);
// Check for gain update
if(req.gains.size() !=0)
updateGains(req.gains, req.type);
// Wait for the first joint angles readings
while(!ok)
{
if(jo_ready_)
ok = true;
}
// Check for errors in required number of controller and type size.
if(req.desired.size() != req.num_ctrls || req.type.size() != req.num_ctrls)
{
ROS_ERROR("The type of controller and/or the desired values does not match the number of controllers!");
return false;
}
// A. Only 1 controller: call primitive controller. Store the delta joint angle update in dqs.
for(unsigned int i=0; i<req.num_ctrls; i++)
{
ROS_INFO("Calling primitive controller %d", i);
if(!ComputePrimitiveController(dqs, req.type[i], req.desired[i], error))
{
ROS_ERROR("Could not compute angle update for type: %s", req.type[i].c_str());
return false;
}
}
// B. 2 Controllers: call compound controllers
if(req.num_ctrls > 1)
{
// Compute a NullSpaceProjection
if(!NullSpaceProjection(dqs, res.update_angles))
{
ROS_ERROR("Could not get null space projection");
return false;
}
}
// Add delta joint angles to current joint angles through fill.
else
res.update_angles = fill(dqs[0]);
res.error = error;
return true;
}
void SDRThread::updateSettings() {
bool doUpdate = false;
if (offset_changed.load()) {
if (!freq_changed.load()) {
frequency.store(frequency.load());
freq_changed.store(true);
}
offset_changed.store(false);
}
if (rate_changed.load()) {
device->setSampleRate(SOAPY_SDR_RX,0,sampleRate.load());
sampleRate.store(device->getSampleRate(SOAPY_SDR_RX,0));
numChannels.store(getOptimalChannelCount(sampleRate.load()));
numElems.store(getOptimalElementCount(sampleRate.load(), 60));
int streamMTU = device->getStreamMTU(stream);
mtuElems.store(streamMTU);
if (!mtuElems.load()) {
mtuElems.store(numElems.load());
}
inpBuffer.data.resize(numElems.load());
overflowBuffer.data.resize(mtuElems.load());
free(buffs[0]);
buffs[0] = malloc(mtuElems.load() * 4 * sizeof(float));
numOverflow = 0;
rate_changed.store(false);
doUpdate = true;
}
if (ppm_changed.load() && hasPPM.load()) {
device->setFrequency(SOAPY_SDR_RX,0,"CORR",ppm.load());
ppm_changed.store(false);
}
if (freq_changed.load()) {
if (frequency_locked.load() && !frequency_lock_init.load()) {
device->setFrequency(SOAPY_SDR_RX,0,"RF",lock_freq.load());
frequency_lock_init.store(true);
} else if (!frequency_locked.load()) {
device->setFrequency(SOAPY_SDR_RX,0,"RF",frequency.load() - offset.load());
}
freq_changed.store(false);
}
// double devFreq = device->getFrequency(SOAPY_SDR_RX,0);
// if (((long long)devFreq + offset.load()) != frequency.load()) {
// wxGetApp().setFrequency((long long)devFreq + offset.load());
// }
if (agc_mode_changed.load()) {
device->setGainMode(SOAPY_SDR_RX, 0, agc_mode.load());
agc_mode_changed.store(false);
if (!agc_mode.load()) {
updateGains();
DeviceConfig *devConfig = deviceConfig.load();
ConfigGains gains = devConfig->getGains();
if (gains.size()) {
for (ConfigGains::iterator gain_i = gains.begin(); gain_i != gains.end(); gain_i++) {
setGain(gain_i->first, gain_i->second);
}
}
}
doUpdate = true;
}
if (gain_value_changed.load() && !agc_mode.load()) {
std::lock_guard < std::mutex > lock(gain_busy);
for (std::map<std::string,bool>::iterator gci = gainChanged.begin(); gci != gainChanged.end(); gci++) {
if (gci->second) {
device->setGain(SOAPY_SDR_RX, 0, gci->first, gainValues[gci->first]);
gainChanged[gci->first] = false;
}
}
gain_value_changed.store(false);
}
if (setting_value_changed.load()) {
std::lock_guard < std::mutex > lock(setting_busy);
for (std::map<std::string, bool>::iterator sci = settingChanged.begin(); sci != settingChanged.end(); sci++) {
if (sci->second) {
device->writeSetting(sci->first, settings[sci->first]);
settingChanged[sci->first] = false;
}
}
setting_value_changed.store(false);
doUpdate = true;
}
if (doUpdate) {
wxGetApp().sdrThreadNotify(SDRThread::SDR_THREAD_INITIALIZED, std::string("Settings updated."));
}
}
GVector::GVector(BitmapFont* f, GroundMixer& g) : SoundComp(g)
{
plane = new SelectPlane(96,96);
plane->maxWaypoints(1);
plane->setAction(new C_UpdatePAD(this));
plane->setMouseMoveAction(new C_UpdatePAD(this));
//plane->setClickMoveDelay(50);
plane->showLine(true);
Pixmap* back_pix = new Pixmap(pix["220x148"]);
add(back_pix, 0,0);
add(new MoveBorder(this,back_pix->pos.w,20),0,0);
Pixmap* vec_pix = new Pixmap(pix["vback96x96"]);
int xoff=94;
int yoff=32;
add(vec_pix, xoff, yoff);
add(plane, xoff, yoff);
mix = new Mixer();
for(int i=0;i<4;i++)
{
inputs[i]=∅
in[i] = new InputTaker(pix["in"], this, i);
addInputTaker(in[i]);
gains[i] = new Gain(0.0);
gains[i]->setInput(inputs[i]);
mix->addInput((Value**)&gains[i]);
}
//CCW start at 9
int h=96;
int w=96;
int hh=h>>1;
int hw=w>>1;
add(in[0], xoff-16, yoff+hh-6);
add(in[1], xoff+hw-6, yoff+h+3);
add(in[2], xoff+3+w, yoff+hh-6);
add(in[3], xoff-6+hw, yoff-14);
out = new OutputGiver(pix["out"],this,0);
addOutputGiver(out);
add(out,6,3);
Coord crd;
crd.x=0;
crd.y=0;
plane->addWaypoint(crd);
/*
rem_sel = new Button(pix["up"],pix["down"]);
rem_sel->stayPressed(false);
rem_sel->setAction(new C_RemSel(this));
add(rem_sel, 30,30);*/
Label* l = new Label(fnt["label"]);
l->printF("vector mix");
add(l,GCOMP_LABEL_XOFF, GCOMP_LABEL_YOFF);
//POS IN
xoff=10;
yoff=20;
l = new Label(f);
l->printF("pos in");
add(l, xoff, yoff);
yoff+=15;
l = new Label(f);
l->printF("x");
add(l, xoff, yoff);
InputTaker* it = new InputTaker(pix["in"], this, 4);
addInputTaker(it);
add(it, xoff, yoff+16);
xoff+=30;
l = new Label(f);
l->printF("y");
add(l, xoff, yoff);
it = new InputTaker(pix["in"], this, 5);
addInputTaker(it);
add(it, xoff, yoff+16);
//POS OUT
xoff=10;
yoff=70;
l = new Label(f);
l->printF("pos out");
add(l, xoff, yoff);
yoff+=10;
l = new Label(f);
l->printF("x");
add(l, xoff, yoff);
OutputGiver* og = new OutputGiver(pix["out"], this, 1);
addOutputGiver(og);
add(og, xoff, yoff+16);
xoff+=30;
l = new Label(f);
l->printF("y");
add(l, xoff, yoff);
og = new OutputGiver(pix["out"], this, 2);
addOutputGiver(og);
add(og, xoff, yoff+16);
pos_in_v=NULL;
pos_in_h=NULL;
pos_out_v = new Const(crd.x);
pos_out_h = new Const(crd.y);
drain = new Drain(this);
ground.addInput((Value**)&drain);
//better do this last
updateGains(crd);
}
void PID::setGains(float gains[3]) {
updateGains(gains[0], gains[1], gains[2]);
}
void PID::setGains(float p_, float i_, float d_) {
updateGains(p_, i_, d_);
}
