void BlockLocalPositionEstimator::detectDistanceSensors()
{
for (int i = 0; i < N_DIST_SUBS; i++) {
uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];
if (s == _sub_lidar || s == _sub_sonar) { continue; }
if (s->updated()) {
s->update();
if (s->get().timestamp == 0) { continue; }
if (s->get().type == \
distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
_sub_lidar == NULL) {
_sub_lidar = s;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] Lidar detected with ID %i", i);
} else if (s->get().type == \
distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
_sub_sonar == NULL) {
_sub_sonar = s;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] Sonar detected with ID %i", i);
}
}
}
}
//-------------------------------------------------
// init
//-------------------------------------------------
void
CStateInit::init()
{
#if MAVLINK_VERBOSE > 1
mavlink_and_console_log_info(m_iMavLinkFd, "PARA: INIT state...\n");
#endif
bool l_bResult;
l_bResult = m_pModule->setAutoLoiterMode();
#if MAVLINK_VERBOSE > 1
mavlink_and_console_log_info(m_iMavLinkFd, "PARA: setAutoLoiterMode(): %d\n", l_bResult);
#endif
m_pModule->setServo(6, 1.0);
l_bResult = m_pModule->setVehicleState(CStateMachine::ARMED);
#if MAVLINK_VERBOSE > 1
mavlink_and_console_log_info(m_iMavLinkFd, "PARA: setVehicleState(ARMED): %d\n", l_bResult);
#endif
l_bResult = m_pModule->setActuatorState(CStateMachine::DISARMED);
#if MAVLINK_VERBOSE > 1
mavlink_and_console_log_info(m_iMavLinkFd, "PARA: setActuatorState(DISARMED): %d\n", l_bResult);
#endif
}
void BlockLocalPositionEstimator::landCorrect()
{
// measure land
Vector<float, n_y_land> y;
if (landMeasure(y) != OK) { return; }
// measurement matrix
Matrix<float, n_y_land, n_x> C;
C.setZero();
// y = -(z - tz)
C(Y_land_vx, X_vx) = 1;
C(Y_land_vy, X_vy) = 1;
C(Y_land_agl, X_z) = -1; // measured altitude, negative down dir.
C(Y_land_agl, X_tz) = 1; // measured altitude, negative down dir.
// use parameter covariance
SquareMatrix<float, n_y_land> R;
R.setZero();
R(Y_land_vx, Y_land_vx) = _land_vxy_stddev.get() * _land_vxy_stddev.get();
R(Y_land_vy, Y_land_vy) = _land_vxy_stddev.get() * _land_vxy_stddev.get();
R(Y_land_agl, Y_land_agl) = _land_z_stddev.get() * _land_z_stddev.get();
// residual
Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * _P * C.transpose()) + R);
Vector<float, n_y_land> r = y - C * _x;
_pub_innov.get().hagl_innov = r(Y_land_agl);
_pub_innov.get().hagl_innov_var = R(Y_land_agl, Y_land_agl);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
// artifically increase beta threshhold to prevent fault during landing
float beta_thresh = 1e2f;
if (beta / BETA_TABLE[n_y_land] > beta_thresh) {
if (!(_sensorFault & SENSOR_LAND)) {
_sensorFault |= SENSOR_LAND;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land fault, beta %5.2f", double(beta));
}
// abort correction
return;
} else if (_sensorFault & SENSOR_LAND) {
_sensorFault &= ~SENSOR_LAND;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land OK");
}
// kalman filter correction always for land detector
Matrix<float, n_x, n_y_land> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
}
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) {
_visionOrigin = _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 (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOrigin = _visionOrigin(2);
}
}
}
void BlockLocalPositionEstimator::baroInit()
{
// measure
Vector<float, n_y_baro> y;
if (baroMeasure(y) != OK) {
_baroStats.reset();
return;
}
// if finished
if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
_baroAltHome = _baroStats.getMean()(0);
mavlink_and_console_log_info(&mavlink_log_pub,
"[lpe] baro init %d m std %d cm",
(int)_baroStats.getMean()(0),
(int)(100 * _baroStats.getStdDev()(0)));
_baroInitialized = true;
_baroFault = FAULT_NONE;
// only initialize alt home with baro if gps is disabled
if (!_altHomeInitialized && !_gps_on.get()) {
_altHomeInitialized = true;
_altHome = _baroAltHome;
}
}
}
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);
}
}
}
//-------------------------------------------------
// init
//-------------------------------------------------
void
CStateLanding::init()
{
#if MAVLINK_VERBOSE > 1
mavlink_and_console_log_info(m_iMavLinkFd, "PARA: LANDING state...\n");
#endif
m_pModule->setServo(6, -1.0);
}
void BlockLocalPositionEstimator::landingTargetCheckTimeout()
{
if (_timeStamp - _time_last_target > TARGET_TIMEOUT) {
if (!(_sensorTimeout & SENSOR_LAND_TARGET)) {
_sensorTimeout |= SENSOR_LAND_TARGET;
mavlink_and_console_log_info(&mavlink_log_pub, "Landing target timeout");
}
}
}
/*
* publishActuators()
* write PWM values to PX4IO
*/
void
CStateMachine::publishActuators()
{
EArmedState_t eState = getActuatorState();
if(DISARMED == eState)
{
// arm system for output
if(OK != setActuatorState(ARMED))
{
#if MAVLINK_VERBOSE > 1
mavlink_and_console_log_info((void**)m_iMavLinkFd, "PARA: arming actuators \t[failed]\n");
#endif
}
else
{
#if MAVLINK_VERBOSE > 1
mavlink_and_console_log_info((void**)m_iMavLinkFd, "PARA: arming actuators \t[done]\n");
#endif
}
}
else
{
#if MAVLINK_VERBOSE > 1
mavlink_and_console_log_info((void**)m_iMavLinkFd, "PARA: actuators already armed\n");
#endif
}
orb_publish(ORB_ID(actuator_controls_3),
m_actuatorsPub,
&m_sUORBActuatorsControl);
if(OK != setActuatorState(eState))
{
#if MAVLINK_VERBOSE > 1
mavlink_and_console_log_info((void**)m_iMavLinkFd, "PARA: reset actuators arming state \t[failed]\n");
#endif
}
else
{
#if MAVLINK_VERBOSE > 1
mavlink_and_console_log_info((void**)m_iMavLinkFd, "PARA: reset actuators arming state \t[done]\n");
#endif
}
}
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) {
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)));
_sensorTimeout &= ~SENSOR_MOCAP;
_sensorFault &= ~SENSOR_MOCAP;
// get reference for global position
globallocalconverter_getref(&_ref_lat, &_ref_lon, &_ref_alt);
_global_ref_timestamp = _timeStamp;
_is_global_cov_init = globallocalconverter_initialized();
if (!_map_ref.init_done && _is_global_cov_init && !_visionUpdated) {
// initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well)
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (mocap) : lat %6.2f lon %6.2f alt %5.1f m",
double(_ref_lat), double(_ref_lon), double(_ref_alt));
map_projection_init(&_map_ref, _ref_lat, _ref_lon);
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
}
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOriginGlobal = true;
_altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f;
}
}
}
void BlockLocalPositionEstimator::lidarCheckTimeout()
{
if (_timeStamp - _time_last_lidar > LIDAR_TIMEOUT) {
if (_lidarInitialized) {
_lidarInitialized = false;
_lidarStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar timeout ");
}
}
}
void BlockLocalPositionEstimator::flowCheckTimeout()
{
if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
if (_flowInitialized) {
_flowInitialized = false;
_flowQStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow timeout ");
}
}
}
void BlockLocalPositionEstimator::mocapCheckTimeout()
{
if (_timeStamp - _time_last_mocap > MOCAP_TIMEOUT) {
if (_mocapInitialized) {
_mocapInitialized = false;
_mocapStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap timeout ");
}
}
}
void BlockLocalPositionEstimator::visionCheckTimeout()
{
if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) {
if (_visionInitialized) {
_visionInitialized = false;
_visionStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position timeout ");
}
}
}
static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
const unsigned samples_num = 750;
accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num);
mavlink_and_console_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation),
(double)worker_data->accel_ref[0][orientation][0],
(double)worker_data->accel_ref[0][orientation][1],
(double)worker_data->accel_ref[0][orientation][2]);
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
return calibrate_return_ok;
}
void BlockLocalPositionEstimator::landCheckTimeout()
{
if (_timeStamp - _time_last_land > LAND_TIMEOUT) {
if (!(_sensorTimeout & SENSOR_LAND)) {
_sensorTimeout |= SENSOR_LAND;
_landCount = 0;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land timeout ");
}
}
}
void BlockLocalPositionEstimator::mocapCheckTimeout()
{
if (_timeStamp - _time_last_mocap > MOCAP_TIMEOUT) {
if (!(_sensorTimeout & SENSOR_MOCAP)) {
_sensorTimeout |= SENSOR_MOCAP;
_mocapStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap timeout ");
}
}
}
void BlockLocalPositionEstimator::baroCheckTimeout()
{
if (_timeStamp - _time_last_baro > BARO_TIMEOUT) {
if (_baroInitialized) {
_baroInitialized = false;
_baroStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro timeout ");
}
}
}
void BlockLocalPositionEstimator::gpsCheckTimeout()
{
if (_timeStamp - _time_last_gps > GPS_TIMEOUT) {
if (_gpsInitialized) {
_gpsInitialized = false;
_gpsStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS timeout ");
}
}
}
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) {
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)));
_sensorTimeout &= ~SENSOR_VISION;
_sensorFault &= ~SENSOR_VISION;
if (!_map_ref.init_done && _sub_vision_pos.get().xy_global) {
// initialize global origin using the visual estimator reference
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m",
double(_sub_vision_pos.get().ref_lat), double(_sub_vision_pos.get().ref_lon), double(_sub_vision_pos.get().ref_alt));
map_projection_init(&_map_ref, _sub_vision_pos.get().ref_lat, _sub_vision_pos.get().ref_lon);
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
}
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOrigin = _sub_vision_pos.get().z_global ? _sub_vision_pos.get().ref_alt : 0.0f;
}
}
}
void BlockLocalPositionEstimator::landingTargetInit()
{
if (_param_ltest_mode.get() == Target_Moving) {
// target is in moving mode, do not initialize
return;
}
Vector<float, n_y_target> y;
if (landingTargetMeasure(y) == OK) {
mavlink_and_console_log_info(&mavlink_log_pub, "Landing target init");
_sensorTimeout &= ~SENSOR_LAND_TARGET;
_sensorFault &= ~SENSOR_LAND_TARGET;
}
}
void BlockLocalPositionEstimator::baroCorrect()
{
// measure
Vector<float, n_y_baro> y;
if (baroMeasure(y) != OK) { return; }
// subtract baro home alt
y -= _baroAltHome;
// baro measurement matrix
Matrix<float, n_y_baro, n_x> C;
C.setZero();
C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir.
Matrix<float, n_y_baro, n_y_baro> R;
R.setZero();
R(0, 0) = _baro_stddev.get() * _baro_stddev.get();
// residual
Matrix<float, n_y_baro, n_y_baro> S_I =
inv<float, n_y_baro>((C * _P * C.transpose()) + R);
Vector<float, n_y_baro> r = y - (C * _x);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_baro]) {
if (_baroFault < FAULT_MINOR) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
_baroFault = FAULT_MINOR;
}
} else if (_baroFault) {
_baroFault = FAULT_NONE;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK");
}
// kalman filter correction if no fault
if (_baroFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
correctionLogic(dx);
_x += dx;
_P -= K * C * _P;
}
}
void BlockLocalPositionEstimator::landInit()
{
// measure
Vector<float, n_y_land> y;
if (landMeasure(y) != OK) {
_landCount = 0;
}
// if finished
if (_landCount > REQ_LAND_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land init");
_sensorTimeout &= ~SENSOR_LAND;
_sensorFault &= ~SENSOR_LAND;
}
}
void BlockLocalPositionEstimator::lidarInit()
{
// measure
Vector<float, n_y_lidar> y;
if (lidarMeasure(y) != OK) {
_lidarStats.reset();
}
// if finished
if (_lidarStats.getCount() > REQ_LIDAR_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar init: "
"mean %d cm stddev %d cm",
int(100 * _lidarStats.getMean()(0)),
int(100 * _lidarStats.getStdDev()(0)));
_lidarInitialized = true;
_lidarFault = FAULT_NONE;
}
}
void BlockLocalPositionEstimator::flowInit()
{
// measure
Vector<float, n_y_flow> y;
if (flowMeasure(y) != OK) {
_flowQStats.reset();
return;
}
// if finished
if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow init: "
"quality %d std %d",
int(_flowQStats.getMean()(0)),
int(_flowQStats.getStdDev()(0)));
_flowInitialized = true;
_flowFault = FAULT_NONE;
}
}
int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods)
{
float t_delay = 0;
uint8_t i_hist = 0;
for (i_hist = 1; i_hist < HIST_LEN; i_hist++) {
t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0));
if (t_delay > delay) {
break;
}
}
*periods = i_hist;
if (t_delay > DELAY_MAX) {
mavlink_and_console_log_info(&mavlink_log_pub, "%sdelayed data old: %8.4f", msg_label, double(t_delay));
return -1;
}
return OK;
}
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;
}
void BlockLocalPositionEstimator::gpsInit()
{
// measure
Vector<double, n_y_gps> y;
if (gpsMeasure(y) != OK) {
_gpsStats.reset();
return;
}
// if finished
if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
double gpsLatHome = _gpsStats.getMean()(0);
double gpsLonHome = _gpsStats.getMean()(1);
if (!_receivedGps) {
_receivedGps = true;
map_projection_init(&_map_ref, gpsLatHome, gpsLonHome);
}
_gpsAltHome = _gpsStats.getMean()(2);
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps init "
"lat %6.2f lon %6.2f alt %5.1f m",
gpsLatHome,
gpsLonHome,
double(_gpsAltHome));
_gpsInitialized = true;
_gpsFault = FAULT_NONE;
_gpsStats.reset();
if (!_altHomeInitialized) {
_altHomeInitialized = true;
_altHome = _gpsAltHome;
}
}
}
void BlockLocalPositionEstimator::checkTimeouts()
{
if (_timeStamp - _time_last_xy > EST_SRC_TIMEOUT) {
if (!_xyTimeout) {
_xyTimeout = true;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] xy timeout ");
}
} else if (_xyTimeout) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] xy resume ");
_xyTimeout = false;
}
if (_timeStamp - _time_last_z > EST_SRC_TIMEOUT) {
if (!_zTimeout) {
_zTimeout = true;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] z timeout ");
}
} else if (_zTimeout) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] z resume ");
_zTimeout = false;
}
if (_timeStamp - _time_last_tz > EST_SRC_TIMEOUT) {
if (!_tzTimeout) {
_tzTimeout = true;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] tz timeout ");
}
} else if (_tzTimeout) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] tz resume ");
_tzTimeout = false;
}
lidarCheckTimeout();
sonarCheckTimeout();
baroCheckTimeout();
gpsCheckTimeout();
flowCheckTimeout();
visionCheckTimeout();
mocapCheckTimeout();
}
void BlockLocalPositionEstimator::gpsInit()
{
// check for good gps signal
uint8_t nSat = _sub_gps.get().satellites_used;
float eph = _sub_gps.get().eph;
float epv = _sub_gps.get().epv;
uint8_t fix_type = _sub_gps.get().fix_type;
if (
nSat < 6 ||
eph > _param_lpe_eph_max.get() ||
epv > _param_lpe_epv_max.get() ||
fix_type < 3
) {
_gpsStats.reset();
return;
}
// measure
Vector<double, n_y_gps> y;
if (gpsMeasure(y) != OK) {
_gpsStats.reset();
return;
}
// if finished
if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
// get mean gps values
double gpsLat = _gpsStats.getMean()(0);
double gpsLon = _gpsStats.getMean()(1);
float gpsAlt = _gpsStats.getMean()(2);
_sensorTimeout &= ~SENSOR_GPS;
_sensorFault &= ~SENSOR_GPS;
_gpsStats.reset();
if (!_receivedGps) {
// this is the first time we have received gps
_receivedGps = true;
// note we subtract X_z which is in down directon so it is
// an addition
_gpsAltOrigin = gpsAlt + _x(X_z);
// find lat, lon of current origin by subtracting x and y
// if not using vision position since vision will
// have it's own origin, not necessarily where vehicle starts
if (!_map_ref.init_done && !(_param_lpe_fusion.get() & FUSE_VIS_POS)) {
double gpsLatOrigin = 0;
double gpsLonOrigin = 0;
// reproject at current coordinates
map_projection_init(&_map_ref, gpsLat, gpsLon);
// find origin
map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
// reinit origin
map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
// always override alt origin on first GPS to fix
// possible baro offset in global altitude at init
_altOrigin = _gpsAltOrigin;
_altOriginInitialized = true;
_altOriginGlobal = true;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (gps) : lat %6.2f lon %6.2f alt %5.1f m",
gpsLatOrigin, gpsLonOrigin, double(_gpsAltOrigin));
}
PX4_INFO("[lpe] gps init "
"lat %6.2f lon %6.2f alt %5.1f m",
gpsLat,
gpsLon,
double(gpsAlt));
}
}
}