void BlockLocalPositionEstimator::mocapInit()
{
// measure
Vector<float, n_y_mocap> y;
if (mocapMeasure(y) != OK) {
_mocapStats.reset();
return;
}
// if finished
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
_mocapHome = _mocapStats.getMean();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
"%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
double(_mocapStats.getMean()(0)),
double(_mocapStats.getMean()(1)),
double(_mocapStats.getMean()(2)),
double(_mocapStats.getStdDev()(0)),
double(_mocapStats.getStdDev()(1)),
double(_mocapStats.getStdDev()(2)));
_mocapInitialized = true;
_mocapFault = FAULT_NONE;
if (!_altHomeInitialized) {
_altHomeInitialized = true;
_altHome = _mocapHome(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;
}