//void imageCallback(const sensor_msgs::ImageConstPtr& msg_ptr,
// const sensor_msgs::CameraInfoConstPtr& info_msg)
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_msg;
try
{
cv_msg = cv_bridge::toCvCopy(msg, "bgr8");
}
catch (sensor_msgs::CvBridgeException& error)
{
ROS_ERROR("error");
return;
}
if(filter){
cv::Mat image;
filter->process(cv_msg->image,image);
if(getDebug()) imshow("Filter",image);
}
// display image if in debug mode
if(getDebug()){
imshow("Debug",cv_msg->image);
int key = cv::waitKey(100);
if(key == 's'){
static int picNum = 0;
std::stringstream ss;
ss << picNum++;
imwrite("image_" + ss.str() + ".jpg",cv_msg->image);
ROS_INFO("Saved image");
}
}
try
{
//ros::Time time = ros::Time::now();
// convert OpenCV image to ROS message
image_pub.publish(cv_msg->toImageMsg());
}
catch (sensor_msgs::CvBridgeException error)
{
ROS_ERROR("error");
}
}
void convert(char *str, int c) {
int i;
if (getDebug() > 2) printf("I: HEX to string: %X\n", c);
sprintf(str, "%X", c);
if (strlen(str) == 1) {
str[1] = str[0];
str[0] = '0';
str[2] = '\0';
}
if (getDebug() > 2)
for (i=0; i<strlen(str); i++)
printf(" * char %d = %d\n", i, str[i]);
}
bool FilterBase::checkMahalanobisThreshold(const Eigen::VectorXd &innovation,
const Eigen::MatrixXd &invCovariance,
const double nsigmas)
{
double sqMahalanobis = innovation.dot(invCovariance * innovation);
double threshold = nsigmas*nsigmas;
if (sqMahalanobis >= threshold)
{
if (getDebug())
{
*debugStream_ << "Innovation mahalanobis distance test failed. Squared Mahalanobis is\n";
*debugStream_ << sqMahalanobis << "\n";
*debugStream_ << "threshold was:\n";
*debugStream_ << threshold << "\n";
*debugStream_ << "Innovation:\n";
*debugStream_ << innovation << "\n";
*debugStream_ << "Inv covariance:\n";
*debugStream_ << invCovariance << "\n";
}
return false;
}
return true;
}
void CVirtualFS::readFile(CFile &file){
if(getDebug())
gcon.printf("CVirtualFS::readFile('%s')\n", file.getName().c_str());
{ // Search all the collection files...
CollectionList::iterator col;
file.freeData();
for(col = fileCollections.begin() ; col != fileCollections.end() ; col++){
col->readFile(file);
if(file.getData())return;
}
if(getDebug())
gcon.printf("CVirtualFS::readFile('%s') not found\n", file.getName().c_str());
throw CException("File not found : " + file.getName());
}
}
void initMessage(MESSAGE *m, char message_type, int message_length) {
if (getDebug() > 0) printf("I: Initialisation message: ");
/* head */
m->soh = SOH;
m->reserve = RES;
m->message_type = message_type;
m->message_length = message_length;
/* message */
m->stx = STX;
m->etx = ETX;
/* delimiter */
m->cr = CR;
if (getDebug() > 0) puts("OK");
}
void CVirtualFS::addCollection(const string &fname){
CFileCollection fileCol;
string fullName = fname;
if(getDebug())
gcon.printf("CVirtualFS::addCollection(%s)\n", fname.c_str());
unzFile *col = (unzFile *)unzOpen(fullName.c_str());
if(col){
if(getDebug())
gcon.printf("CVirtualFS : Adding %s \n",fullName.c_str());
fileCol.colPath = fname;
fileCol.col_type = CFileCollection::NativeCollection;
fileCol.zh = col;
addCollection(fileCol);
return;
}
return;
}
void initReplay(REPLAY *r, char **replay_msg) {
if (getDebug() > 0) printf("I: Initialisation replay: ");
if (strlen(*replay_msg) != 27)
perror("\nE: Bad message length!");
/* head */
r->soh = (*replay_msg)[0];
r->reserve = (*replay_msg)[1];
r->source = (*replay_msg)[2];
r->destination = (*replay_msg)[3];
r->message_type = (*replay_msg)[4];
r->message_length = calloc(2, sizeof(char));
r->message_length = substr(*replay_msg, 5, 2);
/* message */
r->stx = (*replay_msg)[7];
r->result_code = calloc(2, sizeof(char));
r->result_code = substr(*replay_msg, 8, 2);
r->op_code_page = calloc(2, sizeof(char));
r->op_code_page = substr(*replay_msg, 10, 2);
r->op_code = calloc(2, sizeof(char));
r->op_code = substr(*replay_msg, 12, 2);
r->op_type = calloc(2, sizeof(char));
r->op_type = substr(*replay_msg, 14, 2);
r->max_value = calloc(4, sizeof(char));
r->max_value = substr(*replay_msg, 16, 4);
r->value = calloc(4, sizeof(char));
r->value = substr(*replay_msg, 20, 4);
r->etx = (*replay_msg)[24];
/* check_code */
r->bcc = (*replay_msg)[25];
/* delimiter */
r->cr = (*replay_msg)[26];
/* full message */
r->msg = *replay_msg;
if (getDebug() > 0) puts("OK");
}
bool isEligibleForScore()
{
if(!getOfficial()) { uneligibilityReason = "official mode off"; return false; }
if(getDebug()) { uneligibilityReason = "debug mode on"; return false; }
if(!getAutoZoomFactor()) { uneligibilityReason = "modified zoom factor"; return false; }
if(getPlayerSpeed() != 9.45f) { uneligibilityReason = "player speed modified"; return false; }
if(getPlayerFocusSpeed() != 4.625f) { uneligibilityReason = "player focus speed modified"; return false; }
if(getPlayerSize() != 7.3f) { uneligibilityReason = "player size modified"; return false; }
if(getInvincible()) { uneligibilityReason = "invincibility on"; return false; }
if(getNoRotation()) { uneligibilityReason = "rotation off"; return false; }
if(Online::getServerVersion() == -1) { uneligibilityReason = "connection error"; return false; }
if(Online::getServerVersion() > getVersion()) { uneligibilityReason = "version mismatch"; return false; }
return true;
}
void CVirtualFS::mount(const string &mpoint, const string &target){
CFileCollection fileCol;
if(mpoint == "" || target == "")
return;
fileCol.mMountPoint = fixFileName(mpoint);
if(target[target.size() - 1] == '/'){
fileCol.colPath = target;
} else {
fileCol.colPath = target + "/";
}
fileCol.col_type = CFileCollection::Directory;
addCollection(fileCol);
if(getDebug())
gcon.printf("CVirtualFS::mount(%s, %s)\n", fileCol.mMountPoint.c_str(), fileCol.colPath.c_str());
}
void HexagonGame::drawText()
{
ostringstream s;
s << "time: " << toStr(status.currentTime).substr(0, 5) << endl;
if(getOfficial()) s << "official mode" << endl;
if(getDebug()) s << "debug mode" << endl;
if(status.scoreInvalid) s << "score invalidated (performance issues)" << endl;
if(status.hasDied) s << "press r to restart" << endl;
Vector2f pos{15, 3};
vector<Vector2f> offsets{{-1, -1}, {-1, 1}, {1, -1}, {1, 1}};
Color offsetColor{getColor(1)};
if(getBlackAndWhite()) offsetColor = Color::Black;
text.setString(s.str());
text.setCharacterSize(25 / getZoomFactor());
text.setOrigin(0, 0);
text.setColor(offsetColor);
for(const auto& o : offsets) { text.setPosition(pos + o); render(text); }
text.setColor(getColorMain());
text.setPosition(pos);
render(text);
if(messageTextPtr == nullptr) return;
text.setString(messageTextPtr->getString());
text.setCharacterSize(messageTextPtr->getCharacterSize());
text.setOrigin(text.getGlobalBounds().width / 2, 0);
text.setColor(offsetColor);
for(const auto& o : offsets) { text.setPosition(messageTextPtr->getPosition() + o); render(text); }
messageTextPtr->setColor(getColorMain());
render(*messageTextPtr);
}
void Ekf::correct(const Measurement &measurement)
{
if (getDebug())
{
*debugStream_ << "---------------------- Ekf::correct ----------------------\n";
*debugStream_ << "Measurement is:\n";
*debugStream_ << measurement.measurement_ << "\n";
*debugStream_ << "Measurement covariance is:\n";
*debugStream_ << measurement.covariance_ << "\n";
}
// We don't want to update everything, so we need to build matrices that only update
// the measured parts of our state vector
// First, determine how many state vector values we're updating
std::vector<size_t> updateIndices;
for (size_t i = 0; i < measurement.updateVector_.size(); ++i)
{
if (measurement.updateVector_[i])
{
// Handle nan and inf values in measurements
if (std::isnan(measurement.measurement_(i)) || std::isnan(measurement.covariance_(i,i)))
{
if (getDebug())
{
*debugStream_ << "Value at index " << i << " was nan. Excluding from update.\n";
}
}
else if (std::isinf(measurement.measurement_(i)) || std::isinf(measurement.covariance_(i,i)))
{
if (getDebug())
{
*debugStream_ << "Value at index " << i << " was inf. Excluding from update.\n";
}
}
else
{
updateIndices.push_back(i);
}
}
}
if (getDebug())
{
*debugStream_ << "Update indices are:\n";
*debugStream_ << updateIndices << "\n";
}
size_t updateSize = updateIndices.size();
// Now set up the relevant matrices
Eigen::VectorXd stateSubset(updateSize); // x (in most literature)
Eigen::VectorXd measurementSubset(updateSize); // z
Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize); // R
Eigen::MatrixXd stateToMeasurementSubset(updateSize, state_.rows()); // H
Eigen::MatrixXd kalmanGainSubset(state_.rows(), updateSize); // K
Eigen::VectorXd innovationSubset(updateSize); // z - Hx
stateSubset.setZero();
measurementSubset.setZero();
measurementCovarianceSubset.setZero();
stateToMeasurementSubset.setZero();
kalmanGainSubset.setZero();
innovationSubset.setZero();
// Now build the sub-matrices from the full-sized matrices
for (size_t i = 0; i < updateSize; ++i)
{
measurementSubset(i) = measurement.measurement_(updateIndices[i]);
stateSubset(i) = state_(updateIndices[i]);
for (size_t j = 0; j < updateSize; ++j)
{
measurementCovarianceSubset(i, j) = measurement.covariance_(updateIndices[i], updateIndices[j]);
}
// Handle negative (read: bad) covariances in the measurement. Rather
// than exclude the measurement or make up a covariance, just take
// the absolute value.
if (measurementCovarianceSubset(i, i) < 0)
{
if (getDebug())
{
*debugStream_ << "WARNING: Negative covariance for index " << i << " of measurement (value is" << measurementCovarianceSubset(i, i)
<< "). Using absolute value...\n";
}
measurementCovarianceSubset(i, i) = ::fabs(measurementCovarianceSubset(i, i));
}
// If the measurement variance for a given variable is very
// near 0 (as in e-50 or so) and the variance for that
// variable in the covariance matrix is also near zero, then
// the Kalman gain computation will blow up. Really, no
// measurement can be completely without error, so add a small
// amount in that case.
if (measurementCovarianceSubset(i, i) < 1e-6)
{
measurementCovarianceSubset(i, i) = 1e-6;
if (getDebug())
{
*debugStream_ << "WARNING: measurement had very small error covariance for index " << i << ". Adding some noise to maintain filter stability.\n";
}
}
}
// The state-to-measurement function, h, will now be a measurement_size x full_state_size
// matrix, with ones in the (i, i) locations of the values to be updated
for (size_t i = 0; i < updateSize; ++i)
{
stateToMeasurementSubset(i, updateIndices[i]) = 1;
}
if (getDebug())
{
*debugStream_ << "Current state subset is:\n";
*debugStream_ << stateSubset << "\n";
*debugStream_ << "Measurement subset is:\n";
*debugStream_ << measurementSubset << "\n";
*debugStream_ << "Measurement covariance subset is:\n";
*debugStream_ << measurementCovarianceSubset << "\n";
*debugStream_ << "State-to-measurement subset is:\n";
*debugStream_ << stateToMeasurementSubset << "\n";
}
// (1) Compute the Kalman gain: K = (PH') / (HPH' + R)
kalmanGainSubset = estimateErrorCovariance_ * stateToMeasurementSubset.transpose()
* (stateToMeasurementSubset * estimateErrorCovariance_ * stateToMeasurementSubset.transpose() + measurementCovarianceSubset).inverse();
// (2) Apply the gain to the difference between the state and measurement: x = x + K(z - Hx)
innovationSubset = (measurementSubset - stateSubset);
// Wrap angles in the innovation
for (size_t i = 0; i < updateSize; ++i)
{
if (updateIndices[i] == StateMemberRoll || updateIndices[i] == StateMemberPitch || updateIndices[i] == StateMemberYaw)
{
if (innovationSubset(i) < -pi_)
{
innovationSubset(i) += tau_;
}
else if (innovationSubset(i) > pi_)
{
innovationSubset(i) -= tau_;
}
}
}
state_ = state_ + kalmanGainSubset * innovationSubset;
// (3) Update the estimate error covariance using the Joseph form: (I - KH)P(I - KH)' + KRK'
Eigen::MatrixXd gainResidual = identity_ - kalmanGainSubset * stateToMeasurementSubset;
estimateErrorCovariance_ = gainResidual * estimateErrorCovariance_ * gainResidual.transpose() +
kalmanGainSubset * measurementCovarianceSubset * kalmanGainSubset.transpose();
// Handle wrapping of angles
wrapStateAngles();
if (getDebug())
{
*debugStream_ << "Kalman gain subset is:\n";
*debugStream_ << kalmanGainSubset << "\n";
*debugStream_ << "Innovation:\n";
*debugStream_ << innovationSubset << "\n\n";
*debugStream_ << "Corrected full state is:\n";
*debugStream_ << state_ << "\n";
*debugStream_ << "Corrected full estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
*debugStream_ << "Last measurement time is:\n";
*debugStream_ << std::setprecision(20) << lastMeasurementTime_ << "\n";
*debugStream_ << "\n---------------------- /Ekf::correct ----------------------\n";
}
}
void Ekf::predict(const double delta)
{
if (getDebug())
{
*debugStream_ << "---------------------- Ekf::predict ----------------------\n";
*debugStream_ << "delta is " << delta << "\n";
*debugStream_ << "state is " << state_ << "\n";
}
double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);
double xVel = state_(StateMemberVx);
double yVel = state_(StateMemberVy);
double zVel = state_(StateMemberVz);
// We'll need these trig calculations a lot.
double cr = cos(roll);
double cp = cos(pitch);
double cy = cos(yaw);
double sr = sin(roll);
double sp = sin(pitch);
double sy = sin(yaw);
// Prepare the transfer function
transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
transferFunction_(StateMemberPitch, StateMemberVpitch) = delta;
transferFunction_(StateMemberYaw, StateMemberVyaw) = delta;
// Prepare the transfer function Jacobian. This function is analytically derived from the
// transfer function.
double dF0dr = (cy * sp * cr + sy * sr) * delta * yVel + (-cy * sp * sr + sy * cr) * delta * zVel;
double dF0dp = -cy * sp * delta * xVel + cy * cp * sr * delta * yVel + cy * cp * cr * delta * zVel;
double dF0dy = -sy * cp * delta * xVel + (-sy * sp * sr - cy * cr) * delta * yVel + (-sy * sp * cr + cy * sr) * delta * zVel;
double dF1dr = (sy * sp * cr - cy * sr) * delta * yVel + (-sy * sp * sr - cy * cr) * delta * zVel;
double dF1dp = -sy * sp * delta * xVel + sy * cp * sr * delta * yVel + sy * cp * cr * delta * zVel;
double dF1dy = cy * cp * delta * xVel + (cy * sp * sr - sy * cr) * delta * yVel + (cy * sp * cr + sy * sr) * delta * zVel;
double dF2dr = cp * cr * delta * yVel - cp * sr * delta * zVel;
double dF2dp = -cp * delta * xVel - sp * sr * delta * yVel - sp * cr * delta * zVel;
// Much of the transfer function Jacobian is identical to the transfer function
transferFunctionJacobian_ = transferFunction_;
transferFunctionJacobian_(StateMemberX, StateMemberRoll) = dF0dr;
transferFunctionJacobian_(StateMemberX, StateMemberPitch) = dF0dp;
transferFunctionJacobian_(StateMemberX, StateMemberYaw) = dF0dy;
transferFunctionJacobian_(StateMemberY, StateMemberRoll) = dF1dr;
transferFunctionJacobian_(StateMemberY, StateMemberPitch) = dF1dp;
transferFunctionJacobian_(StateMemberY, StateMemberYaw) = dF1dy;
transferFunctionJacobian_(StateMemberZ, StateMemberRoll) = dF2dr;
transferFunctionJacobian_(StateMemberZ, StateMemberPitch) = dF2dp;
if (getDebug())
{
*debugStream_ << "Transfer function is:\n";
*debugStream_ << transferFunction_ << "\n";
*debugStream_ << "Transfer function Jacobian is:\n";
*debugStream_ << transferFunctionJacobian_ << "\n";
*debugStream_ << "Process noise covariance is " << "\n";
*debugStream_ << processNoiseCovariance_ << "\n";
*debugStream_ << "Current state is:\n";
*debugStream_ << state_ << "\n";
}
// (1) Project the state forward: x = Ax (really, x = f(x))
state_ = transferFunction_ * state_;
// Handle wrapping
wrapStateAngles();
if (getDebug())
{
*debugStream_ << "Predicted state is:\n";
*debugStream_ << state_ << "\n";
*debugStream_ << "Current estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
}
// (2) Project the error forward: P = J * P * J' + Q
estimateErrorCovariance_ = (transferFunctionJacobian_ * estimateErrorCovariance_ * transferFunctionJacobian_.transpose())
+ (processNoiseCovariance_ * delta);
if (getDebug())
{
*debugStream_ << "Predicted estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
*debugStream_ << "Last measurement time is:\n";
*debugStream_ << std::setprecision(20) << lastMeasurementTime_ << "\n";
*debugStream_ << "\n--------------------- /Ekf::predict ----------------------\n";
}
}
void render_model_grid(ModelGridsLOD* m, Canvas* c)
{
#ifdef VERBOSE_LOGGING
printf("\n\t--- Render Model Grid ---\n");
printf("Rendering ModelGridsLOD at address 0x%x\n", m);
#endif
if(!m || !c)
{
printf("ERROR: ModelGridsLOD or Canvas don't exist!\n");
return;
}
std::stack< ModelDrawStackNode > dstack;
int l = m->lod;
int cols = m->cols[l];
int rows = m->rows[l];
ModelNode* grid = m->grid[l];
// 6/29/2012 brg: Strangely slow drawing performance led me to discover that the
// algorithm below was drawing the same nodes upwards of 30x per render call!
// For now, track how many times a node has been drawn, ensuring we only draw it
// once. TODO: find flaw in algorithm itself and fix.
const int totalNodes = cols * rows;
char *nodeDrawCounts = new char[totalNodes];
for ( int i = 0; i < totalNodes; i++ ) { nodeDrawCounts[i] = 0; }
float x, y, z;
get_camera_position( c->camera, &x, &y, &z);
#ifdef VERBOSE_RENDER_LOGGING
printf("Camera position %.2f, %.2f\n", x, y);
printf("Camera LR %.2f, %.2f\n", (x+c->coverage_x), (y+c->coverage_y));
printf("Camera coverage: %.2f, %.2f\n", c->coverage_x, c->coverage_y);
#endif
float cl = x; // left
float cr = x + c->coverage_x; // right
float cu = y; // up
float cd = y + c->coverage_y; // down
#ifdef VERBOSE_RENDER_LOGGING
printf("== Orig Cam: (%.2f, %.2f) - (%.2f %.2f)\n",
cl, cu, cr, cd);
#endif
// setup stack
ModelDrawStackNode snode;
snode.col = cols - 1;
snode.row = rows - 1;
snode.span_x = cols - 1;
snode.span_y = rows - 1;
snode.next_subgrid = NW_CORNER;
dstack.push( snode );
// run through iterative quadtree like algorithm
glEnable(GL_TEXTURE_2D);
// glColor3f(1,1,1);
while( dstack.size() > 0)
{
// make sure that the space between upper left most node to
// lower right node of sub grid intersects the camera space
// otherwise pop the dstack and continue loop
float ulx, uly, llx, lly; // upper left x,y and lower left x,y
int ulc, ulr, llc, llr; // uper left col, row and lower left col,row
ulc = dstack.top().col - dstack.top().span_x;
ulr = dstack.top().row - dstack.top().span_y;
llc = dstack.top().col;
llr = dstack.top().row;
ModelNode* n;
n = &(grid[ (ulr * cols) + ulc ]);
ulx = n->x;
uly = n->y;
n = &(grid[ (llr * cols) + llc ]);
llx = n->x + n->w;
lly = n->y + n->h;
// check horizontal or vertical depth
// notice comparision should occure in model's origin/space/coord sys
float _cl, _cu, _cr, _cd;
if(get_horizontal_depth())
{
_cl = cl;
_cu = cu;
_cr = cr;
_cd = cd;
} else {
_cl = cu;
_cu = -cr;
_cr = cd;
_cd = -cl;
}
#ifdef VERBOSE_RENDER_LOGGING
printf("Checking If Nodes (%d, %d) to (%d, %d) intersect\n",
ulc, ulr, llc, llr);
#endif
if(ulx > _cr || uly > _cd || llx < _cl || lly < _cu)
{
#ifdef VERBOSE_RENDER_LOGGING
printf("\tModel Area of (%.2f,%.2f) to (%.2f,%.2f) SKIPPED!\n",
ulx, uly, llx, lly);
printf("\tCamera space covering (%.2f,%.2f) to (%.2f,%.2f)\n",
_cl, _cu, _cr, _cd);
#endif
dstack.pop();
continue;
}
#ifdef VERBOSE_RENDER_LOGGING
else
{
printf("\tModel Area of (%.2f,%.2f) to (%.2f,%.2f) OK!\n",
ulx,uly,llx,lly);
printf("\tCamera space covering (%.2f,%.2f) to (%.2f,%.2f)\n",
_cl, _cu, _cr, _cd);
}
#endif
// is the span zero?
if( dstack.top().span_x == 0 && dstack.top().span_y == 0)
{
// draw
n = &(grid[ (dstack.top().row * cols ) + dstack.top().col]);
dstack.pop();
if(!n)
continue;
#ifdef VERBOSE_RENDER_LOGGING
printf("\t!!! Drawing node at col: %d, row %d !!!\n", n->col,
n->row);
printf("\t!!! Starts at (%.2f, %.2f) to (%.2f, %.2f) !!!\n",
n->x, n->y, n->x + n->w, n->y + n->h);
#endif
// 6/29/2012 brg: don't draw again if node has already been drawn!
nodeDrawCounts[ n->row * cols + n->col ]++;
if ( nodeDrawCounts[ n->row * cols + n->col ] > 1 )
continue;
bind_texblock( m->src, l, n->col, n->row);
float *nwtc = &(n->tex_crd[0][0]);
float *setc = &(n->tex_crd[1][0]);
glBegin(GL_QUADS);
{
glTexCoord2f( nwtc[TEX_S], nwtc[TEX_T]);
glVertex2f( n->x, n->y );
glTexCoord2f( nwtc[TEX_S], setc[TEX_T]);
glVertex2f( n->x, n->y + n->h );
glTexCoord2f( setc[TEX_S], setc[TEX_T]);
glVertex2f( n->x + n->w, n->y + n->h );
glTexCoord2f( setc[TEX_S], nwtc[TEX_T]);
glVertex2f( n->x + n->w, n->y );
}
glEnd();
continue;
}
switch( dstack.top().next_subgrid )
{
case NW_CORNER:
dstack.top().next_subgrid = NE_CORNER;
snode.span_x = dstack.top().span_x / 2;
snode.span_y = dstack.top().span_y / 2;
snode.col = dstack.top().col - int(ceil(dstack.top().span_x * .5));
snode.row = dstack.top().row - int(ceil(dstack.top().span_y * .5));
snode.next_subgrid = NW_CORNER;
#ifdef VERBOSE_RENDER_LOGGING
printf("NW CORNER: Pushing node (%d, %d) onto stack\n",
snode.col, snode.row);
#endif
dstack.push(snode);
break;
case NE_CORNER:
dstack.top().next_subgrid = SE_CORNER;
snode.span_x = dstack.top().span_x / 2;
snode.span_y = dstack.top().span_y / 2;
snode.col = dstack.top().col;
snode.row = dstack.top().row - int(ceil(dstack.top().span_y * .5));
snode.next_subgrid = NW_CORNER;
#ifdef VERBOSE_RENDER_LOGGING
printf("NE CORNER: Pushing node (%d, %d) onto stack\n",
snode.col, snode.row);
#endif
dstack.push(snode);
break;
case SE_CORNER:
dstack.top().next_subgrid = SW_CORNER;
snode.span_x = dstack.top().span_x / 2;
snode.span_y = dstack.top().span_y / 2;
snode.col = dstack.top().col;
snode.row = dstack.top().row;
snode.next_subgrid = NW_CORNER;
#ifdef VERBOSE_RENDER_LOGGING
printf("SE CORNER: Pushing node (%d, %d) onto stack\n",
snode.col, snode.row);
#endif
dstack.push(snode);
break;
case SW_CORNER:
dstack.top().next_subgrid = UNDEF_CORNER;
snode.span_x = dstack.top().span_x / 2;
snode.span_y = dstack.top().span_y / 2;
snode.col = dstack.top().col - int(ceil(dstack.top().span_x * .5));
snode.row = dstack.top().row;
snode.next_subgrid = NW_CORNER;
#ifdef VERBOSE_RENDER_LOGGING
printf("SW CORNER: Pushing node (%d, %d) onto stack\n",
snode.col, snode.row);
#endif
dstack.push(snode);
break;
case UNDEF_CORNER:
default:
dstack.pop();
break;
} // end switch to process next sub grid or not
} // end while stack not empty
delete[] nodeDrawCounts; // 6/29/2012 brg
// go through the whole grid and draw the outline of the blocks
if (getDebug()) {
int i, j;
glBindTexture(GL_TEXTURE_2D, 0);
glColor3f(1,0,0);
glBegin(GL_LINES);
for( i = 0; i < rows; ++i)
{
for( j = 0; j < cols; ++j)
{
int id = (i * cols) + j;
glVertex2f( grid[id].x + grid[id].w, grid[id].y);
glVertex2f( grid[id].x, grid[id].y);
glVertex2f( grid[id].x, grid[id].y);
glVertex2f( grid[id].x, grid[id].y + grid[id].h);
glVertex2f( grid[id].x, grid[id].y + grid[id].h);
glVertex2f( grid[id].x + grid[id].w, grid[id].y + grid[id].h);
glVertex2f( grid[id].x + grid[id].w, grid[id].y + grid[id].h);
glVertex2f( grid[id].x + grid[id].w, grid[id].y);
}
}
glEnd();
}
}
int main( int argc, char **argv ) {
mpconfig *control;
char *path;
FILE *pidlog=NULL;
struct timeval tv;
control=readConfig( );
if( control == NULL ) {
printf( "music directory needs to be set.\n" );
printf( "It will be set up now\n" );
path=(char *)falloc( MAXPATHLEN+1, 1 );
while( 1 ) {
printf( "Default music directory:" );
fflush( stdout );
memset( path, 0, MAXPATHLEN );
fgets(path, MAXPATHLEN, stdin );
path[strlen( path )-1]=0; /* cut off CR */
abspath( path, getenv( "HOME" ), MAXPATHLEN );
if( isDir( path ) ) {
break;
}
else {
printf( "%s is not a directory!\n", path );
}
}
writeConfig( path );
free( path );
control=readConfig();
if( control == NULL ) {
printf( "Could not create config file!\n" );
return 1;
}
}
muteVerbosity();
/* improve 'randomization' */
gettimeofday( &tv,NULL );
srand( (getpid()*tv.tv_usec)%RAND_MAX );
switch( getArgs( argc, argv ) ) {
case 0: /* no arguments given */
break;
case 1: /* stream - does this even make sense? */
break;
case 2: /* single file */
break;
case 3: /* directory */
/* if no default directory is set, use the one given */
if( control->musicdir == NULL ) {
incDebug();
addMessage( 0, "Setting default configuration values and initializing..." );
setProfile( control );
if( control->root == NULL ) {
addMessage( 0, "No music found at %s!", control->musicdir );
return -1;
}
addMessage( 0, "Initialization successful!" );
writeConfig( argv[optind] );
freeConfig( );
return 0;
}
break;
case 4: /* playlist */
break;
default:
addMessage( 0, "Unknown argument!\n", argv[optind] );
return -1;
}
snprintf( control->pidpath, MAXPATHLEN, "%s/.mixplay/mixplayd.pid", getenv("HOME") );
if( access( control->pidpath, F_OK ) == 0 ) {
addMessage( 0, "Mixplayd is already running!" );
freeConfig();
return -1;
}
signal(SIGINT, sigint );
signal(SIGTERM, sigint );
/* daemonization must happen before childs are created otherwise the pipes are cut */
if( getDebug() == 0 ) {
daemon( 0, 1 );
openlog ("mixplayd", LOG_PID, LOG_DAEMON);
control->isDaemon=1;
pidlog=fopen( control->pidpath, "w");
if( pidlog == NULL ) {
addMessage( 0, "Cannot open %s!", control->pidpath );
return -1;
}
fprintf( pidlog, "%i", getpid() );
fclose(pidlog);
}
addUpdateHook( &s_updateHook );
control->inUI=1;
initAll( );
#ifdef EPAPER
sleep(1);
if( control->status != mpc_quit ) {
epSetup();
addUpdateHook( &ep_updateHook );
}
#endif
pthread_join( control->stid, NULL );
pthread_join( control->rtid, NULL );
control->inUI=0;
#ifdef EPAPER
epExit();
#endif
if( control->changed ) {
writeConfig( NULL );
}
unlink(control->pidpath);
addMessage( 0, "Daemon terminated" );
freeConfig( );
return 0;
}
void Ukf::correct(const Measurement &measurement)
{
if (getDebug())
{
*debugStream_ << "---------------------- Ukf::correct ----------------------\n";
*debugStream_ << "State is:\n";
*debugStream_ << state_ << "\n";
*debugStream_ << "Measurement is:\n";
*debugStream_ << measurement.measurement_ << "\n";
*debugStream_ << "Measurement covariance is:\n";
*debugStream_ << measurement.covariance_ << "\n";
}
// In our implementation, it may be that after we call predict once, we call correct
// several times in succession (multiple measurements with different time stamps). In
// that event, the sigma points need to be updated to reflect the current state.
if(!uncorrected_)
{
// Take the square root of a small fraction of the estimateErrorCovariance_ using LL' decomposition
weightedCovarSqrt_ = ((STATE_SIZE + lambda_) * estimateErrorCovariance_).llt().matrixL();
// Compute sigma points
// First sigma point is the current state
sigmaPoints_[0] = state_;
// Next STATE_SIZE sigma points are state + weightedCovarSqrt_[ith column]
// STATE_SIZE sigma points after that are state - weightedCovarSqrt_[ith column]
for(size_t sigmaInd = 0; sigmaInd < STATE_SIZE; ++sigmaInd)
{
sigmaPoints_[sigmaInd + 1] = state_ + weightedCovarSqrt_.col(sigmaInd);
sigmaPoints_[sigmaInd + 1 + STATE_SIZE] = state_ - weightedCovarSqrt_.col(sigmaInd);
}
}
// We don't want to update everything, so we need to build matrices that only update
// the measured parts of our state vector
// First, determine how many state vector values we're updating
std::vector<size_t> updateIndices;
for (size_t i = 0; i < measurement.updateVector_.size(); ++i)
{
if (measurement.updateVector_[i])
{
// Handle nan and inf values in measurements
if (std::isnan(measurement.measurement_(i)))
{
if (getDebug())
{
*debugStream_ << "Value at index " << i << " was nan. Excluding from update.\n";
}
}
else if (std::isinf(measurement.measurement_(i)))
{
if (getDebug())
{
*debugStream_ << "Value at index " << i << " was inf. Excluding from update.\n";
}
}
else
{
updateIndices.push_back(i);
}
}
}
if (getDebug())
{
*debugStream_ << "Update indices are:\n";
*debugStream_ << updateIndices << "\n";
}
size_t updateSize = updateIndices.size();
// Now set up the relevant matrices
Eigen::VectorXd stateSubset(updateSize); // x (in most literature)
Eigen::VectorXd measurementSubset(updateSize); // z
Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize); // R
Eigen::MatrixXd stateToMeasurementSubset(updateSize, STATE_SIZE); // H
Eigen::MatrixXd kalmanGainSubset(STATE_SIZE, updateSize); // K
Eigen::VectorXd innovationSubset(updateSize); // z - Hx
Eigen::VectorXd predictedMeasurement(updateSize);
Eigen::VectorXd sigmaDiff(updateSize);
Eigen::MatrixXd predictedMeasCovar(updateSize, updateSize);
Eigen::MatrixXd crossCovar(STATE_SIZE, updateSize);
std::vector<Eigen::VectorXd> sigmaPointMeasurements(sigmaPoints_.size(), Eigen::VectorXd(updateSize));
stateSubset.setZero();
measurementSubset.setZero();
measurementCovarianceSubset.setZero();
stateToMeasurementSubset.setZero();
kalmanGainSubset.setZero();
innovationSubset.setZero();
predictedMeasurement.setZero();
predictedMeasCovar.setZero();
crossCovar.setZero();
// Now build the sub-matrices from the full-sized matrices
for (size_t i = 0; i < updateSize; ++i)
{
measurementSubset(i) = measurement.measurement_(updateIndices[i]);
stateSubset(i) = state_(updateIndices[i]);
for (size_t j = 0; j < updateSize; ++j)
{
measurementCovarianceSubset(i, j) = measurement.covariance_(updateIndices[i], updateIndices[j]);
}
// Handle negative (read: bad) covariances in the measurement. Rather
// than exclude the measurement or make up a covariance, just take
// the absolute value.
if (measurementCovarianceSubset(i, i) < 0)
{
if (getDebug())
{
*debugStream_ << "WARNING: Negative covariance for index " << i << " of measurement (value is" << measurementCovarianceSubset(i, i)
<< "). Using absolute value...\n";
}
measurementCovarianceSubset(i, i) = ::fabs(measurementCovarianceSubset(i, i));
}
// If the measurement variance for a given variable is very
// near 0 (as in e-50 or so) and the variance for that
// variable in the covariance matrix is also near zero, then
// the Kalman gain computation will blow up. Really, no
// measurement can be completely without error, so add a small
// amount in that case.
if (measurementCovarianceSubset(i, i) < 1e-6)
{
measurementCovarianceSubset(i, i) = 1e-6;
if (getDebug())
{
*debugStream_ << "WARNING: measurement had very small error covariance for index " << updateIndices[i] << ". Adding some noise to maintain filter stability.\n";
}
}
}
// The state-to-measurement function, h, will now be a measurement_size x full_state_size
// matrix, with ones in the (i, i) locations of the values to be updated
for (size_t i = 0; i < updateSize; ++i)
{
stateToMeasurementSubset(i, updateIndices[i]) = 1;
}
if (getDebug())
{
*debugStream_ << "Current state subset is:\n";
*debugStream_ << stateSubset << "\n";
*debugStream_ << "Measurement subset is:\n";
*debugStream_ << measurementSubset << "\n";
*debugStream_ << "Measurement covariance subset is:\n";
*debugStream_ << measurementCovarianceSubset << "\n";
*debugStream_ << "State-to-measurement subset is:\n";
*debugStream_ << stateToMeasurementSubset << "\n";
}
// (1) Generate sigma points, use them to generate a predicted measurement
for(size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
{
sigmaPointMeasurements[sigmaInd] = stateToMeasurementSubset * sigmaPoints_[sigmaInd];
predictedMeasurement += stateWeights_[sigmaInd] * sigmaPointMeasurements[sigmaInd];
}
// (2) Use the sigma point measurements and predicted measurement to compute a predicted
// measurement covariance matrix P_zz and a state/measurement cross-covariance matrix P_xz.
for(size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
{
sigmaDiff = sigmaPointMeasurements[sigmaInd] - predictedMeasurement;
predictedMeasCovar += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose());
crossCovar += covarWeights_[sigmaInd] * ((sigmaPoints_[sigmaInd] - state_) * sigmaDiff.transpose());
}
// (3) Compute the Kalman gain, making sure to use the actual measurement covariance: K = P_xz * (P_zz + R)^-1
Eigen::MatrixXd invInnovCov = (predictedMeasCovar + measurementCovarianceSubset).inverse();
kalmanGainSubset = crossCovar * invInnovCov;
// (4) Apply the gain to the difference between the actual and predicted measurements: x = x + K(z - z_hat)
innovationSubset = (measurementSubset - predictedMeasurement);
// (5) Check Mahalanobis distance of innovation
if (checkMahalanobisThreshold(innovationSubset, invInnovCov, measurement.mahalanobisTh_))
{
// Wrap angles in the innovation
for (size_t i = 0; i < updateSize; ++i)
{
if (updateIndices[i] == StateMemberRoll || updateIndices[i] == StateMemberPitch || updateIndices[i] == StateMemberYaw)
{
if (innovationSubset(i) < -pi_)
{
innovationSubset(i) += tau_;
}
else if (innovationSubset(i) > pi_)
{
innovationSubset(i) -= tau_;
}
}
}
state_ = state_ + kalmanGainSubset * innovationSubset;
// (6) Compute the new estimate error covariance P = P - (K * P_zz * K')
estimateErrorCovariance_ = estimateErrorCovariance_.eval() - (kalmanGainSubset * predictedMeasCovar * kalmanGainSubset.transpose());
wrapStateAngles();
// Mark that we need to re-compute sigma points for successive corrections
uncorrected_ = false;
if (getDebug())
{
*debugStream_ << "Predicated measurement covariance is:\n";
*debugStream_ << predictedMeasCovar << "\n";
*debugStream_ << "Cross covariance is:\n";
*debugStream_ << crossCovar << "\n";
*debugStream_ << "Kalman gain subset is:\n";
*debugStream_ << kalmanGainSubset << "\n";
*debugStream_ << "Innovation:\n";
*debugStream_ << innovationSubset << "\n\n";
*debugStream_ << "Corrected full state is:\n";
*debugStream_ << state_ << "\n";
*debugStream_ << "Corrected full estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
*debugStream_ << "\n---------------------- /Ukf::correct ----------------------\n";
}
}
}
void Ukf::predict(const double delta)
{
if (getDebug())
{
*debugStream_ << "---------------------- Ukf::predict ----------------------\n";
*debugStream_ << "delta is " << delta << "\n";
*debugStream_ << "state is " << state_ << "\n";
}
double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);
double xVel = state_(StateMemberVx);
double yVel = state_(StateMemberVy);
double zVel = state_(StateMemberVz);
double xAcc = state_(StateMemberAx);
double yAcc = state_(StateMemberAy);
double zAcc = state_(StateMemberAz);
// We'll need these trig calculations a lot.
double cr = cos(roll);
double cp = cos(pitch);
double cy = cos(yaw);
double sr = sin(roll);
double sp = sin(pitch);
double sy = sin(yaw);
// Prepare the transfer function
transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
transferFunction_(StateMemberPitch, StateMemberVpitch) = delta;
transferFunction_(StateMemberYaw, StateMemberVyaw) = delta;
transferFunction_(StateMemberVx, StateMemberAx) = delta;
transferFunction_(StateMemberVy, StateMemberAy) = delta;
transferFunction_(StateMemberVz, StateMemberAz) = delta;
// (1) Take the square root of a small fraction of the estimateErrorCovariance_ using LL' decomposition
weightedCovarSqrt_ = ((STATE_SIZE + lambda_) * estimateErrorCovariance_).llt().matrixL();
// (2) Compute sigma points *and* pass them through the transfer function to save
// the extra loop
// First sigma point is the current state
sigmaPoints_[0] = transferFunction_ * state_;
// Next STATE_SIZE sigma points are state + weightedCovarSqrt_[ith column]
// STATE_SIZE sigma points after that are state - weightedCovarSqrt_[ith column]
for(size_t sigmaInd = 0; sigmaInd < STATE_SIZE; ++sigmaInd)
{
sigmaPoints_[sigmaInd + 1] = transferFunction_ * (state_ + weightedCovarSqrt_.col(sigmaInd));
sigmaPoints_[sigmaInd + 1 + STATE_SIZE] = transferFunction_ * (state_ - weightedCovarSqrt_.col(sigmaInd));
}
// (3) Sum the weighted sigma points to generate a new state prediction
state_.setZero();
for(size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
{
state_ += stateWeights_[sigmaInd] * sigmaPoints_[sigmaInd];
}
// (4) Now us the sigma points and the predicted state to compute a predicted covariance
estimateErrorCovariance_.setZero();
Eigen::VectorXd sigmaDiff(STATE_SIZE);
for(size_t sigmaInd = 0; sigmaInd < sigmaPoints_.size(); ++sigmaInd)
{
sigmaDiff = (sigmaPoints_[sigmaInd] - state_);
estimateErrorCovariance_ += covarWeights_[sigmaInd] * (sigmaDiff * sigmaDiff.transpose());
}
// (5) Not strictly in the theoretical UKF formulation, but necessary here
// to ensure that we actually incorporate the processNoiseCovariance_
estimateErrorCovariance_ += delta * processNoiseCovariance_;
// Keep the angles bounded
wrapStateAngles();
// Mark that we can keep these sigma points
uncorrected_ = true;
if (getDebug())
{
*debugStream_ << "Predicted state is:\n";
*debugStream_ << state_ << "\n";
*debugStream_ << "Predicted estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
*debugStream_ << "\n--------------------- /Ukf::predict ----------------------\n";
}
}
void readFromPipe(HANDLE *hChildStdoutRdDup) {
DWORD dwRead;
HANDLE hStdout = GetStdHandle(STD_OUTPUT_HANDLE);
char chBuff[BUFSIZ];
char *pchBuff, *pdelimiter;
int lineCnt=0, ii=0, iRet=0, startOfCommandFound=FALSE, endOfCommandFound=FALSE;
int foundSync=FALSE, conFlag=FALSE;
int *pfoundSync, *plineCnt, *pconFlag;
// set buffer pointer to buffer
pchBuff=chBuff;
pdelimiter = delimiter;
pfoundSync = &foundSync;
plineCnt = &lineCnt;
pconFlag = &conFlag;
// Read output from the child process, and write to parent's STDOUT.
lineCnt=0;
//PrintEvent(_T("Reading output of process..."));
for (;;) {
// read in one less than buffer so null pointer can be added
if( !ReadFile( hChildStdoutRdDup, chBuff, BUFSIZ - 1, &dwRead, NULL) || dwRead == 0) {
//ErrorPrinter(_T("ReadFile Returned a False"), GetLastError());
if(getDebug()) {
printf("ReadFile Returned False %d lines have been read", lineCnt);
}
break;
}
// null terminate string for search, should need to check length, but just make sure
// fgets does this automatically
if (dwRead < BUFSIZ) {
chBuff[dwRead] = '\0';
}
if(getDebug()) {
// send string to stdout
printf("\n\n\n>>Number Read in: %d\n%s", dwRead, chBuff);
//WriteFile(hStdout, chBuff, dwRead, &dwWritten, NULL);
}
iRet=digestData(pchBuff, delimiter, pfoundSync, plineCnt, pconFlag);
if(iRet == 1) {
// start of command was found, so command should be okay
printDebug("*** digestData returned START OF COMMAND FOUND");
startOfCommandFound=TRUE;
} else if (iRet == 2) {
// end of command was found, so command should be okay
printDebug("*** digestData returned END OF COMMAND FOUND");
endOfCommandFound=TRUE;
} else if (iRet == 3) {
// both start and end of command was found, so command should be okay
printDebug("*** digestData returned START AND END OF COMMAND FOUND");
startOfCommandFound=TRUE;
endOfCommandFound=TRUE;
}
}
if (!startOfCommandFound || !endOfCommandFound) {
// error either start of end of command was not found
errorExit("Either the Start or End command tokens where not found by the native launcher. Set the \"DEBUGX\" environment variable and re-run to debug the problem.");
}
if(getDebug()) {
for(ii=0; ii < lineCnt; ii++) {
printf("\n%d = %s", ii, commandLine[ii]);
}
}
}
void Ekf::predict(const double delta)
{
if (getDebug())
{
*debugStream_ << "---------------------- Ekf::predict ----------------------\n";
*debugStream_ << "delta is " << delta << "\n";
*debugStream_ << "state is " << state_ << "\n";
}
double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);
double xVel = state_(StateMemberVx);
double yVel = state_(StateMemberVy);
double zVel = state_(StateMemberVz);
double xAcc = state_(StateMemberAx);
double yAcc = state_(StateMemberAy);
double zAcc = state_(StateMemberAz);
// We'll need these trig calculations a lot.
double cr = cos(roll);
double cp = cos(pitch);
double cy = cos(yaw);
double sr = sin(roll);
double sp = sin(pitch);
double sy = sin(yaw);
// Prepare the transfer function
transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
transferFunction_(StateMemberPitch, StateMemberVpitch) = delta;
transferFunction_(StateMemberYaw, StateMemberVyaw) = delta;
transferFunction_(StateMemberVx, StateMemberAx) = delta;
transferFunction_(StateMemberVy, StateMemberAy) = delta;
transferFunction_(StateMemberVz, StateMemberAz) = delta;
// Prepare the transfer function Jacobian. This function is analytically derived from the
// transfer function.
double xCoeff = 0.0;
double yCoeff = 0.0;
double zCoeff = 0.0;
double oneHalfATSquared = 0.5 * delta * delta;
yCoeff = cy * sp * cr + sy * sr;
zCoeff = -cy * sp * sr + sy * cr;
double dF0dr = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = -cy * sp;
yCoeff = cy * cp * sr;
zCoeff = cy * cp * cr;
double dF0dp = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = -sy * cp;
yCoeff = -sy * sp * sr - cy * cr;
zCoeff = -sy * sp * cr + cy * sr;
double dF0dy = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
yCoeff = sy * sp * cr - cy * sr;
zCoeff = -sy * sp * sr - cy * cr;
double dF1dr = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = -sy * sp;
yCoeff = sy * cp * sr;
zCoeff = sy * cp * cr;
double dF1dp = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = cy * cp;
yCoeff = cy * sp * sr - sy * cr;
zCoeff = cy * sp * cr + sy * sr;
double dF1dy = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
yCoeff = cp * cr;
zCoeff = -cp * sr;
double dF2dr = (yCoeff * yVel + zCoeff * zVel) * delta +
(yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
xCoeff = -cp;
yCoeff = -sp * sr;
zCoeff = -sp * cr;
double dF2dp = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
(xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
// Much of the transfer function Jacobian is identical to the transfer function
transferFunctionJacobian_ = transferFunction_;
transferFunctionJacobian_(StateMemberX, StateMemberRoll) = dF0dr;
transferFunctionJacobian_(StateMemberX, StateMemberPitch) = dF0dp;
transferFunctionJacobian_(StateMemberX, StateMemberYaw) = dF0dy;
transferFunctionJacobian_(StateMemberY, StateMemberRoll) = dF1dr;
transferFunctionJacobian_(StateMemberY, StateMemberPitch) = dF1dp;
transferFunctionJacobian_(StateMemberY, StateMemberYaw) = dF1dy;
transferFunctionJacobian_(StateMemberZ, StateMemberRoll) = dF2dr;
transferFunctionJacobian_(StateMemberZ, StateMemberPitch) = dF2dp;
if (getDebug())
{
*debugStream_ << "Transfer function is:\n";
*debugStream_ << transferFunction_ << "\n";
*debugStream_ << "Transfer function Jacobian is:\n";
*debugStream_ << transferFunctionJacobian_ << "\n";
*debugStream_ << "Process noise covariance is " << "\n";
*debugStream_ << processNoiseCovariance_ << "\n";
*debugStream_ << "Current state is:\n";
*debugStream_ << state_ << "\n";
}
// (1) Project the state forward: x = Ax (really, x = f(x))
state_ = transferFunction_ * state_;
// Handle wrapping
wrapStateAngles();
if (getDebug())
{
*debugStream_ << "Predicted state is:\n";
*debugStream_ << state_ << "\n";
*debugStream_ << "Current estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
}
// (2) Project the error forward: P = J * P * J' + Q
estimateErrorCovariance_ = (transferFunctionJacobian_ * estimateErrorCovariance_ * transferFunctionJacobian_.transpose())
+ (processNoiseCovariance_ * delta);
if (getDebug())
{
*debugStream_ << "Predicted estimate error covariance is:\n";
*debugStream_ << estimateErrorCovariance_ << "\n";
*debugStream_ << "\n--------------------- /Ekf::predict ----------------------\n";
}
}
/*
Tokens:
ARCH Build architecture (64)
GCC_ARCH ARCH mapped to gcc -arch switches (x86_64)
CC Compiler (cc)
DEBUG Debug compilation options (-g, -Zi -Od)
INC Include directory out/inc
LIB Library directory (out/lib, xcode/VS: out/bin)
LIBS Libraries required to link with ESP
OBJ Name of compiled source (out/lib/view-MD5.o)
MOD Output module (view_MD5.dylib)
SHLIB Host Shared library (.lib, .so)
SHOBJ Host Shared Object (.dll, .so)
SRC Source code for view or controller (already templated)
TMP Temp directory
VS Visual Studio directory
WINSDK Windows SDK directory
*/
PUBLIC char *espExpandCommand(EspRoute *eroute, cchar *command, cchar *source, cchar *module)
{
MprBuf *buf;
MaAppweb *appweb;
cchar *cp, *outputModule, *os, *arch, *profile;
char *tmp;
if (command == 0) {
return 0;
}
appweb = MPR->appwebService;
outputModule = mprTrimPathExt(module);
maParsePlatform(appweb->platform, &os, &arch, &profile);
buf = mprCreateBuf(-1, -1);
for (cp = command; *cp; ) {
if (*cp == '$') {
if (matchToken(&cp, "${ARCH}")) {
/* Target architecture (x86|mips|arm|x64) */
mprPutStringToBuf(buf, arch);
} else if (matchToken(&cp, "${GCC_ARCH}")) {
/* Target architecture mapped to GCC mtune|mcpu values */
mprPutStringToBuf(buf, getMappedArch(arch));
} else if (matchToken(&cp, "${INC}")) {
/* Include directory for the configuration */
mprPutStringToBuf(buf, mprJoinPath(appweb->platformDir, "inc"));
} else if (matchToken(&cp, "${LIBPATH}")) {
/* Library directory for Appweb libraries for the target */
mprPutStringToBuf(buf, mprJoinPath(appweb->platformDir, "bin"));
} else if (matchToken(&cp, "${LIBS}")) {
/* Required libraries to link. These may have nested ${TOKENS} */
mprPutStringToBuf(buf, espExpandCommand(eroute, getLibs(os), source, module));
} else if (matchToken(&cp, "${MOD}")) {
/* Output module path in the cache without extension */
mprPutStringToBuf(buf, outputModule);
} else if (matchToken(&cp, "${OBJ}")) {
/* Output object with extension (.o) in the cache directory */
mprPutStringToBuf(buf, mprJoinPathExt(outputModule, getObjExt(os)));
} else if (matchToken(&cp, "${OS}")) {
/* Target architecture (freebsd|linux|macosx|windows|vxworks) */
mprPutStringToBuf(buf, os);
} else if (matchToken(&cp, "${SHLIB}")) {
/* .dll, .so, .dylib */
mprPutStringToBuf(buf, getShlibExt(os));
} else if (matchToken(&cp, "${SHOBJ}")) {
/* .dll, .so, .dylib */
mprPutStringToBuf(buf, getShobjExt(os));
} else if (matchToken(&cp, "${SRC}")) {
/* View (already parsed into C code) or controller source */
mprPutStringToBuf(buf, source);
} else if (matchToken(&cp, "${TMP}")) {
if ((tmp = getenv("TMPDIR")) == 0) {
if ((tmp = getenv("TMP")) == 0) {
tmp = getenv("TEMP");
}
}
mprPutStringToBuf(buf, tmp ? tmp : ".");
} else if (matchToken(&cp, "${VS}")) {
mprPutStringToBuf(buf, getVisualStudio());
} else if (matchToken(&cp, "${VXCPU}")) {
mprPutStringToBuf(buf, getVxCPU(arch));
} else if (matchToken(&cp, "${WINSDK}")) {
mprPutStringToBuf(buf, getWinSDK());
/*
These vars can be configured from environment variables.
NOTE: the default esp.conf includes "esp->vxworks.conf" which has EspEnv definitions for the configured
VxWorks toolchain
*/
} else if (matchToken(&cp, "${CC}")) {
mprPutStringToBuf(buf, getEnvString(eroute, "CC", getCompilerPath(os, arch)));
} else if (matchToken(&cp, "${LINK}")) {
mprPutStringToBuf(buf, getEnvString(eroute, "LINK", ""));
} else if (matchToken(&cp, "${CFLAGS}")) {
mprPutStringToBuf(buf, getEnvString(eroute, "CFLAGS", ""));
} else if (matchToken(&cp, "${DEBUG}")) {
mprPutStringToBuf(buf, getEnvString(eroute, "DEBUG", getDebug()));
} else if (matchToken(&cp, "${LDFLAGS}")) {
mprPutStringToBuf(buf, getEnvString(eroute, "LDFLAGS", ""));
} else if (matchToken(&cp, "${WIND_BASE}")) {
mprPutStringToBuf(buf, getEnvString(eroute, "WIND_BASE", WIND_BASE));
} else if (matchToken(&cp, "${WIND_HOME}")) {
mprPutStringToBuf(buf, getEnvString(eroute, "WIND_HOME", WIND_HOME));
} else if (matchToken(&cp, "${WIND_HOST_TYPE}")) {
mprPutStringToBuf(buf, getEnvString(eroute, "WIND_HOST_TYPE", WIND_HOST_TYPE));
} else if (matchToken(&cp, "${WIND_PLATFORM}")) {
mprPutStringToBuf(buf, getEnvString(eroute, "WIND_PLATFORM", WIND_PLATFORM));
} else if (matchToken(&cp, "${WIND_GNU_PATH}")) {
mprPutStringToBuf(buf, getEnvString(eroute, "WIND_GNU_PATH", WIND_GNU_PATH));
} else if (matchToken(&cp, "${WIND_CCNAME}")) {
mprPutStringToBuf(buf, getEnvString(eroute, "WIND_CCNAME", getCompilerName(os, arch)));
} else {
mprPutCharToBuf(buf, *cp++);
}
} else {
mprPutCharToBuf(buf, *cp++);
}
}
mprAddNullToBuf(buf);
return sclone(mprGetBufStart(buf));
}
static
int dnsSD_Request(HSP *sp, char *dname, uint16_t rtype, HSPDnsCB callback, HSPSFlowSettings *settings)
{
u_char buf[PACKETSZ];
// We could have a config option to set the DNS servers that we will ask. If so
// they should be written into this array of sockaddrs...
// myDebug(1,"_res_nsaddr=%p", &_res.nsaddr);
myDebug(1,"=== res_search(%s, C_IN, %u) ===", dname, rtype);
int anslen = res_search(dname, C_IN, rtype, buf, PACKETSZ);
if(anslen == -1) {
if(errno == 0 && (h_errno == HOST_NOT_FOUND || h_errno == NO_DATA)) {
// although res_search returned -1, the request did actually get an answer,
// it's just that there was no SRV record configured, or the response was
// not authoritative. Interpret this the same way as answer_count==0.
myDebug(1,"res_search(%s, C_IN, %u) came up blank (h_errno=%d)", dname, rtype, h_errno);
return 0;
}
else {
myLog(LOG_ERR,"res_search(%s, C_IN, %u) failed : %s (h_errno=%d)", dname, rtype, strerror(errno), h_errno);
return -1;
}
}
if(anslen < sizeof(HEADER)) {
myLog(LOG_ERR,"res_search(%s) returned %d (too short)", dname, anslen);
return -1;
}
HEADER *ans = (HEADER *)buf;
if(ans->rcode != NOERROR) {
myLog(LOG_ERR,"res_search(%s) returned response code %d", dname, ans->rcode);
return -1;
}
uint32_t answer_count = (ntohs(ans->ancount));
if(answer_count == 0) {
myLog(LOG_INFO,"res_search(%s) returned no answer", dname);
return 0;
}
myDebug(1, "dnsSD: answer_count = %d", answer_count);
u_char *p = buf + sizeof(HEADER);
u_char *endp = buf + anslen;
// consume query
int query_name_len = dn_skipname(p, endp);
if(query_name_len == -1) {
myLog(LOG_ERR,"dn_skipname() <query> failed");
return -1;
}
myDebug(1, "dnsSD: (compressed) query_name_len = %d", query_name_len);
p += (query_name_len);
p += QFIXEDSZ;
// collect array of results
for(int entry = 0; entry < answer_count; entry++) {
myDebug(1, "dnsSD: entry %d, bytes_left=%d", entry, (endp - p));
// consume name (again)
query_name_len = dn_skipname(p, endp);
if(query_name_len == -1) {
myLog(LOG_ERR,"dn_skipname() <ans> failed");
return -1;
}
p += (query_name_len);
// now p should be looking at:
// [type:16][class:16][ttl:32][len:16][record]
if((endp - p) <= 16) {
myLog(LOG_ERR,"ans %d of %d: ran off end -- only %d bytes left",
entry, answer_count, (endp-p));
return -1;
}
uint16_t res_typ = (p[0] << 8) | p[1];
uint16_t res_cls = (p[2] << 8) | p[3];
uint32_t res_ttl = (p[4] << 24) | (p[5] << 16) | (p[6] << 8) | p[7];
uint16_t res_len = (p[8] << 8) | p[9];
p += 10;
// use another pointer to walk the payload and move p to the next answer
u_char *x = p;
p += res_len;
uint16_t res_payload = res_len;
// sanity check
if(res_typ != rtype ||
res_cls != C_IN) {
myLog(LOG_ERR,"expected t=%d,c=%d, got t=%d,c=%d", rtype, C_IN, res_typ, res_cls);
return -1;
}
switch(rtype) {
case T_SRV:
{
// now x should see
// [priority:2][weight:2][port:2][FQDN:res_len-6]
uint16_t res_pri = (x[0] << 8) | x[1];
uint16_t res_wgt = (x[2] << 8) | x[3];
uint32_t res_prt = (x[4] << 8) | x[5];
x += 6;
res_payload -= 6;
// still got room for an FQDN?
if((endp - x) < HSP_MIN_DNAME) {
myLog(LOG_ERR,"no room for target name -- only %d bytes left", (endp - x));
return -1;
}
char fqdn[MAXDNAME];
int ans_len = dn_expand(buf, endp, x, fqdn, MAXDNAME);
if(ans_len == -1) {
myLog(LOG_ERR,"dn_expand() failed");
return -1;
}
// cross-check
if(ans_len != res_payload) {
myLog(LOG_ERR,"target name len cross-check failed");
return -1;
}
if(ans_len < HSP_MIN_DNAME) {
// just ignore this one -- e.g. might just be "."
}
else {
// fqdn[ans_len] = '\0';
myDebug(1, "answer %d is <%s>:<%u> (wgt=%d; pri=%d; ttl=%d; ans_len=%d; res_len=%d)",
entry,
fqdn,
res_prt,
res_wgt,
res_pri,
res_ttl,
ans_len,
res_len);
if(callback) {
char fqdn_port[PACKETSZ];
sprintf(fqdn_port, "%s/%u", fqdn, res_prt);
// use key == NULL to indicate that the value is host:port
(*callback)(sp, rtype, res_ttl, NULL, 0, (u_char *)fqdn_port, strlen(fqdn_port), settings);
}
}
}
break;
case T_TXT:
{
// now x should see
// [TXT:res_len]
// still got room for a text record?
if((endp - x) < HSP_MIN_TXT) {
myLog(LOG_ERR,"no room for text record -- only %d bytes left", (endp - x));
return -1;
}
if(getDebug()) {
printf("dsnSD TXT Record: ");
for(int i = 0; i < res_len; i++) {
int ch = x[i];
if(isalnum(ch)) printf("%c", ch);
else printf("{%02x}", ch);
}
printf("\n");
}
// format is [len][<key>=<val>][len][<key>=<val>]...
// so we can pull out the settings and give them directly
// to the callback fn without copying
u_char *txtend = x + res_len;
// need at least 3 chars for a var=val setting
while((txtend - x) >= 3) {
int pairlen = *x++;
int klen = strcspn((char *)x, "=");
if(klen < 0) {
myLog(LOG_ERR, "dsnSD TXT record not in var=val format: %s", x);
}
else {
if(callback) (*callback)(sp, rtype, res_ttl, x, klen, (x+klen+1), (pairlen - klen - 1), settings);
}
x += pairlen;
}
}
break;
default:
myLog(LOG_ERR, "unsupported query type: %u" , rtype);
return -1;
break;
}
}
return answer_count;
}
int executeJavaCommand(char *childExecutorFile, int iVerbose, int iLauncherReturn, char *displayName) {
SECURITY_ATTRIBUTES saAttr;
BOOL fSuccess;
PROCESS_INFORMATION piProcInfo;
STARTUPINFO siStartInfo;
BOOL bFuncRetn = FALSE;
DWORD dwWritten;
int ii=0;
DWORD dwFlag, dwRet;
HANDLE hChildStdinRd, hChildStdinWr, hChildStdinWrDup;
char *commandSyncStart = "STARTOFCOMMAND";
char *commandSyncEnd = "ENDOFCOMMAND";
HANDLE hStdin = GetStdHandle(STD_INPUT_HANDLE);
char chBuff[BUFSIZ];
char *startChar;
// The steps for redirecting child process's STDIN:
// 1. Save current STDIN, to be restored later.
// 2. Create anonymous pipe to be STDIN for child process.
// 3. Set STDIN of the parent to be the read handle to the
// pipe, so it is inherited by the child process.
// 4. Create a noninheritable duplicate of the write handle,
// and close the inheritable write handle.
// Set the bInheritHandle flag so pipe handles are inherited.
saAttr.nLength = sizeof(SECURITY_ATTRIBUTES);
saAttr.bInheritHandle = TRUE;
saAttr.lpSecurityDescriptor = NULL;
// Create a pipe for the child process's STDIN.
if (! CreatePipe(&hChildStdinRd, &hChildStdinWr, &saAttr, 0))
errorExit("Stdin pipe creation failed");
// Duplicate the write handle to the pipe so it is not inherited.
fSuccess = DuplicateHandle(GetCurrentProcess(), hChildStdinWr,
GetCurrentProcess(), &hChildStdinWrDup, 0,
FALSE, // not inherited
DUPLICATE_SAME_ACCESS);
if (! fSuccess)
errorExit("DuplicateHandle failed");
CloseHandle(hChildStdinWr);
// Now create the child process.
// Set up members of the PROCESS_INFORMATION structure.
ZeroMemory( &piProcInfo, sizeof(PROCESS_INFORMATION) );
// Set up members of the STARTUPINFO structure.
ZeroMemory( &siStartInfo, sizeof(STARTUPINFO) );
siStartInfo.cb = sizeof(STARTUPINFO);
siStartInfo.lpReserved = 0;
siStartInfo.lpDesktop = NULL;
siStartInfo.lpTitle = displayName;
siStartInfo.dwX = 0;
siStartInfo.dwY = 0;
siStartInfo.dwXSize = 0;
siStartInfo.dwYSize = 0;
siStartInfo.dwXCountChars = 0;
siStartInfo.dwYCountChars = 0;
siStartInfo.dwFillAttribute = 0;
siStartInfo.dwFlags = STARTF_USESHOWWINDOW | STARTF_USESTDHANDLES;
siStartInfo.wShowWindow = SW_SHOWDEFAULT;
siStartInfo.cbReserved2 = 0;
siStartInfo.lpReserved2 = 0;
siStartInfo.hStdInput = hChildStdinRd;
siStartInfo.hStdOutput = GetStdHandle(STD_OUTPUT_HANDLE);
siStartInfo.hStdError = GetStdHandle(STD_OUTPUT_HANDLE);
if (getDebug()) {
printf("\n\n*** Executing ->%s<-\n", childExecutorFile);
fflush(stdout);
}
// create console for verbose mode
if (iVerbose) {
dwFlag=CREATE_NEW_CONSOLE | CREATE_NEW_PROCESS_GROUP;
} else {
dwFlag=CREATE_NEW_PROCESS_GROUP;
}
// Create the child process.
bFuncRetn=CreateProcess(NULL,
childExecutorFile, // command line
NULL, // process security attributes
NULL, // primary thread security attributes
TRUE, // handles are inherited
dwFlag, // creation flags
NULL, // use parent's environment
NULL, // use parent's current directory
&siStartInfo, // STARTUPINFO pointer
&piProcInfo); // receives PROCESS_INFORMATION
//printf("\n*** Child Process ID = %s\n", piProcInfo.dwProcessId);
if (bFuncRetn == 0) {
ErrorPrinter(_T("Could Not Create Process"), GetLastError());
errorExit("CreateProcess for java command failed");
return 0;
}
if (getDebug()) printf("\n*** Writing to Child process' input using delimiter '%s'\n", delimiter); fflush(stdout);
// Write to pipe that is the standard input for a child process.
// write out start of command
WriteFile(hChildStdinWrDup, commandSyncStart, strlen(commandSyncStart), &dwWritten, NULL);
WriteFile(hChildStdinWrDup, delimiter, 1, &dwWritten, NULL);
while (commandLine[ii] != NULL) {
WriteFile(hChildStdinWrDup, commandLine[ii], strlen(commandLine[ii]), &dwWritten, NULL);
WriteFile(hChildStdinWrDup, delimiter, 1, &dwWritten, NULL);
ii++;
}
// writeout end command
WriteFile(hChildStdinWrDup, commandSyncEnd, strlen(commandSyncEnd), &dwWritten, NULL);
WriteFile(hChildStdinWrDup, delimiter, 1, &dwWritten, NULL);
// write out identity information that should be on the stdin
WriteFile(hChildStdinWrDup, "IDENTITYINFORMATION", 19, &dwWritten, NULL);
WriteFile(hChildStdinWrDup, delimiter, 1, &dwWritten, NULL);
while (fgets(chBuff, BUFSIZ, stdin) != NULL) {
//printf("%s", buff);
if(getDebug()) {
// send string to stdout
printf("\n\n\n>>STDIN Number Read in: %d - %s", strlen(chBuff), chBuff);
}
// parse for carrage return
startChar=strtok(chBuff, "\n");
while (startChar != NULL) {
if(getDebug()) {
// send string to pipe for child process
printf("Writing Identity element to input pipe - %s", startChar);
}
WriteFile(hChildStdinWrDup, startChar, strlen(startChar), &dwWritten, NULL);
WriteFile(hChildStdinWrDup, delimiter, 1, &dwWritten, NULL);
// get next token, if exists
startChar=strtok(NULL, "\n");
}
}
// Close the pipe handle so the child process stops reading.
if (! CloseHandle(hChildStdinWrDup)) {
errorExit("Close pipe failed");
}
if (iVerbose || iLauncherReturn) {
// Wait until process exits.
WaitForSingleObject(piProcInfo.hProcess, INFINITE);
GetExitCodeProcess(piProcInfo.hProcess, &dwRet);
if(getDebug()) printf("\nChildProcess returned with and exit code = %d\n", dwRet); fflush(stdout);
}
// need to detach parent from child
return dwRet;
}
int sendMessage(MESSAGE *m) {
int i, bcc = 0, error = 0;
char ocp[3], oc[3], cmd[3], head[7], message[m->message_length];
/* convert HEX to string */
if (m->message_type == 'A') {
convert(cmd, m->command);
} else {
convert(ocp, m->op_code_page);
convert(oc, m->op_code);
}
if (getDebug() > 0) {
puts(">>>> >>> >> >");
puts("I: Preparing message:");
}
/* create head string */
sprintf(head, "%c%c%c%c%c%s",
m->soh,
m->reserve,
m->destination,
m->source,
m->message_type,
int2strhex(m->message_length, 2)
);
if (getDebug() > 0) {
printf(" * Head = [");
print(7, head);
printf("]\n");
}
/* create message string */
if (m->message_type == 'A')
/* command */
sprintf(message, "%c%s%c",
m->stx,
cmd,
m->etx
);
else if (m->message_type == 'C')
/* get current parameter */
sprintf(message, "%c%s%s%c",
m->stx,
ocp,
oc,
m->etx
);
else if (m->message_type == 'E')
/* set parameter message */
sprintf(message, "%c%s%s%s%c",
m->stx,
ocp,
oc,
m->value,
m->etx
);
if (getDebug() > 0) {
printf(" * Message = [");
print(m->message_length, message);
printf("]\n");
}
/* count check code */
for (i=1; i<7; i++)
bcc ^= head[i];
for (i=0; i<m->message_length; i++)
bcc ^= message[i];
if (getDebug() > 0) {
printf(" * Check code = [%#x]\n", bcc);
printf(" * Delimiter = [%#x]\n", m->cr);
}
m->msg = calloc(19, sizeof(char));
sprintf(m->msg, "%s%s%c%c", head, message, bcc, m->cr);
m->replay = NULL;
send_message(m->msg, &m->replay);
if (getDebug() > 0 && m->replay == NULL)
perror("E: No replay!");
else if (getDebug() > 1 && m->replay != NULL) {
REPLAY r;
r.initReplay = initReplay;
r.initReplay(&r, &m->replay);
r.printReplay = printReplay;
r.printReplay(&r);
}
if (m->replay == NULL || strcmp(substr(m->replay, 8, 2), "00") != 0)
error = -1;
if (getDebug() > 0 ) puts("< << <<< <<<<");
return error;
}
/*!
** @brief Xsocket implemention of the standard getsockopt function.
**
** Xgetsockopt is used to retrieve the settings of the underlying Xsocket
** in the Click layer. It does not access the settings of the actual
** socket passed in which is used by the API to communicate with Click.
**
** Supported Options:
** \n XOPT_HLIM Retrieves the 'hop limit' element of the XIA header as an integer value
** \n XOPT_NEXT_PROTO Gets the next proto field in the XIA header
** \n SO_TYPE Returns the type of socket (SOCK_STREAM, etc...)
**
** @param sockfd The control socket
** @param optname The socket option to set (currently must be IP_TTL)
** @param optval A pointer to the value to retrieve
** @param optlen A pointer to the length of optval. On input this
** should be set to the length of the data passed in. If optlen is not
** large enough, Xgetsockopt will set it to the size needed and return
** with errno set to EINVAL. If larger than needed, Xgetsockopt will set
** it to the actual length of the returned data.
**
** @returns 0 on success
** @returns -1 on error with errno set
** @returns errno values:
** @returns EBADF: The socket descriptor is invalid
** @returns EINVAL: Either optval is null, or optlen is invalid, or optval is out of range
** @returns ENOPROTOOPT: the specified optname is not recognized
*/
int Xgetsockopt(int sockfd, int optname, void *optval, socklen_t *optlen)
{
int rc = 0;
if (getSocketType(sockfd) == XSOCK_INVALID) {
errno = EBADF;
return -1;
}
if (!optval || !optlen) {
errno = EINVAL;
return -1;
}
switch (optname) {
// get these values from click ******************************
case XOPT_HLIM:
case XOPT_NEXT_PROTO:
case XOPT_ERROR_PEEK:
case SO_ACCEPTCONN:
case SO_ERROR:
rc = ssoGetInt(sockfd, optname, (int *)optval, optlen);
break;
case SO_DEBUG:
// stored in the API & in click, return from the API
rc = ssoGetParam(getDebug(sockfd), optval, optlen);
break;
case SO_DOMAIN:
// FIXME: conver this to AF_INET in wrapper
rc = ssoGetParam(AF_XIA, optval, optlen);
break;
case SO_PROTOCOL:
rc = ssoGetParam(getProtocol(sockfd), optval, optlen);
break;
case SO_TYPE:
ssoGetParam(getSocketType(sockfd), optval, optlen);
break;
// SHOIULD IMPLEMENT! ***************************************
// FIXME: implement these!
case SO_SNDBUF:
case SO_RCVBUF:
case SO_SNDTIMEO:
case SO_RCVTIMEO:
case SO_LINGER:
LOGF("Uh Oh, we need support for %s in XIA\n", optValue(optname));
rc = -1;
break;
// FIXME: MAY NEED ******************************************
case SO_BROADCAST:
// FIXME: can we just go ahead and mark this as success?
// or do we need to do something to check to see if bradcast DAGS are allowed on this socket?
if (getSocketType(sockfd) == SOCK_DGRAM) {
rc = -1;
errno = ENOPROTOOPT;
break;
}
// else silently ignore
break;
case SO_REUSEADDR:
// just say it's not enabled
ssoGetParam(0, optval, optlen);
rc = 0;
break;
case SO_RCVLOWAT:
case SO_SNDLOWAT:
// Probably will never need to support this
// return the default linux value of 1
rc = ssoGetParam(1, optval, optlen);
break;
// CAN SAFELY IGNORE ****************************************
case SO_KEEPALIVE:
// we don't have keepalive so lie and say we do
ssoGetParam(0, optval, optlen);
rc = 0;
break;
case SO_BSDCOMPAT:
// safe to mark as unsupported, being phased out on linux anyway
errno = ENOPROTOOPT;
rc = -1;
break;
// NOT SUPPORTED/DOESN"T MAKE SENSE IN XIA ******************
case SO_BINDTODEVICE: // should we support this one when we get multihoming?
case SO_DONTROUTE:
case SO_MARK:
case SO_OOBINLINE:
case SO_PASSCRED:
case SO_PEERCRED:
case SO_PRIORITY:
case SO_RCVBUFFORCE:
case SO_SNDBUFFORCE:
case SO_TIMESTAMP:
default:
// return generic error
LOGF("Option %s not supported in XIA\n", optValue(optname));
errno = ENOPROTOOPT;
rc = -1;
}
return rc;
}
// Returns true if there is any constraint to the move
bool TR_LocalLiveRangeReduction::isAnySymInDefinedOrUsedBy(TR_TreeRefInfo *currentTreeRefInfo, TR::Node *currentNode, TR_TreeRefInfo *movingTreeRefInfo )
{
TR::Node *movingNode = movingTreeRefInfo->getTreeTop()->getNode();
// ignore anchors
//
if (movingNode->getOpCode().isAnchor())
movingNode = movingNode->getFirstChild();
TR::ILOpCode &opCode = currentNode->getOpCode();
////if ((opCode.getOpCodeValue() == TR::monent) || (opCode.getOpCodeValue() == TR::monexit))
if (nodeMaybeMonitor(currentNode))
{
if (trace())
traceMsg(comp(),"cannot move %p beyond monitor %p\n",movingNode,currentNode);
return true;
}
// Don't move gc points or things across gc points
//
if (movingNode->canGCandReturn() ||
currentNode->canGCandReturn())
{
if (trace())
traceMsg(comp(), "cannot move gc points %p past %p\n", movingNode, currentNode);
return true;
}
// Don't move checks or calls at all
//
if (containsCallOrCheck(movingTreeRefInfo,movingNode))
{
if (trace())
traceMsg(comp(),"cannot move check or call %s\n", getDebug()->getName(movingNode));
return true;
}
// Don't move object header store past a GC point
//
if ((currentNode->getOpCode().isWrtBar() || currentNode->canCauseGC()) && mayBeObjectHeaderStore(movingNode, fe()))
{
if (trace())
traceMsg(comp(),"cannot move possible object header store %s past GC point %s\n", getDebug()->getName(movingNode), getDebug()->getName(currentNode));
return true;
}
if (TR::Compiler->target.cpu.isPower() && opCode.getOpCodeValue() == TR::allocationFence)
{
// Can't move allocations past flushes
if (movingNode->getOpCodeValue() == TR::treetop &&
movingNode->getFirstChild()->getOpCode().isNew() &&
(currentNode->getAllocation() == NULL ||
currentNode->getAllocation() == movingNode->getFirstChild()))
{
if (trace())
{
traceMsg(comp(),"cannot move %p beyond flush %p - ", movingNode, currentNode);
if (currentNode->getAllocation() == NULL)
traceMsg(comp(),"(flush with null allocation)\n");
else
traceMsg(comp(),"(flush for allocation %p)\n", currentNode->getAllocation());
}
return true;
}
// Can't move certain stores past flushes
// Exclude all indirect stores, they may be for stack allocs, in which case the flush is needed at least as a scheduling barrier
// Direct stores to autos and parms are the only safe candidates
if (movingNode->getOpCode().isStoreIndirect() ||
(movingNode->getOpCode().isStoreDirect() && !movingNode->getSymbol()->isParm() && !movingNode->getSymbol()->isAuto()))
{
if (trace())
traceMsg(comp(),"cannot move %p beyond flush %p - (flush for possible stack alloc)", movingNode, currentNode);
return true;
}
}
for (int32_t i = 0; i < currentNode->getNumChildren(); i++)
{
TR::Node *child = currentNode->getChild(i);
//Any node that has side effects (like call and newarrya) cannot be evaluated in the middle of the tree.
if (movingTreeRefInfo->getFirstRefNodesList()->find(child))
{
//for calls and unresolve symbol that are not under check
if (child->exceptionsRaised() ||
(child->getOpCode().hasSymbolReference() && child->getSymbolReference()->isUnresolved()))
{
if (trace())
traceMsg(comp(),"cannot move %p beyond %p - cannot change evaluation point of %p\n ",movingNode,currentTreeRefInfo->getTreeTop()->getNode(),child);
return true;
}
else if(movingNode->getOpCode().isStore())
{
TR::SymbolReference *stSymRef = movingNode->getSymbolReference();
int32_t stSymRefNum = stSymRef->getReferenceNumber();
//TR::SymbolReference *stSymRef = movingNode->getSymbolReference();
int32_t numHelperSymbols = comp()->getSymRefTab()->getNumHelperSymbols();
if ((comp()->getSymRefTab()->isNonHelper(stSymRefNum, TR::SymbolReferenceTable::vftSymbol))||
(comp()->getSymRefTab()->isNonHelper(stSymRefNum, TR::SymbolReferenceTable::contiguousArraySizeSymbol))||
(comp()->getSymRefTab()->isNonHelper(stSymRefNum, TR::SymbolReferenceTable::discontiguousArraySizeSymbol))||
(stSymRef == comp()->getSymRefTab()->findHeaderFlagsSymbolRef())||
(stSymRef->getSymbol() == comp()->getSymRefTab()->findGenericIntShadowSymbol()))
return true;
}
else if (movingNode->getOpCode().isResolveOrNullCheck())
{
if (trace())
traceMsg(comp(),"cannot move %p beyond %p - node %p under ResolveOrNullCheck",movingNode,currentTreeRefInfo->getTreeTop()->getNode(),currentNode);
return true;
}
else if (TR::Compiler->target.is64Bit() &&
movingNode->getOpCode().isBndCheck() &&
((opCode.getOpCodeValue() == TR::i2l) || (opCode.getOpCodeValue() == TR::iu2l)) &&
!child->isNonNegative())
{
if (trace())
traceMsg(comp(),"cannot move %p beyond %p - changing the eval point of %p will casue extra cg instruction ",movingNode,currentTreeRefInfo->getTreeTop()->getNode(),currentNode);
return true;
}
}
//don't recurse over nodes each are not the first reference
if (child->getReferenceCount()==1 || currentTreeRefInfo->getFirstRefNodesList()->find(child))
{
if (isAnySymInDefinedOrUsedBy(currentTreeRefInfo, child, movingTreeRefInfo ))
return true;
}
}
return false;
}
Board_APM1::Board_APM1(const parameters_t & parameters) :
AP_Board(parameters) {
AP_Var::load_all();
// start I2C
I2c.begin();
I2c.timeOut(20);
//start SPI
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
// ports
_ports[0]=&Serial;
_ports[1]=&Serial1;
_ports[2]=&Serial3;
_ports[3]=NULL;
setPort(PORT_DEBUG,0);
setPort(PORT_HIL,1);
setPort(PORT_GCS,2);
// start debug
getDebug()->println_P(PSTR("initialized getDebug() port"));
// button and switch locations
_slideSwitchPin = 40;
_pushButtonPin = 41;
_aLedPin = 37;
_bLedPin = 36;
_cLedPin = 35;
// eeprom memory
_eepromMaxAddr = 1024;
// pin modes
pinMode(_aLedPin, OUTPUT); // extra led
pinMode(_bLedPin, OUTPUT); // imu ledclass AP_CommLink;
pinMode(_cLedPin, OUTPUT); // gps led
pinMode(_slideSwitchPin, INPUT);
pinMode(_pushButtonPin, INPUT);
DDRL |= B00000100; // set port L, pint 2 to output for the relay
// components
_isr_registry = new Arduino_Mega_ISR_Registry;
_radio = new APM_RC_APM1;
_radio->Init(_isr_registry);
_dataFlash = new DataFlash_APM1;
_scheduler = new AP_TimerProcess(_isr_registry);
_adc = new AP_ADC_ADS7844(_scheduler);
// live mode devices
if (_parameters.mode == MODE_LIVE) {
if (_parameters.options & opt_batteryMonitor) {
_batteryMonitor = new AP_BatteryMonitor(
_parameters.batteryPin,
_parameters.batteryVoltageDivRatio,
_parameters.batteryMinVolt,
_parameters.batteryMaxVolt);
}
if (_parameters.options & opt_gps) {
setPort(PORT_GPS,1);
getDebug()->println_P(PSTR("initializing gps"));
AP_GPS_Auto gpsDriver(getPort(PORT_GPS), &(_gps));
_gps = &gpsDriver;
_gps->callback = delay;
_gps->init();
}
if (_parameters.options & opt_baro) {
getDebug()->println_P(PSTR("initializing baro"));
_baro = new AP_Baro_BMP085(false);
_baro->init(_scheduler);
}
if (_parameters.options & opt_compass) {
getDebug()->println_P(PSTR("initializing compass"));
_compass = new AP_Compass_HMC5843;
_compass->set_orientation(_parameters.compassOrientation);
_compass->set_offsets(0,0,0);
_compass->set_declination(0.0);
_compass->init();
}
}
/**
* Initialize ultrasonic sensors. If sensors are not plugged in, the navigator will not
* initialize them and NULL will be assigned to those corresponding pointers.
* On detecting NU/LL assigned to any ultrasonic sensor, its corresponding block of code
* will not be executed by the navigator.
* The coordinate system is assigned by the right hand rule with the thumb pointing down.
* In set_orientation, it is defined as (front/back,left/right,down,up)
*/
// XXX this isn't really that general, should be a better way
if (_parameters.options & opt_rangeFinderFront) {
getDebug()->println_P(PSTR("initializing front range finder"));
RangeFinder * rangeFinder =
new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(1),new ModeFilter);
rangeFinder->set_orientation(1, 0, 0);
_rangeFinders.push_back(rangeFinder);
}
if (_parameters.options & opt_rangeFinderBack) {
getDebug()->println_P(PSTR("initializing back range finder"));
RangeFinder * rangeFinder =
new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(2),new ModeFilter);
rangeFinder->set_orientation(-1, 0, 0);
_rangeFinders.push_back(rangeFinder);
}
if (_parameters.options & opt_rangeFinderLeft) {
getDebug()->println_P(PSTR("initializing left range finder"));
RangeFinder * rangeFinder =
new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(3),new ModeFilter);
rangeFinder->set_orientation(0, -1, 0);
_rangeFinders.push_back(rangeFinder);
}
if (_parameters.options & opt_rangeFinderRight) {
getDebug()->println_P(PSTR("initializing right range finder"));
RangeFinder * rangeFinder =
new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(4),new ModeFilter);
rangeFinder->set_orientation(0, 1, 0);
_rangeFinders.push_back(rangeFinder);
}
if (_parameters.options & opt_rangeFinderUp) {
getDebug()->println_P(PSTR("initializing up range finder"));
RangeFinder * rangeFinder =
new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(5),new ModeFilter);
rangeFinder->set_orientation(0, 0, -1);
_rangeFinders.push_back(rangeFinder);
}
if (_parameters.options & opt_rangeFinderDown) {
getDebug()->println_P(PSTR("initializing down range finder"));
RangeFinder * rangeFinder =
new AP_RangeFinder_MaxsonarXL(new AP_AnalogSource_Arduino(6),new ModeFilter);
rangeFinder->set_orientation(0, 0, 1);
_rangeFinders.push_back(rangeFinder);
}
// navigation sensors
getDebug()->println_P(PSTR("initializing imu"));
_ins = new AP_InertialSensor_Oilpan(_adc,_scheduler);
getDebug()->println_P(PSTR("initializing ins"));
_imu = new AP_IMU_INS(_ins, k_sensorCalib,_scheduler);
getDebug()->println_P(PSTR("setup completed"));
}
