void BlockLocalPositionEstimator::visionInit()
{
// measure
Vector<float, n_y_vision> y;
if (visionMeasure(y) != OK) {
_visionStats.reset();
return;
}
// increament sums for mean
if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
_visionHome = _visionStats.getMean();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: "
"%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
double(_visionStats.getMean()(0)),
double(_visionStats.getMean()(1)),
double(_visionStats.getMean()(2)),
double(_visionStats.getStdDev()(0)),
double(_visionStats.getStdDev()(1)),
double(_visionStats.getStdDev()(2)));
_visionInitialized = true;
_visionFault = FAULT_NONE;
if (!_altHomeInitialized) {
_altHomeInitialized = true;
_altHome = _visionHome(2);
}
}
}
void BlockLocalPositionEstimator::updateHome()
{
double lat = _sub_home.get().lat;
double lon = _sub_home.get().lon;
float alt = _sub_home.get().alt;
// updating home causes absolute measurements
// like gps and baro to be off, need to allow it
// to reset by resetting covariance
initP();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] home "
"lat %6.2f lon %6.2f alt %5.1f m",
lat, lon, double(alt));
map_projection_init(&_map_ref, lat, lon);
float delta_alt = alt - _altHome;
_altHomeInitialized = true;
_altHome = alt;
_gpsAltHome += delta_alt;
_baroAltHome += delta_alt;
_visionHome(2) += delta_alt;
_mocapHome(2) += delta_alt;
}
