void Drive::update() {
updateSensors();
// Determine elapsed time since last update()
struct timeval currenttime;
gettimeofday(¤ttime, NULL);
float dtime = currenttime.tv_sec - mLastUpdate.tv_sec
+ (currenttime.tv_usec - mLastUpdate.tv_usec) / 1000000.0f;
mLastUpdate = currenttime;
mTargetYaw += mRotate * dtime;
// Calculate average sensor readings over time
Vector3<float> accel = averageAccelerometer();
Vector3<float> gyro = averageGyroscope();
// Adjust for calibration
accel.x -= mAccelOffset.x;
accel.y -= mAccelOffset.y;
accel.z -= mAccelOffset.z;
gyro.x -= mGyroOffset.x;
gyro.y -= mGyroOffset.y;
gyro.z -= mGyroOffset.z;
calculateOrientation(dtime, accel, gyro);
stabilize(gyro);
try {
for (int i = 0; i < 4; ++i)
mMotors[i]->update();
} catch (Exception &e) {
std::cout << "Motor update failure." << std::endl;
}
}
eCollisionResult Enemy::handleBallCollision(Actor *ball)
{
stabilize();
Ball *tempBall = static_cast<Ball*>(ball);
int startX, startY, endX, endY;
leftSideLine(startX, startY, endX, endY);
if (SDL_IntersectRectAndLine(tempBall->getImageRect(), &startX, &startY, &endX, &endY)) {
if (tempBall->PrevColHor != ID) {
tempBall->moveLeft();
// store an object a ball currently collides with horizontally
tempBall->PrevColHor = ID;
}
}
else {
rightSideLine(startX, startY, endX, endY);
if (SDL_IntersectRectAndLine(tempBall->getImageRect(), &startX, &startY, &endX, &endY)) {
if (tempBall->PrevColHor != ID) {
tempBall->moveRight();
tempBall->PrevColHor = ID;
}
}
}
// handle top/bottom collision a bit differently
topSideLine(startX, startY, endX, endY);
if (SDL_IntersectRectAndLine(tempBall->getImageRect(), &startX, &startY, &endX, &endY)) {
if (tempBall->PrevColVert != ID) {
// calculate new angle (more to the sides of object, greater the angle)
float xVel = -tempBall->getAcceleration() * (X - tempBall->getX()) / Width;
tempBall->alterXVelocity(xVel);
tempBall->reverseYVelocity();
// store an object a ball currently collides with vertically
tempBall->PrevColVert = ID;
}
}
else {
bottomSideLine(startX, startY, endX, endY);
if (SDL_IntersectRectAndLine(tempBall->getImageRect(), &startX, &startY, &endX, &endY)) {
if (tempBall->PrevColVert != ID) {
float xVel = -tempBall->getAcceleration() * (X - tempBall->getX()) / Width;
tempBall->alterXVelocity(xVel);
tempBall->reverseYVelocity();
tempBall->PrevColVert = ID;
}
}
}
tempBall->PrevBorCol = 0;
tempBall->approveBoost(true);
return RESULT_NORMAL;
}
// Thread-safe and non-blocking. Returns false if the deque is empty.
// Complexity: O(Processes)
bool pop_right(T& r)
{
// Loop until we either pop an element or learn that the deque is empty.
while (true)
{
// Load the anchor.
anchor_pair lrs = anchor_.lrs();
// Check if the deque is empty.
// FIXME: Should we check both pointers here?
if (lrs.get_right_ptr() == nullptr)
return false;
// Check if the deque has 1 element.
if (lrs.get_right_ptr() == lrs.get_left_ptr())
{
// Try to set both anchor pointer
if (anchor_.cas(lrs, anchor_pair(nullptr, nullptr,
lrs.get_left_tag(), lrs.get_right_tag() + 1)))
{
// Set the result, deallocate the popped node, and return.
r = lrs.get_right_ptr()->data;
dealloc_node(lrs.get_right_ptr());
return true;
}
}
// Check if the deque is stable.
else if (lrs.get_left_tag() == stable)
{
// Make sure the anchor hasn't changed since we loaded it.
if (anchor_ != lrs)
continue;
// Get the rightmost nodes' left node.
node_pointer prev = lrs.get_right_ptr()->left.load();
// Try to update the anchor to point to prev as the rightmost
// node.
if (anchor_.cas(lrs, anchor_pair(lrs.get_left_ptr(),
prev.get_ptr(), lrs.get_left_tag(),
lrs.get_right_tag() + 1)))
{
// Set the result, deallocate the popped node, and return.
r = lrs.get_right_ptr()->data;
dealloc_node(lrs.get_right_ptr());
return true;
}
}
// The deque must be unstable, so we have to stabilize it before
// we can continue.
else // lrs.s() != stable
stabilize(lrs);
}
}
void Enemy::update()
{
if (Direction < 0 && Speed > 0 || Direction > 0 && Speed < 0) {
stabilize();
Speed /= 2;
reverse();
}
X += Speed;
ImageHandler->Rect->x = int(X + (Width >> 1));
}
// Thread-safe and non-blocking (may block if node needs to be allocated
// from the operating system). Returns false if the freelist is not able to
// allocate a new deque node.
// Complexity: O(Processes)
bool push_right(T const& data)
{
// Allocate the new node which we will be inserting.
node* n = alloc_node(0, 0, data);
if (n == 0)
return false;
// Loop until we insert successfully.
while (true)
{
// Load the anchor.
anchor_pair lrs = anchor_.lrs();
// Check if the deque is empty.
// FIXME: Should we check both pointers here?
if (lrs.get_right_ptr() == 0)
{
// If the deque is empty, we simply install a new anchor which
// points to the new node as both its leftmost and rightmost
// element.
if (anchor_.cas(lrs, anchor_pair(n, n,
lrs.get_left_tag(), lrs.get_right_tag() + 1)))
return true;
}
// Check if the deque is stable.
else if (lrs.get_left_tag() == stable)
{
// Make the left pointer on our new node refer to the current
// rightmost node.
n->left.store(node_pointer(lrs.get_right_ptr()));
// Now we want to make the anchor point to our new node as the
// leftmost node. We change the state to lpush as the deque
// will become unstable if this operation succeeds.
anchor_pair new_anchor(lrs.get_left_ptr(), n,
rpush, lrs.get_right_tag() + 1);
if (anchor_.cas(lrs, new_anchor))
{
stabilize_right(new_anchor);
return true;
}
}
// The deque must be unstable, so we have to stabilize it before
// we can continue.
else // lrs.s() != stable
stabilize(lrs);
}
}
void* finger_table_update(){
int i;
while(1){
for(i=0;i < KEY_BITS;i++){
fix_fingers(i);
}
sleep(1);
stabilize();
notify();
heart_beat();
}
pthread_exit(0);
}
// Change settings (anything else slot).
void synthv1widget_control::changed (void)
{
if (m_iDirtySetup > 0)
return;
#ifdef CONFIG_DEBUG_0
qDebug("synthv1widget_control::changed()");
#endif
++m_iDirtyCount;
stabilize();
}
const double *llsp_solve(llsp_t *llsp)
{
double *result = NULL;
if (llsp->data) {
stabilize(&llsp->sort, &llsp->good);
trisolve(llsp->good);
/* collect coefficients */
size_t result_row = llsp->good.columns;
for (size_t column = 0; column < llsp->metrics; column++)
llsp->result[column] = llsp->full.matrix[column][result_row];
result = llsp->result;
}
return result;
}
int MichaelDeque::pop_front() {
Anchor a;
while (true) {
a = m_Anchor.load(std::memory_order::memory_order_acquire);
if (c_nEmptyIndex == a.idxLeft) {
return -1;
}
if (a.idxRight == a.idxLeft) {
if (m_Anchor.compare_exchange_weak(a, Anchor(c_nEmptyIndex, c_nEmptyIndex), std::memory_order::memory_order_release, std::memory_order::memory_order_relaxed))
break;
} else if (Stable == status(a)) {
unsigned int idxLeft = index(a.idxLeft), idxRight = index(a.idxRight);
Node *pLeft, *pRight;
pLeft = (Node*) idxLeft;
pRight = (Node*) idxRight;
if (m_Anchor.load(std::memory_order::memory_order_acquire) != a) {
continue;
}
unsigned int nPrev = pLeft->m_Links.load(std::memory_order::memory_order_acquire).idxRight;
if (m_Anchor.compare_exchange_weak(a, Anchor(nPrev, a.idxRight), std::memory_order::memory_order_release, std::memory_order::memory_order_relaxed))
//return pRight->data;
break;
} else {
stabilize(a);
}
}
if (a.idxLeft != c_nEmptyIndex){
Node *res = (Node*) index(a.idxLeft);
return res->data;
}
return -1;
}
void MichaelDeque::push_back( int& data) {
Node* node = new Node();
node->data = data;
while (1) {
Anchor a = m_Anchor.load(std::memory_order::memory_order_acquire);
if (a.idxRight == c_nEmptyIndex) {
if (m_Anchor.compare_exchange_weak(a, Anchor((int) node | c_nEmptyIndex, (int) node | c_nEmptyIndex), std::memory_order::memory_order_release, std::memory_order::memory_order_relaxed))
break;
} else if ( Stable == status(a)) {
node->m_Links.store(Anchor(a.idxRight, c_nEmptyIndex), std::memory_order::memory_order_release);
Anchor aNew(a.idxLeft, (int) node | c_nFlagMask);
if (m_Anchor.compare_exchange_weak(a, aNew, std::memory_order::memory_order_release, std::memory_order::memory_order_relaxed)) {
stabilize_back(aNew);
break;
}
} else {
stabilize(a);
}
}
}
void FreeCameraController::update(double dt)
{
if (!isActive())
return;
double linDist = mLinSpeed * dt;
double rotDist = mRotSpeed * dt;
if (mFast ^ mFastAlternate)
linDist *= mSpeedMult;
if (mLeft)
translate(LocalLeft * linDist);
if (mRight)
translate(LocalLeft * -linDist);
if (mForward)
translate(LocalForward * linDist);
if (mBackward)
translate(LocalForward * -linDist);
if (!mLockUpright)
{
if (mRollLeft)
roll(-rotDist);
if (mRollRight)
roll(rotDist);
}
else if(mModified)
{
stabilize();
mModified = false;
}
// Normalize the matrix to counter drift
getCamera()->getViewMatrix().orthoNormal(getCamera()->getViewMatrix());
}
/// This one should be called periodically
void AP_Mount::update_mount_position()
{
#if MNT_RETRACT_OPTION == ENABLED
static bool mount_open = 0; // 0 is closed
#endif
switch((enum MAV_MOUNT_MODE)_mount_mode.get())
{
#if MNT_RETRACT_OPTION == ENABLED
// move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
case MAV_MOUNT_MODE_RETRACT:
{
Vector3f vec = _retract_angles.get();
_roll_angle = vec.x;
_tilt_angle = vec.y;
_pan_angle = vec.z;
break;
}
#endif
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
{
Vector3f vec = _neutral_angles.get();
_roll_angle = vec.x;
_tilt_angle = vec.y;
_pan_angle = vec.z;
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
{
Vector3f vec = _control_angles.get();
_roll_control_angle = radians(vec.x);
_tilt_control_angle = radians(vec.y);
_pan_control_angle = radians(vec.z);
stabilize();
break;
}
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING:
{
#if MNT_JSTICK_SPD_OPTION == ENABLED
if (_joystick_speed) { // for spring loaded joysticks
// allow pilot speed position input to come directly from an RC_Channel
if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) {
//_roll_control_angle += angle_input(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max) * 0.00001 * _joystick_speed;
_roll_control_angle += rc_ch[_roll_rc_in-1]->norm_input() * 0.00001 * _joystick_speed;
if (_roll_control_angle < radians(_roll_angle_min*0.01)) _roll_control_angle = radians(_roll_angle_min*0.01);
if (_roll_control_angle > radians(_roll_angle_max*0.01)) _roll_control_angle = radians(_roll_angle_max*0.01);
}
if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) {
//_tilt_control_angle += angle_input(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max) * 0.00001 * _joystick_speed;
_tilt_control_angle += rc_ch[_tilt_rc_in-1]->norm_input() * 0.00001 * _joystick_speed;
if (_tilt_control_angle < radians(_tilt_angle_min*0.01)) _tilt_control_angle = radians(_tilt_angle_min*0.01);
if (_tilt_control_angle > radians(_tilt_angle_max*0.01)) _tilt_control_angle = radians(_tilt_angle_max*0.01);
}
if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) {
//_pan_control_angle += angle_input(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max) * 0.00001 * _joystick_speed;
_pan_control_angle += rc_ch[_pan_rc_in-1]->norm_input() * 0.00001 * _joystick_speed;
if (_pan_control_angle < radians(_pan_angle_min*0.01)) _pan_control_angle = radians(_pan_angle_min*0.01);
if (_pan_control_angle > radians(_pan_angle_max*0.01)) _pan_control_angle = radians(_pan_angle_max*0.01);
}
} else {
#endif
// allow pilot position input to come directly from an RC_Channel
if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) {
_roll_control_angle = angle_input_rad(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max);
}
if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) {
_tilt_control_angle = angle_input_rad(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max);
}
if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) {
_pan_control_angle = angle_input_rad(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max);
}
#if MNT_JSTICK_SPD_OPTION == ENABLED
}
#endif
stabilize();
break;
}
#if MNT_GPSPOINT_OPTION == ENABLED
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
{
if(_gps->fix) {
calc_GPS_target_angle(&_target_GPS_location);
stabilize();
}
break;
}
#endif
default:
//do nothing
break;
}
#if MNT_RETRACT_OPTION == ENABLED
// move mount to a "retracted position" into the fuselage with a fourth servo
bool mount_open_new = (enum MAV_MOUNT_MODE)_mount_mode.get()==MAV_MOUNT_MODE_RETRACT ? 0 : 1;
if (mount_open != mount_open_new) {
mount_open = mount_open_new;
move_servo(_open_idx, mount_open_new, 0, 1);
}
#endif
// write the results to the servos
move_servo(_roll_idx, _roll_angle*10, _roll_angle_min*0.1, _roll_angle_max*0.1);
move_servo(_tilt_idx, _tilt_angle*10, _tilt_angle_min*0.1, _tilt_angle_max*0.1);
move_servo(_pan_idx, _pan_angle*10, _pan_angle_min*0.1, _pan_angle_max*0.1);
}
// Constructor.
qtractorPluginSelectForm::qtractorPluginSelectForm (
QWidget *pParent, Qt::WindowFlags wflags )
: QDialog(pParent, wflags)
{
// Setup UI struct...
m_ui.setupUi(this);
// Window modality (let plugin/tool windows rave around).
QDialog::setWindowModality(Qt::WindowModal);
m_pPluginList = NULL;
// Populate plugin type hints...
m_ui.PluginTypeComboBox->addItem(
qtractorPluginType::textFromHint(qtractorPluginType::Any));
#ifdef CONFIG_LADSPA
m_ui.PluginTypeComboBox->addItem(
qtractorPluginType::textFromHint(qtractorPluginType::Ladspa));
#endif
#ifdef CONFIG_DSSI
m_ui.PluginTypeComboBox->addItem(
qtractorPluginType::textFromHint(qtractorPluginType::Dssi));
#endif
#ifdef CONFIG_VST
m_ui.PluginTypeComboBox->addItem(
qtractorPluginType::textFromHint(qtractorPluginType::Vst));
#endif
#ifdef CONFIG_LV2
m_ui.PluginTypeComboBox->addItem(
qtractorPluginType::textFromHint(qtractorPluginType::Lv2));
#endif
QHeaderView *pHeader = m_ui.PluginListView->header();
// pHeader->setDefaultSectionSize(240);
#if QT_VERSION >= 0x050000
// pHeader->setSectionResizeMode(QHeaderView::Custom);
pHeader->setSectionsMovable(false);
#else
// pHeader->setResizeMode(QHeaderView::Custom);
pHeader->setMovable(false);
#endif
pHeader->setStretchLastSection(true);
QTreeWidgetItem *pHeaderItem = m_ui.PluginListView->headerItem();
pHeaderItem->setTextAlignment(0, Qt::AlignLeft); // Name.
pHeaderItem->setTextAlignment(5, Qt::AlignLeft); // Filename.
pHeaderItem->setTextAlignment(6, Qt::AlignLeft); // Index.
pHeaderItem->setTextAlignment(7, Qt::AlignLeft); // Instances.
pHeaderItem->setTextAlignment(8, Qt::AlignLeft); // Type.
pHeader->resizeSection(0, 240); // Name.
m_ui.PluginListView->resizeColumnToContents(1); // Audio.
m_ui.PluginListView->resizeColumnToContents(2); // MIDI.
m_ui.PluginListView->resizeColumnToContents(3); // Controls.
m_ui.PluginListView->resizeColumnToContents(4); // Modes.
pHeader->resizeSection(5, 120); // Path.
m_ui.PluginListView->resizeColumnToContents(6); // Index
m_ui.PluginListView->resizeColumnToContents(7); // Instances
m_ui.PluginListView->setSortingEnabled(true);
m_ui.PluginListView->sortItems(0, Qt::AscendingOrder);
m_ui.PluginScanProgressBar->setMaximum(100);
m_ui.PluginScanProgressBar->hide();
// Initialize conveniency options...
qtractorOptions *pOptions = qtractorOptions::getInstance();
if (pOptions) {
pOptions->loadComboBoxHistory(m_ui.PluginSearchComboBox);
m_ui.PluginSearchComboBox->setEditText(
pOptions->sPluginSearch);
m_ui.PluginTypeComboBox->setCurrentIndex(pOptions->iPluginType);
m_ui.PluginActivateCheckBox->setChecked(pOptions->bPluginActivate);
}
// Let the search begin...
m_ui.PluginSearchComboBox->setFocus();
adjustSize();
// Restore last seen dialog position and extent...
if (pOptions)
pOptions->loadWidgetGeometry(this, true);
// UI signal/slot connections...
QObject::connect(m_ui.PluginResetToolButton,
SIGNAL(clicked()),
SLOT(reset()));
QObject::connect(m_ui.PluginSearchComboBox,
SIGNAL(editTextChanged(const QString&)),
SLOT(refresh()));
QObject::connect(m_ui.PluginTypeComboBox,
SIGNAL(activated(int)),
SLOT(typeHintChanged(int)));
QObject::connect(m_ui.PluginListView,
SIGNAL(itemSelectionChanged()),
SLOT(stabilize()));
// QObject::connect(m_ui.PluginListView,
// SIGNAL(itemActivated(QTreeWidgetItem *, int)),
// SLOT(accept()));
QObject::connect(m_ui.PluginListView,
SIGNAL(itemDoubleClicked(QTreeWidgetItem *, int)),
SLOT(accept()));
QObject::connect(m_ui.PluginActivateCheckBox,
SIGNAL(clicked()),
SLOT(stabilize()));
QObject::connect(m_ui.PluginRescanPushButton,
SIGNAL(clicked()),
SLOT(rescan()));
QObject::connect(m_ui.DialogButtonBox,
SIGNAL(accepted()),
SLOT(accept()));
QObject::connect(m_ui.DialogButtonBox,
SIGNAL(rejected()),
SLOT(reject()));
qtractorPluginFactory *pPluginFactory
= qtractorPluginFactory::getInstance();
if (pPluginFactory) {
QObject::connect(pPluginFactory,
SIGNAL(scanned(int)),
SLOT(scanned(int)));
}
typeHintChanged(m_ui.PluginTypeComboBox->currentIndex());
}
// Refresh plugin listing.
void qtractorPluginSelectForm::refresh (void)
{
m_ui.PluginListView->clear();
qtractorPluginFactory *pPluginFactory
= qtractorPluginFactory::getInstance();
if (pPluginFactory == NULL)
return;
// FIXME: Should this be a global (singleton) registry?
if (pPluginFactory->types().isEmpty()) {
// Tell the world we'll take some time...
QApplication::setOverrideCursor(QCursor(Qt::WaitCursor));
const bool bRescan = m_ui.PluginRescanPushButton->isVisible();
if (bRescan) m_ui.PluginRescanPushButton->hide();
m_ui.DialogButtonBox->button(QDialogButtonBox::Cancel)->setEnabled(false);
m_ui.PluginScanProgressBar->show();
pPluginFactory->scan();
m_ui.PluginScanProgressBar->hide();
m_ui.DialogButtonBox->button(QDialogButtonBox::Cancel)->setEnabled(true);
if (bRescan) m_ui.PluginRescanPushButton->show();
// We're formerly done.
QApplication::restoreOverrideCursor();
}
if (m_pPluginList == NULL) {
stabilize();
return;
}
const unsigned short iChannels = m_pPluginList->channels();
const bool bMidi = m_pPluginList->isMidi();
QString sSearch = m_ui.PluginSearchComboBox->currentText().simplified();
const QRegExp rx(sSearch.replace(QRegExp("[\\s]+"), ".*"), Qt::CaseInsensitive);
QStringList cols;
QList<QTreeWidgetItem *> items;
QListIterator<qtractorPluginType *> type_iter(pPluginFactory->types());
while (type_iter.hasNext()) {
qtractorPluginType *pType = type_iter.next();
const QString& sFilename = pType->filename();
const QString& sName = pType->name();
if (rx.isEmpty()
|| rx.indexIn(sName) >= 0
|| rx.indexIn(sFilename) >= 0) {
// Try primary instantiation...
const int iAudioIns = pType->audioIns();
const int iAudioOuts = pType->audioOuts();
const int iMidiIns = pType->midiIns();
const int iMidiOuts = pType->midiOuts();
const int iControlIns = pType->controlIns();
const int iControlOuts = pType->controlOuts();
// All that to check whether it will get properly instantiated.
const unsigned short iInstances
= pType->instances(iChannels, bMidi);
cols.clear();
cols << sName;
cols << QString("%1:%2").arg(iAudioIns).arg(iAudioOuts);
cols << QString("%1:%2").arg(iMidiIns).arg(iMidiOuts);
cols << QString("%1:%2").arg(iControlIns).arg(iControlOuts);
QStringList modes;
if (pType->isEditor())
modes << tr("GUI");
if (pType->isConfigure())
modes << tr("EXT");
if (pType->isRealtime())
modes << tr("RT");
if (modes.isEmpty())
cols << "-";
else
cols << modes.join(",");
cols << sFilename;
cols << QString::number(pType->index());
cols << QString::number(iInstances);
cols << qtractorPluginType::textFromHint(pType->typeHint());
QTreeWidgetItem *pItem = new QTreeWidgetItem(cols);
if (iInstances < 1) {
pItem->setFlags(pItem->flags() & ~Qt::ItemIsSelectable);
const int iColumnCount = m_ui.PluginListView->columnCount();
const QPalette& pal = m_ui.PluginListView->palette();
const QColor& rgbForeground
= pal.color(QPalette::Disabled, QPalette::WindowText);
for (int i = 0; i < iColumnCount; ++i)
pItem->setForeground(i, rgbForeground);
}
pItem->setTextAlignment(1, Qt::AlignHCenter); // Audio
pItem->setTextAlignment(2, Qt::AlignHCenter); // MIDI
pItem->setTextAlignment(3, Qt::AlignHCenter); // Controls
pItem->setTextAlignment(4, Qt::AlignHCenter); // Modes
items.append(pItem);
}
}
m_ui.PluginListView->addTopLevelItems(items);
QHeaderView *pHeader = m_ui.PluginListView->header();
m_ui.PluginListView->sortItems(
pHeader->sortIndicatorSection(),
pHeader->sortIndicatorOrder());
m_ui.PluginResetToolButton->setEnabled(!rx.isEmpty());
stabilize();
}
// update mount position - should be called periodically
void AP_Mount_Servo::update()
{
static bool mount_open = 0; // 0 is closed
// check servo map every three seconds to allow users to modify parameters
uint32_t now = hal.scheduler->millis();
if (now - _last_check_servo_map_ms > 3000) {
check_servo_map();
_last_check_servo_map_ms = now;
}
switch(get_mode()) {
// move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
case MAV_MOUNT_MODE_RETRACT:
{
_angle_bf_output_deg = _state._retract_angles.get();
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
{
_angle_bf_output_deg = _state._neutral_angles.get();
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
{
// earth-frame angle targets (i.e. _angle_ef_target_rad) should have already been set by a MOUNT_CONTROL message from GCS
stabilize();
break;
}
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING:
{
// update targets using pilot's rc inputs
update_targets_from_rc();
stabilize();
break;
}
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
{
if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, _flags.tilt_control, _flags.pan_control);
stabilize();
}
break;
}
default:
//do nothing
break;
}
// move mount to a "retracted position" into the fuselage with a fourth servo
bool mount_open_new = (get_mode() == MAV_MOUNT_MODE_RETRACT) ? 0 : 1;
if (mount_open != mount_open_new) {
mount_open = mount_open_new;
move_servo(_open_idx, mount_open_new, 0, 1);
}
// write the results to the servos
move_servo(_roll_idx, _angle_bf_output_deg.x*10, _state._roll_angle_min*0.1f, _state._roll_angle_max*0.1f);
move_servo(_tilt_idx, _angle_bf_output_deg.y*10, _state._tilt_angle_min*0.1f, _state._tilt_angle_max*0.1f);
move_servo(_pan_idx, _angle_bf_output_deg.z*10, _state._pan_angle_min*0.1f, _state._pan_angle_max*0.1f);
}
//#include <QTime>
void StickMan::paint(QPainter *painter, const QStyleOptionGraphicsItem *, QWidget *)
{
/* static int frames = 0;
static QTime time;
if (frames++ % 100 == 0) {
frames = 1;
time.restart();
}
if (time.elapsed() > 0) {
painter->setPen(Qt::white);
painter->drawText(0, 0, QString::number(frames / (time.elapsed() / 1000.0)));
}*/
stabilize();
if (m_sticks) {
painter->setPen(Qt::white);
for (int i=0; i<BoneCount; ++i) {
int n1 = Bones[i * 2];
int n2 = Bones[i * 2 + 1];
Node *node1 = m_nodes[n1];
Node *node2 = m_nodes[n2];
painter->drawLine(node1->pos(), node2->pos());
}
} else {
// first bone is neck and will be used for head
QPainterPath path;
path.moveTo(posFor(0));
path.lineTo(posFor(1));
// right arm
path.lineTo(posFor(2));
path.lineTo(posFor(6));
path.lineTo(posFor(7));
// left arm
path.moveTo(posFor(3));
path.lineTo(posFor(8));
path.lineTo(posFor(9));
// body
path.moveTo(posFor(2));
path.lineTo(posFor(4));
path.lineTo(posFor(10));
path.lineTo(posFor(11));
path.lineTo(posFor(5));
path.lineTo(posFor(3));
path.lineTo(posFor(1));
// right leg
path.moveTo(posFor(10));
path.lineTo(posFor(12));
path.lineTo(posFor(13));
// left leg
path.moveTo(posFor(11));
path.lineTo(posFor(14));
path.lineTo(posFor(15));
painter->setPen(QPen(m_penColor, 5.0, Qt::SolidLine, Qt::RoundCap));
painter->drawPath(path);
{
int n1 = Bones[0];
int n2 = Bones[1];
Node *node1 = m_nodes[n1];
Node *node2 = m_nodes[n2];
QPointF dist = node2->pos() - node1->pos();
qreal sinAngle = dist.x() / sqrt(pow(dist.x(), 2) + pow(dist.y(), 2));
qreal angle = asin(sinAngle) * 180.0 / M_PI;
QPointF headPos = node1->pos();
painter->translate(headPos);
painter->rotate(-angle);
painter->setBrush(m_fillColor);
painter->drawEllipse(QPointF(0,0), 50.0, 50.0);
painter->setBrush(m_penColor);
painter->setPen(QPen(m_penColor, 2.5, Qt::SolidLine, Qt::RoundCap));
// eyes
if (m_isDead) {
painter->drawLine(-30.0, -30.0, -20.0, -20.0);
painter->drawLine(-20.0, -30.0, -30.0, -20.0);
painter->drawLine(20.0, -30.0, 30.0, -20.0);
painter->drawLine(30.0, -30.0, 20.0, -20.0);
} else {
painter->drawChord(QRectF(-30.0, -30.0, 25.0, 70.0), 30.0*16, 120.0*16);
painter->drawChord(QRectF(5.0, -30.0, 25.0, 70.0), 30.0*16, 120.0*16);
}
// mouth
if (m_isDead) {
painter->drawLine(-28.0, 2.0, 29.0, 2.0);
} else {
painter->setBrush(QColor(128, 0, 64 ));
painter->drawChord(QRectF(-28.0, 2.0-55.0/2.0, 57.0, 55.0), 0.0, -180.0*16);
}
// pupils
if (!m_isDead) {
painter->setPen(QPen(m_fillColor, 1.0, Qt::SolidLine, Qt::RoundCap));
painter->setBrush(m_fillColor);
painter->drawEllipse(QPointF(-12.0, -25.0), 5.0, 5.0);
painter->drawEllipse(QPointF(22.0, -25.0), 5.0, 5.0);
}
}
}
}
STDMETHODIMP COAHolder::Advise(IAdviseSink FAR* pAdvSink,
DWORD FAR* pdwConnection)
{
VDATEHEAP();
int iAdv; // records the first free entry found, or (-1)
int iAdvScan; // counts across array entries
IAdviseSink FAR *FAR *ppIAS; // points at the array entry being examined
IAdviseSink FAR *pIAS; // the actual entry at *ppIAS
M_PROLOG(this);
VDATEIFACE(pAdvSink);
HRESULT hr = NOERROR;
LEDebugOut((DEB_ITRACE, "%p _IN COAHolder::Advise ( %p , %p )"
"\n", this, pAdvSink, pdwConnection));
// Validate where to return the connection.
if (pdwConnection)
{
VDATEPTRIN(pdwConnection, DWORD);
// Default to error case
*pdwConnection = 0;
}
// check our zombie state and stabilize. If we are in a zombie
// state, we do not want to be adding new advise sinks.
CStabilize stabilize((CSafeRefCount *)this);
if( IsZombie() )
{
hr = ResultFromScode(CO_E_RELEASED);
goto errRtn;
}
// find an empty slot and clean up disconnected handlers
for (iAdv = (-1), ppIAS = m_ppIAS, iAdvScan = 0;
iAdvScan < m_iSize; ++ppIAS, ++iAdvScan)
{
if ((pIAS = *ppIAS) == NULL)
{
// NULL entries are handled below, to catch
// any of the below cases creating new NULL values
;
}
else if (!IsValidInterface(pIAS))
{
// not valid; don't try to release
*ppIAS = NULL;
}
else if (!CoIsHandlerConnected(pIAS))
{
// advise sink not connected to server anymore; release
// REVIEW, why do we have to constantly poll these
// to see if they are ok?
pIAS->Release();
*ppIAS = NULL;
}
// if first NULL, save rather than extend array
if ((*ppIAS == NULL) && (iAdv == (-1)))
iAdv = iAdvScan;
}
// if we didn't find an empty slot, we have to add space
if (iAdv == (-1))
{
ppIAS = (IAdviseSink FAR * FAR *)PubMemRealloc(m_ppIAS,
sizeof(IAdviseSink FAR *)*(m_iSize + COAHOLDER_GROWBY));
if (ppIAS != NULL)
{
// zero out new space
_xmemset((void FAR *) (ppIAS + m_iSize), 0,
sizeof(IAdviseSink *) * COAHOLDER_GROWBY);
// this is the index of the new element to use
iAdv = m_iSize;
// replace the old array
m_ppIAS = ppIAS;
m_iSize += COAHOLDER_GROWBY;
}
else
{
// quit if there was an error
hr = ReportResult(0, E_OUTOFMEMORY, 0, 0);
}
}
// a macrostep
InterpreterState InterpreterDraft6::step(bool blocking) {
try {
monIter_t monIter;
NodeSet<std::string> enabledTransitions;
// setup document and interpreter
if (!_isInitialized)
init();
// if we failed return false
if (!_isInitialized)
return INIT_FAILED;
// run initial transitions
if (!_stable) {
stabilize();
// we might only need a single step
if (!_running)
goto EXIT_INTERPRETER;
return INITIALIZED;
}
if (!_running)
return FINISHED;
// read an external event and react
if (blocking) {
// wait until an event becomes available
while(_externalQueue.isEmpty()) {
_condVar.wait(_mutex);
}
} else {
// return immediately if external queue is empty
if (_externalQueue.isEmpty())
return NOTHING_TODO;
}
_currEvent = _externalQueue.pop();
#if VERBOSE
std::cout << "Received externalEvent event " << _currEvent.name << std::endl;
if (_running && _currEvent.name == "unblock.and.die") {
std::cout << "Still running " << this << std::endl;
} else {
std::cout << "Aborting " << this << std::endl;
}
#endif
_currEvent.eventType = Event::EXTERNAL; // make sure it is set to external
// when we were blocking on destructor invocation
if (!_running) {
goto EXIT_INTERPRETER;
return INTERRUPTED;
}
// --- MONITOR: beforeProcessingEvent ------------------------------
for(monIter_t monIter = _monitors.begin(); monIter != _monitors.end(); monIter++) {
try {
(*monIter)->beforeProcessingEvent(shared_from_this(), _currEvent);
}
USCXML_MONITOR_CATCH_BLOCK(beforeProcessingEvent)
}
if (iequals(_currEvent.name, "cancel.invoke." + _sessionId))
return INTERRUPTED;
try {
_dataModel.setEvent(_currEvent);
} catch (Event e) {
LOG(ERROR) << "Syntax error while setting external event:" << std::endl << e << std::endl << _currEvent;
}
for (std::map<std::string, Invoker>::iterator invokeIter = _invokers.begin();
invokeIter != _invokers.end();
invokeIter++) {
if (iequals(invokeIter->first, _currEvent.invokeid)) {
Arabica::XPath::NodeSet<std::string> finalizes = filterChildElements(_nsInfo.xmlNSPrefix + "finalize", invokeIter->second.getElement());
for (int k = 0; k < finalizes.size(); k++) {
Element<std::string> finalizeElem = Element<std::string>(finalizes[k]);
executeContent(finalizeElem);
}
}
if (HAS_ATTR(invokeIter->second.getElement(), "autoforward") && DOMUtils::attributeIsTrue(ATTR(invokeIter->second.getElement(), "autoforward"))) {
try {
// do not autoforward to invokers that send to #_parent from the SCXML IO Processor!
// Yes do so, see test229!
// if (!boost::equals(_currEvent.getOriginType(), "http://www.w3.org/TR/scxml/#SCXMLEventProcessor"))
invokeIter->second.send(_currEvent);
} catch(...) {
LOG(ERROR) << "Exception caught while sending event to invoker " << invokeIter->first;
}
}
}
// run internal processing until we reach a stable configuration again
enabledTransitions = selectTransitions(_currEvent.name);
if (!enabledTransitions.empty()) {
// test 403b
enabledTransitions.to_document_order();
microstep(enabledTransitions);
}
stabilize();
return PROCESSED;
EXIT_INTERPRETER:
if (!_running) {
// --- MONITOR: beforeCompletion ------------------------------
for(monIter_t monIter = _monitors.begin(); monIter != _monitors.end(); monIter++) {
try {
(*monIter)->beforeCompletion(shared_from_this());
}
USCXML_MONITOR_CATCH_BLOCK(beforeCompletion)
}
exitInterpreter();
if (_sendQueue) {
std::map<std::string, std::pair<InterpreterImpl*, SendRequest> >::iterator sendIter = _sendIds.begin();
while(sendIter != _sendIds.end()) {
_sendQueue->cancelEvent(sendIter->first);
sendIter++;
}
}
// --- MONITOR: afterCompletion ------------------------------
for(monIter_t monIter = _monitors.begin(); monIter != _monitors.end(); monIter++) {
try {
(*monIter)->afterCompletion(shared_from_this());
}
USCXML_MONITOR_CATCH_BLOCK(afterCompletion)
}
return FINISHED;
}
assert(hasLegalConfiguration());
_mutex.unlock();
// remove datamodel
if(_dataModel)
_dataModel = DataModel();
return PROCESSED;
} catch (boost::bad_weak_ptr e) {
LOG(ERROR) << "Unclean shutdown " << std::endl << std::endl;
return INTERRUPTED;
}
// set datamodel to null from this thread
if(_dataModel)
_dataModel = DataModel();
}
main()
{
auto word ms, hook_periodic, a1_divisor, upshift, maxvel;
auto long clkspd, scpr;
auto float lovolt, hivolt, zvolt;
auto float lovel, hivel, zvel, v, xv, minrpm, maxrpm;
auto float pwmperRPM, voltperRPM, voltperpwm, pwm_offs, pwm_yintercept, volt_yintercept, vmax, velperpwm;
auto float R, I, L;
auto float kt, kte, akte, inl, radps;
auto int pwm_range, zpwm, gotvolt, ipwm;
auto word p, minpwm, maxpwm, startminpwm, startmaxpwm;
loop_rate_hz = SERVO_LOOP_RATE_HZ;
dut = 0;
clkspd = get_clkspd(); // Do this before servo ISRs installed!
servo_init();
printf("Servo motor parameter determination program\n");
printf("-------------------------------------------\n");
hook_periodic = get_hook_divisor();
if (!hook_periodic)
a1_divisor = get_a1_divisor(clkspd);
_next_motor:
printf("\nPress enter to enable servo #%d and proceed.\n", dut+1);
printf("...or press 'x <enter>' if you have motor datasheet parameters.\n");
_restart:
gets(s);
if (toupper(s[0]) == 'X') {
// Get datasheet parameters as well as performing empirical test
printf("Enter motor torque constant in Newton-meters per Ampere.\n");
printf("(This is the same as the back-EMF constant in Volt-seconds per radian):\n");
gets(s);
kt = atof(s);
}
else
kt = 0; // unknown from datasheet
printf("Enabling and setting output drive to center (512).\n");
servo_openloop(dut, 512);
dut ? SERVO_ENABLE_1() : SERVO_ENABLE_0();
accelerate_to(512+256, 3000, 0);
stabilize(2000, 0);
hivel = velocity(500, 0);
hivolt = ask_voltage();
accelerate_to(512-256, 6000, 0);
stabilize(2000, 0);
lovel = velocity(500, 0);
lovolt = ask_voltage();
if (fabs(hivel - lovel) < 1.0) {
dut ? SERVO_DISABLE_1() : SERVO_DISABLE_0();
printf("Velocity difference seems small or non-existent (< 1 RPM).\n");
printf("Maybe the encoder or motor drive is not connected?\n");
printf("Press enter to restart.\n");
gets(s);
goto _restart;
}
pwmperRPM = 512 / (hivel - lovel);
voltperpwm = (hivolt - lovolt) / 512;
voltperRPM = (hivolt - lovolt) / (hivel - lovel);
pwm_yintercept = lovel - 256 / pwmperRPM;
volt_yintercept = lovolt - 256 * voltperpwm;
pwm_offs = -pwm_yintercept * pwmperRPM;
ipwm = (int)pwm_offs;
zpwm = ipwm > 0 && ipwm < 1024;
if (zpwm) {
accelerate_to(ipwm, 3000, 1); // Stop the motor
stabilize(2000, 1);
zvel = velocity(500, 1);
printf("Velocity at interpolated neutral pwm (%d):\n", ipwm);
printf(" RPM: %f\n", zvel);
if (fabs(zvel) < 1.0) {
// Good, motor stopped (or nearly stopped) when expected.
printf("\nFinding start in forward direction...\n");
for (p = ipwm - 1; p > 1; p--) {
accelerate_to(p, 5, 1);
stabilize(50, 1);
v = velocity(50, 1);
// Continue until over 1 RPM
if (fabs(v) > 1.0)
break;
}
startminpwm = p;
printf(" ... Starting minimum pwm = %u\n", p);
accelerate_to(ipwm, 300, 1);
printf("\nFinding start in reverse direction...\n");
for (p = ipwm + 1; p < 1024; p++) {
accelerate_to(p, 5, 1);
stabilize(50, 1);
v = velocity(50, 1);
// Continue until over 1 RPM
if (fabs(v) > 1.0)
break;
}
startmaxpwm = p;
printf(" ... Starting maximum pwm = %u\n", p);
printf(" ... Dead zone from %u to %u\n", startminpwm+1, startmaxpwm-1);
}
printf("\nFinding control limit in forward direction...\n");
accelerate_to(256, 3000, 1);
for (p = 255; p > 1; p--) {
accelerate_to(p, 5, 1);
stabilize(25, 1);
v = velocity(25, 1);
xv = pwm_yintercept + p / pwmperRPM;
if (fabs(xv) < 1.0)
continue;
// Continue until specified nonlinearity
if (v / xv < (1.0 - ALLOW_NONLINEARITY) || v / xv > (1.0 + ALLOW_NONLINEARITY))
break;
}
minpwm = p;
printf(" ... Minimum pwm = %u\n", p);
printf("\nFinding control limit in reverse direction...\n");
accelerate_to(768, 6000, 1);
for (p = 769; p < 1024; p++) {
accelerate_to(p, 5, 1);
stabilize(25, 1);
v = velocity(25, 1);
xv = pwm_yintercept + p / pwmperRPM;
if (fabs(xv) < 1.0)
continue;
// Continue until specified nonlinearity
if (v / xv < (1.0 - ALLOW_NONLINEARITY) || v / xv > (1.0 + ALLOW_NONLINEARITY))
break;
}
maxpwm = p;
printf(" ... Maximum pwm = %u\n", p);
accelerate_to(ipwm, 6000, 1);
}
dut ? SERVO_DISABLE_1() : SERVO_DISABLE_0();
gotvolt = hivolt != lovolt;
if (dut) { R = ARMATURE_RESISTANCE_1; I = CURRENT_LIMIT_1; L = ARMATURE_INDUCTANCE_1; }
else { R = ARMATURE_RESISTANCE_0; I = CURRENT_LIMIT_0; L = ARMATURE_INDUCTANCE_0; }
vmax = R * I;
// Empirically determined torque constant
if (gotvolt) {
kte = voltperRPM * (60.0 / (2 * 3.14159));
akte = fabs(kte);
if (kt != 0.0) {
// Compare with datasheet value to determine running friction.
if (kt > akte)
printf("Datasheet torque constant did not agree with measured value. Using measured value.\n");
else {
// The RMP "drop" is used to deduce the no-load current at 1/2 max speed.
radps = fabs(hivel - lovel) * (3.14159 / 60.0);
inl = ((akte - kt) * radps) / R;
printf("\nNo-load current at %f rad/sec = %fA\n", radps, inl);
printf(" ... friction load torque = %fNm\n", inl * kt);
// Use the datasheet value for computing the rest of the data
hivel *= (akte / kt);
lovel *= (akte / kt);
kte *= (kt / akte);
pwmperRPM = 512 / (hivel - lovel);
voltperRPM = (hivolt - lovolt) / (hivel - lovel);
}
}
}
printf("\nResults and recommendations:\n");
printf( "----------------------------\n");
if (gotvolt) {
printf("RPM/Volt = %f\n", 1.0 / voltperRPM);
printf(" ... Volt-seconds/radian = %f\n", kte);
printf(" ... = Newton-meters/Ampere\n");
printf("Volt/pwm-step = %f\n", voltperpwm);
printf(" ... Voltage at 0 pwm = %f\n", volt_yintercept);
printf(" ... Voltage at 1024 pwm = %f\n", volt_yintercept + voltperpwm * 1024);
}
printf("RPM/pwm-step = %f\n", 1.0 / pwmperRPM);
printf(" ... RPM at 0 pwm = %f\n", minrpm = pwm_yintercept);
printf(" ... RPM at 1024 pwm = %f\n", maxrpm = pwm_yintercept + 1024 / pwmperRPM);
minrpm = fabs(minrpm);
maxrpm = fabs(maxrpm);
if (minrpm > maxrpm)
maxrpm = minrpm;
if (maxrpm < (dut ? SERVO_MAX_RPM_1 : SERVO_MAX_RPM_0))
maxrpm = (dut ? SERVO_MAX_RPM_1 : SERVO_MAX_RPM_0);
printf("pwm for zero RPM = %d\n", ipwm);
if (!zpwm) {
printf(" ... this seems to be a unidirectional application!\n");
}
scpr = dut ? SERVO_COUNT_PER_REV_1 : SERVO_COUNT_PER_REV_0;
maxvel = (word)((float)maxrpm * scpr / (60.0 * loop_rate_hz));
velperpwm = scpr / (60.0 * pwmperRPM * loop_rate_hz);
upshift = 0;
while (upshift < 6 && maxvel < 8192 >> upshift) {
upshift++;
velperpwm *= 2.0;
}
if (vmax > 0.0 && gotvolt) {
printf("Max voltage diff to remain within current limit of %fA:\n", I);
printf(" = %f\n", vmax);
printf(" ... max pwm-diff = %f\n", vmax / voltperpwm);
}
if (R > 0 && L > 0)
printf("Winding L/R time constant = %fms\n", L/R);
printf("\n/* Recommended definitions for servo #%d. Copy and paste into following samples... */\n", dut+1);
if (!dut) {
printf("#define NUM_SERVOS %u\n", (word)WANT_NUM_SERVOS);
printf("#define SERVO_LOOP_RATE_HZ %u\n", loop_rate_hz);
if (hook_periodic)
printf("#define SERVO_HOOK_PERIODIC %d\n", hook_periodic);
else
printf("#define SERVO_A1_DIVISOR %d\n", a1_divisor);
printf("#define SERVO_IP_LEVEL %u\n", (word)SERVO_IP_LEVEL);
printf("#define SERVO_PWM_FREQ %u\n", (word)SERVO_PWM_FREQ);
printf("#define SERVO_PWM_MODE %u\n", (word)SERVO_PWM_MODE);
#ifdef SERVO_PWM_OPENDRAIN
printf("#define SERVO_PWM_OPENDRAIN\n");
#endif
#ifdef SERVO_PWM_INVERT
printf("#define SERVO_PWM_INVERT\n");
#endif
#ifdef SERVO_PWM_SWAP
printf("#define SERVO_PWM_SWAP\n");
#endif
printf("#define SERVO_VS_UPSHIFT %u\n", upshift);
printf("#define SERVO_VS_DOWNSHIFT 0\n");
}
if (pwmperRPM > 0.0)
printf("#define SERVO_FORWARD_%d // High PWM duty gives increasing count\n", dut);
else
printf("#define SERVO_REVERSE_%d // High PWM duty gives decreasing count\n", dut);
printf("#define SERVO_COUNT_PER_REV_%d %ldL\n", dut, (long)scpr);
printf("#define SERVO_MAX_RPM_%d %luL\n", dut, (unsigned long)maxrpm);
if (zpwm) {
printf("#define SERVO_PWM_INIT_OFFS_%d %u\n", dut, ipwm);
pwm_range = ipwm - 1;
if (pwm_range >= ipwm - minpwm)
pwm_range = ipwm - minpwm - 1;
if (pwm_range >= maxpwm - ipwm)
pwm_range = maxpwm - ipwm - 1;
if (ipwm + pwm_range >= 1024)
pwm_range = 1023 - ipwm;
printf("#define SERVO_PWM_INIT_RANGE_%d %u\n", dut, pwm_range);
if (vmax > 0.0 && gotvolt) {
printf("#define SERVO_PWM_MAXDELTA_%d %u\n", dut, (word)fabs(floor(vmax / voltperpwm)));
printf("#define SERVO_PWM_FAC_%d %d\n", dut, (int)(65536.0 / velperpwm));
}
printf("#define SERVO_PWM_MINSTART_%d %u\n", dut, startminpwm);
printf("#define SERVO_PWM_MAXSTART_%d %u\n", dut, startmaxpwm);
}
else
printf("// Cannot make recommendation for SERVO_PWM_INIT_OFFS_%d\n", dut);
printf("/* End of copy and paste section */\n");
dut = !dut;
printf("\n\nRun test for servo #%d? (y/n)\n", dut+1);
gets(s);
if (*s == 'y' || *s == 'Y')
goto _next_motor;
while (1);
return 0;
}
void Enemy::reset()
{
Actor::reset();
stabilize();
}
/* Called by the scheduler for every time slice in which this instrument
should run. This is where the real work of the instrument is done.
*/
int LPCPLAY::run()
{
int n = 0;
float out[2]; /* Space for only 2 output chans! */
#if 0
printf("\nLPCPLAY::run()\n");
#endif
// Samples may have been left over from end of previous run's block
if (_leftOver > 0)
{
int toAdd = min(_leftOver, framesToRun());
#ifdef debug
printf("using %d leftover samps starting at offset %d\n",
_leftOver, _savedOffset);
#endif
rtbaddout(&_alpvals[_savedOffset], toAdd);
increment(toAdd);
n += toAdd;
_leftOver -= toAdd;
_savedOffset += toAdd;
}
/* framesToRun() returns the number of sample frames -- 1 sample for each
channel -- that we have to write during this scheduler time slice.
*/
for (; n < framesToRun(); n += _counter) {
double p[12];
update(p, 12);
_amp = p[2];
_pitch = p[3];
_transposition = ABS(_pitch);
_warpFactor = p[6];
_reson_is_on = p[7] ? true : false;
_cf_fact = p[7];
_bw_fact = p[8];
int loc;
if ( _unvoiced_rate && !_voiced )
{
++_frameno; /* if unvoiced set to normal rate */
}
else
{
_frameno = _frame1 + ((float)(currentFrame())/nSamps()) * _frames;
}
#if 0
printf("frame %g\n", _frameno);
#endif
if (_dataSet->getFrame(_frameno,_coeffs) == -1)
break;
// If requested, stabilize this frame before using
if (_autoCorrect)
stabilize(_coeffs, _nPoles);
float buzamp = getVoicedAmp(_coeffs[THRESH]);
_voiced = (buzamp > 0.1); /* voiced = 0 for 10:1 noise */
float noisamp = (1.0 - buzamp) * _randamp; /* for now */
_ampmlt = _amp * _coeffs[RESIDAMP];
if (_coeffs[RMSAMP] < _cutoff)
_ampmlt = 0;
float cps = tablei(currentFrame(),_pchvals,_tblvals);
float newpch = cps;
// If input pitch was specified as -X.YZ, use this as actual pitch
if ((_pitch < 0) && (ABS(_pitch) >= 1))
newpch = _transposition;
if (_reson_is_on) {
/* If _cf_fact is greater than 20, treat as absolute freq.
Else treat as factor.
If _bw_fact is greater than 20, treat as absolute freq.
Else treat as factor (i.e., cf * factor == bw).
*/
float cf = (_cf_fact < 20.0) ? _cf_fact*cps : _cf_fact;
float bw = (_bw_fact < 20.0) ? cf * _bw_fact : _bw_fact;
rszset(SR, cf, bw, 1., _rsnetc);
#ifdef debug
printf("cf %g bw %g cps %g\n", cf, bw,cps);
#endif
}
float si = newpch * _magic;
float *cpoint = _coeffs + 4;
if (_warpFactor != 0.0)
{
float warp = (_warpFactor > 1.) ? .0001 : _warpFactor;
_ampmlt *= _warpPole.set(warp, cpoint, _nPoles);
}
if (_hnfactor < 1.0)
{
buzamp *= _hnfactor; /* compensate for gain increase */
}
float hn = (_hnfactor <= 1.0) ? (int)(_hnfactor*_srd2/newpch)-2 : _hnfactor;
_counter = int(((float)SR/(newpch * _perperiod) ) * .5);
_counter = (_counter > (nSamps() - currentFrame())) ? nSamps() - currentFrame() : _counter;
#ifdef debug
printf("fr: %g err: %g bzamp: %g noisamp: %g pch: %g ctr: %d\n",
_frameno,_coeffs[THRESH],_ampmlt*buzamp,_ampmlt*noisamp,newpch,_counter);
#endif
if (_counter <= 0)
break;
// Catch bad pitches which generate array overruns
else if (_counter > _arrayLen) {
rtcmix_warn("LPCPLAY", "Counter exceeded array size -- limiting. Frame pitch: %f", newpch);
_counter = _arrayLen;
}
bbuzz(_ampmlt*buzamp,si,hn,_sineFun,&_phs,_buzvals,_counter);
#ifdef debug
printf("\t _buzvals[0] = %g\n", _buzvals[0]);
#endif
l_brrand(_ampmlt*noisamp,_noisvals,_counter); /* TEMPORARY */
#ifdef debug
printf("\t _noisvals[0] = %g\n", _noisvals[0]);
#endif
for (loc=0; loc<_counter; loc++) {
_buzvals[loc] += _noisvals[loc]; /* add voiced and unvoiced */
}
if (_warpFactor) {
float warp = (_warpFactor > 1.) ? shift(_coeffs[PITCH],newpch,(float)SR) : _warpFactor;
#ifdef debug
printf("\tpch: %f newpch: %f d: %f\n",_coeffs[PITCH], newpch, warp);
#endif
/*************
warp = ABS(warp) > .2 ? SIGN(warp) * .15 : warp;
***************/
_warpPole.run(_buzvals, warp, cpoint, _alpvals, _counter);
}
else
{
ballpole(_buzvals,&_jcount,_nPoles,_past,cpoint,_alpvals,_counter);
}
#ifdef debug
{ int x; float maxamp=0; for (x=0;x<_counter;x++) { if (ABS(_alpvals[x]) > ABS(maxamp)) maxamp = _alpvals[x]; }
printf("\t maxamp = %g\n", maxamp);
}
#endif
if (_reson_is_on)
bresonz(_alpvals,_rsnetc,_alpvals,_counter);
// Apply envelope last
float envelope = evp(currentFrame(),_envFun,_envFun,_evals);
bmultf(_alpvals, envelope, _counter);
int sampsToAdd = min(_counter, framesToRun() - n);
/* Write this block to the output buffer. */
rtbaddout(_alpvals, sampsToAdd);
/* Keep track of how many sample frames this instrument has generated. */
increment(sampsToAdd);
}
// Handle case where last synthesized block extended beyond framesToRun()
if (n > framesToRun())
{
_leftOver = n - framesToRun();
_savedOffset = _counter - _leftOver;
#ifdef debug
printf("saving %d samples left over at offset %d\n", _leftOver, _savedOffset);
#endif
}
return framesToRun();
}
void
chord_main(char *conf_file, int parent_sock)
{
fd_set interesting, readable;
int nfound, nfds;
struct in_addr ia;
char id[4*ID_LEN];
FILE *fp;
int64_t stabilize_wait;
struct timeval timeout;
setprogname("chord");
srandom(getpid() ^ time(0));
memset(&srv, 0, sizeof(Server));
srv.to_fix_finger = NFINGERS-1;
fp = fopen(conf_file, "r");
if (fp == NULL)
eprintf("fopen(%s,\"r\") failed:", conf_file);
if (fscanf(fp, "%hd", (short*)&srv.node.port) != 1)
eprintf("Didn't find port in \"%s\"", conf_file);
if (fscanf(fp, " %s\n", id) != 1)
eprintf("Didn't find id in \"%s\"", conf_file);
srv.node.id = atoid(id);
/* Figure out one's own address somehow */
srv.node.addr = ntohl(get_addr());
ia.s_addr = htonl(srv.node.addr);
fprintf(stderr, "Chord started.\n");
fprintf(stderr, "id="); print_id(stderr, &srv.node.id);
fprintf(stderr, "\n");
fprintf(stderr, "ip=%s\n", inet_ntoa(ia));
fprintf(stderr, "port=%d\n", srv.node.port);
initialize(&srv);
srv_ref = &srv;
join(&srv, fp);
fclose(fp);
FD_ZERO(&interesting);
FD_SET(srv.in_sock, &interesting);
FD_SET(parent_sock, &interesting);
nfds = MAX(srv.in_sock, parent_sock) + 1;
NumKeys = read_keys(ACCLIST_FILE, KeyArray, MAX_KEY_NUM);
if (NumKeys == -1) {
printf("Error opening file: %s\n", ACCLIST_FILE);
}
if (NumKeys == 0) {
printf("No key found in %s\n", ACCLIST_FILE);
}
/* Loop on input */
for (;;) {
readable = interesting;
stabilize_wait = (int64_t)(srv.next_stabilize_us - wall_time());
stabilize_wait = MAX(stabilize_wait,0);
timeout.tv_sec = stabilize_wait / 1000000UL;
timeout.tv_usec = stabilize_wait % 1000000UL;
nfound = select(nfds, &readable, NULL, NULL, &timeout);
if (nfound < 0 && errno == EINTR) {
continue;
}
if (nfound == 0) {
stabilize_wait = (int64_t)(srv.next_stabilize_us - wall_time());
if( stabilize_wait <= 0 ) {
stabilize( &srv );
}
continue;
}
if (FD_ISSET(srv.in_sock, &readable)) {
handle_packet(srv.in_sock);
}
else if (FD_ISSET(parent_sock, &readable)) {
handle_packet(parent_sock);
}
else {
assert(0);
}
}
}
int LPCIN::run()
{
int n = 0;
float out[2]; /* Space for only 2 output chans! */
const int inchans = inputChannels();
// Samples may have been left over from end of previous run's block
if (_leftOver > 0)
{
int toAdd = min(_leftOver, framesToRun());
#ifdef debug
printf("using %d leftover samps starting at offset %d\n",
_leftOver, _savedOffset);
#endif
bmultf(&_alpvals[_savedOffset], _ampmlt, toAdd); // Scale signal
rtbaddout(&_alpvals[_savedOffset], toAdd);
increment(toAdd);
n += toAdd;
_leftOver -= toAdd;
_savedOffset += toAdd;
}
/* framesToRun() returns the number of sample frames -- 1 sample for each
channel -- that we have to write during this scheduler time slice.
*/
for (; n < framesToRun(); n += _counter) {
double p[10];
update(p, LPCIN_bw + 1);
_amp = p[LPCIN_amp];
_warpFactor = p[LPCIN_warp];
_reson_is_on = p[LPCIN_cf] ? true : false;
_cf_fact = p[LPCIN_cf];
_bw_fact = p[LPCIN_bw];
int loc;
_frameno = _frame1 + ((float)(currentFrame())/nSamps()) * _frames;
#ifdef debug
printf("\tgetting frame %g of %d (%d out of %d signal samps)\n",
_frameno, (int)_frames, currentFrame(), nSamps());
#endif
if (_dataSet->getFrame(_frameno,_coeffs) == -1)
break;
// If requested, stabilize this frame before using
if (_autoCorrect)
stabilize(_coeffs, _nPoles);
_ampmlt = _amp * _coeffs[RESIDAMP] / 10000.0; // XXX normalize this!
float newpch = (_coeffs[PITCH] > 0.0) ? _coeffs[PITCH] : 64.0;
if (_coeffs[RMSAMP] < _cutoff)
_ampmlt = 0;
if (_reson_is_on) {
/* Treat _cf_fact as absolute freq.
If _bw_fact is greater than 20, treat as absolute freq.
Else treat as factor (i.e., cf * factor == bw).
*/
float cf = _cf_fact;
float bw = (_bw_fact < 20.0) ? cf * _bw_fact : _bw_fact;
rszset(SR, cf, bw, 1., _rsnetc);
/* printf("%f %f %f %f\n",_cf_fact*cps,
_bw_fact*_cf_fact*cps,_cf_fact,_bw_fact,cps); */
}
float *cpoint = _coeffs + 4;
if (_warpFactor != 0.0)
{
float warp = (_warpFactor > 1.) ? .0001 : _warpFactor;
_ampmlt *= _warpPole.set(warp, cpoint, _nPoles);
}
_counter = int(((float)SR/(newpch * /*_perperiod*/ 1.0) ) * .5);
// _counter = (RTBUFSAMPS < MAXVALS) ? RTBUFSAMPS : MAXVALS;
// _counter = (_counter > (nSamps() - currentFrame())) ? nSamps() - currentFrame() : _counter;
_counter = min(_counter, framesToRun() - n);
if (_counter <= 0)
break;
rtgetin(_inbuf, this, _counter);
// Deinterleave input
for (int from=_inChannel, to=0; to < _counter; from += inchans, ++to)
_buzvals[to] = _inbuf[from];
#ifdef debug
printf("\t _buzvals[0] = %g\n", _buzvals[0]);
#endif
if (_warpFactor) {
// float warp = (_warpFactor > 1.) ? shift(_coeffs[PITCH],newpch,(float)SR) : _warpFactor;
float warp = _warpFactor;
#ifdef debug
printf("\tpch: %f newpch: %f d: %f\n",_coeffs[PITCH], newpch, warp);
#endif
/*************
warp = ABS(warp) > .2 ? SIGN(warp) * .15 : warp;
***************/
_warpPole.run(_buzvals, warp, cpoint, _alpvals, _counter);
}
else
{
ballpole(_buzvals,&_jcount,_nPoles,_past,cpoint,_alpvals,_counter);
}
#ifdef debug
{ int x; float maxamp=0; for (x=0;x<_counter;x++) { if (ABS(_alpvals[x]) > ABS(maxamp)) maxamp = _alpvals[x]; }
printf("\t maxamp = %g\n", maxamp);
}
#endif
if (_reson_is_on)
bresonz(_alpvals,_rsnetc,_alpvals,_counter);
int sampsToAdd = min(_counter, framesToRun() - n);
// printf("\tscaling %d samples by %g\n", sampsToAdd, _ampmlt);
bmultf(_alpvals, _ampmlt, sampsToAdd); // Scale signal
/* Write this block to the output buffer. */
rtbaddout(_alpvals, sampsToAdd);
/* Keep track of how many sample frames this instrument has generated. */
increment(sampsToAdd);
}
// Handle case where last synthesized block extended beyond framesToRun()
if (n > framesToRun())
{
_leftOver = n - framesToRun();
_savedOffset = _counter - _leftOver;
#ifdef debug
printf("saving %d samples left over at offset %d\n", _leftOver, _savedOffset);
#endif
}
return framesToRun();
}
