void runCalibrationMenu(const StaticEditMenu::StaticEditData * menuData, const AnalogInputs::Name * name1, const AnalogInputs::Name * name2, uint8_t calibrationPoint = false) { StaticEditMenu menu(menuData); uint16_t selector = COND_ALWAYS ^ COND_POINT; if(calibrationPoint || settings.menuType == Settings::MenuAdvanced) { selector |= COND_POINT; } menu.setSelector(selector); int8_t item; do { item = menu.runSimple(true); if(item < 0) break; uint8_t c = menu.getEnableCondition(item); if(!(c & 1)) { if(c & COND_E_ANALOG) { AnalogInputs::Name Vinput = pgm::read(&name1[item]); if(AnalogInputs::isConnected(Vinput)) { AnalogInputs::doFullMeasurement(); AnalogInputs::on_ = false; if(menu.runEdit()) { bool doCopyVout = c & COND_COPY_VOUT; saveCalibration(doCopyVout, Vinput, pgm::read(&name2[item])); } AnalogInputs::on_ = true; } } else { menu.runEdit(); } } } while(true); }
void Drive::calibrate(unsigned int millis) { Vector3<float> accel_total; Vector3<float> gyro_total; unsigned int elapsed = 0, num_iterations = 0; accel_total.x = 0.0f; accel_total.y = 0.0f; accel_total.z = 0.0f; gyro_total.x = 0.0f; gyro_total.y = 0.0f; gyro_total.z = 0.0f; while (elapsed < millis) { accel_total += mAccelerometer->read(); gyro_total += mGyroscope->read(); usleep(10000); elapsed += 10; ++num_iterations; } mAccelOffset.x = accel_total.x / num_iterations; mAccelOffset.y = accel_total.y / num_iterations; mAccelOffset.z = accel_total.z / num_iterations + 1.0f; mGyroOffset.x = gyro_total.x / num_iterations; mGyroOffset.y = gyro_total.y / num_iterations; mGyroOffset.z = gyro_total.z / num_iterations; saveCalibration("calibration.ini"); }
void runCalibrationMenu(const StaticEditMenu::StaticEditData * menuData, const AnalogInputs::Name * name1, const AnalogInputs::Name * name2) { StaticEditMenu menu(menuData); menu.setSelector(COND_ALWAYS); int8_t item; do { item = menu.runSimple(true); if(item < 0) break; uint8_t c = menu.getEnableCondition(item); if(!(c & 1)) { if(c & COND_E_ANALOG) { AnalogInputs::Name Vinput = pgm::read(&name1[item]); if(AnalogInputs::isConnected(Vinput)) { AnalogInputs::doFullMeasurement(); AnalogInputs::on_ = false; if(menu.runEdit()) { bool doCopyVout = c & COND_COPY_VOUT; saveCalibration(doCopyVout, Vinput, pgm::read(&name2[item])); } AnalogInputs::on_ = true; } } else { menu.runEdit(); } } } while(true); }
void CalibrationUtils::nextCalibrationStep() { if(bCalibrating) { calibrationStep++; if(calibrationStep >= GRID_POINTS) { bCalibrating = false; calibrationStep = 0; saveCalibration(); calculateBox(); // computeCameraToScreenMap(); saveCalibration(); } } }
void saveCalibration(bool doCopyVbalVout, AnalogInputs::Name name1, AnalogInputs::Name name2) { Buzzer::soundSelect(); AnalogInputs::ValueType newValue = AnalogInputs::getRealValue(name1); SerialLog::flush(); saveCalibration(name1, name2, AnalogInputs::getAvrADCValue(name2), newValue); if(doCopyVbalVout) { AnalogInputs::on_ = true; AnalogInputs::doFullMeasurement(); SerialLog::flush(); copyVbalVout(); } eeprom::restoreCalibrationCRC(); }
void calibrationB::nextCalibrationStep() { if(bCalibrating) { calibrationStep++; if(calibrationStep >= GRID_POINTS) { printf("Calibration complete\n"); bCalibrating = false; calibrationStep = 0; saveCalibration(); calculateBox(); computeCameraToScreenMap(); } } }
//-------------------------------------------------------------- void testApp::setup() { //the pirmary listener can't yet be set by default, so even if you only have one window, you need to call this line ofxFensterManager::get()->getPrimaryWindow()->addListener(this); ofSetFrameRate(30); ofBackground(0, 0, 0); ofEnableAlphaBlending(); ofSetVerticalSync(true); ofSetWindowTitle("Control Panel"); vout.setup(); ofxFenster* win=ofxFensterManager::get()->createFenster(500,0,1024,768,OF_WINDOW); win->addListener(&vout); win->setWindowTitle("Output"); if(calibFile.open("calibration")){ printf("Calibration file found\n"); calibFile.close(); loadCalibration(); }else{ printf("Calibration file not found: using defaults\n"); skHratio = ofGetWidth()/640.0; skVratio = ofGetHeight()/480.0; skelHoffset = 0; skelVoffset = 0; spdWings = 0; hgtWings = 0; sclWings = 1; saveCalibration(); } bKinect = true; if(bKinect){ context.setupUsingXMLFile(ofToDataPath("openni/config/ofxopenni_config.xml",true)); context.getXnContext().SetGlobalMirror(false); depth.setup(&context); user.setup(&context, &depth); } wing.setup(); wLa.set(ofGetWidth()/2,ofGetHeight()/2); wRa.set(ofGetWidth()/2,ofGetHeight()/2); head.setup(); bDebug = true; bCalib = false; bMouseMode = false; bWingsGrown= false; trkEase = 0.6; scene = 0; growAmtR = 0; growAmtL = 0; oldHandL = 0; oldHandR = 0; cellEmitter.clear(); font.loadFont("verdana.ttf",60); curS = 0; }
//-------------------------------------------------------------- void testApp::keyPressed(int key, ofxFenster* win) { switch (key) { case ' ': wing.toggleAnim(); break; /*case 'd': bDebug=!bDebug; break;*/ case 'c': bCalib=!bCalib; break; case 'q': spdWings+=0.1; break; case 'a': spdWings-=0.1; break; case 'w': hgtWings+=0.1; break; case 's': hgtWings-=0.1; break; case 'e': sclWings+=0.1; break; case 'd': sclWings-=0.1; break; case 'g': bWingsGrown=!bWingsGrown; break; case OF_KEY_RETURN: saveCalibration(); //bCalib=!bCalib; break; case 'm': bMouseMode=!bMouseMode; break; case 'f': vout.toggleFullScreen(); break; case '0': scene = 0; break; case '1': head.setup(); scene = 1; break; case '2': scene = 2; break; case '3': scene = 3; wing.curAnim = 1; if(!bWingsGrown){ wing.bPlayAnim=true; bWingsGrown = true; } break; case '4': scene = 3; wing.curAnim = 0; break; case '8': scene = 8; break; case '9': scene = 9; break; } if(bCalib){ switch(key){ case 'h'://OF_KEY_DOWN: skelVoffset+=1.0; //vout.p.y+=1.0; //warper.moveCorner(0,0,1); break; case 'y'://OF_KEY_UP: skelVoffset-=1.0; //vout.p.y-=1.0; //warper.moveCorner(0,0,-1); break; case 'u'://OF_KEY_LEFT: skelHoffset-=1.0; //vout.p.x-=1.0; //warper.moveCorner(0,-1,0); break; case 'j'://OF_KEY_RIGHT: //printf("cippa\n"); skelHoffset+=1.0; //vout.p.x+=1.0; //warper.moveCorner(0,1,0); break; case '+': skHratio+=0.1; skVratio+=0.1; break; case '-': skHratio-=0.1; skVratio-=0.1; break; case 'z': skelHoffset+=10; break; } } }
//-------------------------------------------------------------- void calibrationManager::update(){ calibrationRectangle.x = 0; calibrationRectangle.y = 0; calibrationRectangle.width = ofGetWidth(); calibrationRectangle.height = ofGetHeight(); float widthPad = ofGetWidth() * 0.025f; float heightPad = ofGetHeight() * 0.025f; calibrationRectangle.x += widthPad; calibrationRectangle.y += heightPad; calibrationRectangle.width -= widthPad*2; calibrationRectangle.height -= heightPad*2; panel.update(); nDivisionsWidth = panel.getValueI("N_DIV_W"); nDivisionsHeight = panel.getValueI("N_DIV_H");; preTimePerDot = panel.getValueF("PRE_RECORD_TIME");; recordTimePerDot = panel.getValueF("RECORD_TIME");; smoothing = panel.getValueF("AMOUNT_SMOOTHING"); totalTimePerDot = preTimePerDot + recordTimePerDot; if (panel.getValueB("START_AUTO")){ bPreAutomatic = true; panel.setValueB("START_AUTO", false); } if (panel.getValueB("RESET_CALIB")){ clear(); panel.setValueB("RESET_CALIB", false); } if (panel.getValueB("SAVE_CALIB")){ if (bBeenFit){ saveCalibration(); } panel.setValueB("SAVE_CALIB", false); } if (panel.getValueB("LOAD_CALIB")){ loadCalibration(); panel.setValueB("LOAD_CALIB", false); } if ((bAutomatic == true && bAmInAutodrive == true) || bPreAutomatic){ menuEnergy = 0.94f * menuEnergy + 0.06f * 0.0f; } else { menuEnergy = 0.94f * menuEnergy + 0.06f * 1.0f; } // do the auto: if (bAutomatic == true && bAmInAutodrive == true){ int nPts = nDivisionsWidth * nDivisionsHeight; float totalTime = totalTimePerDot * nPts; if (ofGetElapsedTimef() - startTime > totalTime){ bAmInAutodrive = false; bInAutoRecording = false; calculate(); } else { float diffTime = ofGetElapsedTimef() - startTime ; int pt = (int)(diffTime / totalTimePerDot); float diffDotTime = diffTime - pt * totalTimePerDot; //cout << diffTime << " " << pt << " " << diffDotTime << endl; if (diffDotTime < preTimePerDot){ autoPct = (diffDotTime / preTimePerDot); bInAutoRecording = false; } else{ autoPct = (diffDotTime - preTimePerDot) / recordTimePerDot; bInAutoRecording = true; } pos = pt; } } inputEnergy *= 0.98f; int xx = (pos % nDivisionsWidth); int yy = (pos / nDivisionsWidth); bool bEven = false; if (yy % 2 == 0) bEven = true; //xp = ((float)ofGetWidth() / (float)(nDivisionsWidth-1)) * xx; //yp = ofGetHeight() - ((float)ofGetHeight() / (float)(nDivisionsHeight-1)) * yy; xp = bEven ? calibrationRectangle.x + ((float)calibrationRectangle.width / (float)(nDivisionsWidth-1)) * xx : calibrationRectangle.x + (calibrationRectangle.width - ((float)calibrationRectangle.width / (float)(nDivisionsWidth-1)) * xx); yp = calibrationRectangle.y + calibrationRectangle.height - ((float)calibrationRectangle.height / (float)(nDivisionsHeight-1)) * yy; }
void Node::inputCallback(const sensor_msgs::ImuConstPtr& imuMsg, const sensor_msgs::MagneticFieldConstPtr& magMsg) { if (enableMag_) { assert(magMsg); } vec3 wm; // measured angular rate vec3 am; // measured acceleration vec3 mm(0,0,0); // measured magnetic field wm = vec3(imuMsg->angular_velocity.x, imuMsg->angular_velocity.y, imuMsg->angular_velocity.z); am = vec3(imuMsg->linear_acceleration.x, imuMsg->linear_acceleration.y, imuMsg->linear_acceleration.z); if (magMsg) { mm = vec3(magMsg->magnetic_field.x, magMsg->magnetic_field.y, magMsg->magnetic_field.z); } // copy covariances from message to matrix kr::AttitudeESKF::mat3 wCov, aCov, mCov = kr::AttitudeESKF::mat3::Zero(); for (int i=0; i < 3; i++) { for (int j=0; j < 3; j++) { wCov(i,j) = imuMsg->angular_velocity_covariance[i*3 + j]; aCov(i,j) = imuMsg->linear_acceleration_covariance[i*3 + j]; if (magMsg) { mCov(i,j) = magMsg->magnetic_field_covariance[i*3 + j]; } } } wCov *= processScaleFactor_; // apply fudge factor if (enableMag_ && calibState_==CalibrationComplete) { // correct magnetic field mm.noalias() -= magBias_; for (int i=0; i < 3; i++) { mm[i] /= magScale_[i]; } eskf_.setMagneticReference(magReference_); eskf_.setUsesMagnetometer(true); } else { eskf_.setUsesMagnetometer(false); } if (prevStamp_.sec != 0) { if (!init_) { const vec3 aCovDiag(aCov(0,0),aCov(1,1),aCov(2,2)); const vec3 mCovDiag(mCov(0,0),mCov(1,1),mCov(2,2)); eskf_.initialize(am,aCovDiag,mm,mCovDiag); init_ = true; ROS_INFO("Initialized ESKF"); } // run kalman filter const double delta = (imuMsg->header.stamp - prevStamp_).toSec(); eskf_.predict(wm, delta, wCov); eskf_.update(am, aCov, mm, mCov); const kr::AttitudeESKF::quat wQb = eskf_.getQuat(); const vec3 w = eskf_.getAngularVelocity(); // ang vel. minus bias if (calibState_ == Calibrating) { // update the calibrator if (!calib_.isCalibrated()) { calib_.appendSample(wQb, mm); if (calib_.isReady()) { ROS_INFO("Collected sufficient samples. Calibrating..."); // calibrate bias, scale and reference vector try { calib_.calibrate(kr::AttitudeMagCalib::FullCalibration); ROS_INFO_STREAM("Bias: " << calib_.getBias().transpose()); ROS_INFO_STREAM("Scale: " << calib_.getScale().transpose()); magBias_ = calib_.getBias(); magScale_ = calib_.getScale(); // save to rosparam nh_.setParam("enable_magnetometer", true); saveCalibration(); // done, we can use this new calibration immediately calibState_ = CalibrationComplete; enableMag_ = true; } catch (std::exception& e) { ROS_ERROR("Calibration failed: %s", e.what()); } } } } sensor_msgs::Imu imu = *imuMsg; imu.header.seq = 0; imu.angular_velocity.x = w[0]; imu.angular_velocity.y = w[1]; imu.angular_velocity.z = w[2]; imu.orientation.w = wQb.w(); imu.orientation.x = wQb.x(); imu.orientation.y = wQb.y(); imu.orientation.z = wQb.z(); // append our covariance estimate to the new IMU message for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { imu.orientation_covariance[i*3 + j] = eskf_.getCovariance()(i,j); } } geometry_msgs::Vector3Stamped bias; bias.header = imu.header; bias.vector.x = eskf_.getGyroBias()[0]; bias.vector.y = eskf_.getGyroBias()[1]; bias.vector.z = eskf_.getGyroBias()[2]; // pose with no position geometry_msgs::PoseStamped pose; pose.header = imu.header; pose.pose.orientation = imu.orientation; if (enableMag_) { sensor_msgs::MagneticField field = *magMsg; field.header.seq = 0; field.magnetic_field.x = mm[0]; field.magnetic_field.y = mm[1]; field.magnetic_field.z = mm[2]; pubField_.publish(field); } pubImu_.publish(imu); pubBias_.publish(bias); pubPose_.publish(pose); } prevStamp_ = imuMsg->header.stamp; updater_.update(); }