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();
}
