float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
{

	/* Do not calculate control signal with bad inputs */
	if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
	      PX4_ISFINITE(ctl_data.roll) &&
	      PX4_ISFINITE(ctl_data.pitch) &&
	      PX4_ISFINITE(ctl_data.airspeed))) {
		perf_count(_nonfinite_input_perf);
		warnx("not controlling pitch");
		return _rate_setpoint;
	}

	/* Calculate the error */
	float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;

	/*  Apply P controller: rate setpoint from current error and time constant */
	_rate_setpoint =  pitch_error / _tc;

	/* limit the rate */
	if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
		if (_rate_setpoint > 0.0f) {
			_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;

		} else {
			_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
		}

	}

	return _rate_setpoint;
}

float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
    /* Do not calculate control signal with bad inputs */
    if (!(PX4_ISFINITE(ctl_data.yaw_rate) &&
          PX4_ISFINITE(ctl_data.groundspeed) &&
          PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
        return math::constrain(_last_output, -1.0f, 1.0f);
    }

    /* get the usual dt estimate */
    uint64_t dt_micros = ecl_elapsed_time(&_last_run);
    _last_run = ecl_absolute_time();
    float dt = (float)dt_micros * 1e-6f;

    /* lock integral for long intervals */
    bool lock_integrator = ctl_data.lock_integrator;

    if (dt_micros > 500000) {
        lock_integrator = true;
    }

    /* input conditioning */
    float min_speed = 1.0f;

    /* Calculate body angular rate error */
    _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error

    if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {

        float id = _rate_error * dt * ctl_data.groundspeed_scaler;

        /*
         * anti-windup: do not allow integrator to increase if actuator is at limit
         */
        if (_last_output < -1.0f) {
            /* only allow motion to center: increase value */
            id = math::max(id, 0.0f);

        } else if (_last_output > 1.0f) {
            /* only allow motion to center: decrease value */
            id = math::min(id, 0.0f);
        }

        _integrator += id * _k_i;
    }

    /* integrator limit */
    //xxx: until start detection is available: integral part in control signal is limited here
    float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);

    /* Apply PI rate controller and store non-limited output */
    _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
            _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler +
            integrator_constrained;
    /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f",
            (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/


    return math::constrain(_last_output, -1.0f, 1.0f);
}

void
Battery::estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed)
{
	const float bat_r = _param_r_internal.get();

	// remaining charge estimate based on voltage and internal resistance (drop under load)
	float bat_v_empty_dynamic = _param_v_empty.get();

	if (bat_r >= 0.0f) {
		bat_v_empty_dynamic -= current_a * bat_r;

	} else {
		// assume 10% voltage drop of the full drop range with motors idle
		const float thr = (armed) ? ((fabsf(throttle_normalized) + 0.1f) / 1.1f) : 0.0f;

		bat_v_empty_dynamic -= _param_v_load_drop.get() * thr;
	}

	// the range from full to empty is the same for batteries under load and without load,
	// since the voltage drop applies to both the full and empty state
	const float voltage_range = (_param_v_full.get() - _param_v_empty.get());

	// remaining battery capacity based on voltage
	const float rvoltage = (voltage_v - (_param_n_cells.get() * bat_v_empty_dynamic))
			       / (_param_n_cells.get() * voltage_range);
	const float rvoltage_filt = _remaining_voltage * 0.99f + rvoltage * 0.01f;

	if (PX4_ISFINITE(rvoltage_filt)) {
		_remaining_voltage = rvoltage_filt;
	}

	// remaining battery capacity based on used current integrated time
	const float rcap = 1.0f - _discharged_mah / _param_capacity.get();
	const float rcap_filt = _remaining_capacity * 0.99f + rcap * 0.01f;

	if (PX4_ISFINITE(rcap_filt)) {
		_remaining_capacity = rcap_filt;
	}

	// limit to sane values
	_remaining_voltage = (_remaining_voltage < 0.0f) ? 0.0f : _remaining_voltage;
	_remaining_voltage = (_remaining_voltage > 1.0f) ? 1.0f : _remaining_voltage;

	_remaining_capacity = (_remaining_capacity < 0.0f) ? 0.0f : _remaining_capacity;
	_remaining_capacity = (_remaining_capacity > 1.0f) ? 1.0f : _remaining_capacity;

	// choose which quantity we're using for final reporting
	if (_param_capacity.get() > 0.0f) {
		// if battery capacity is known, use discharged current for estimate,
		// but don't show more than voltage estimate
		_remaining = fminf(_remaining_voltage, _remaining_capacity);

	} else {
		// else use voltage
		_remaining = _remaining_voltage;
	}
}

bool
MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance)
{
	if (max_distance <= 0.0f) {
		/* param not set, check is ok */
		return true;
	}

	double last_lat = (double)NAN;
	double last_lon = (double)NAN;

	/* Go through all waypoints */
	for (size_t i = 0; i < mission.count; i++) {

		struct mission_item_s mission_item {};

		if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
			/* error reading, mission is invalid */
			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.");
			return false;
		}

		/* check only items with valid lat/lon */
		if (!MissionBlock::item_contains_position(mission_item)) {
			continue;
		}

		/* Compare it to last waypoint if already available. */
		if (PX4_ISFINITE(last_lat) && PX4_ISFINITE(last_lon)) {

			/* check distance from current position to item */
			const float dist_between_waypoints = get_distance_to_next_waypoint(
					mission_item.lat, mission_item.lon,
					last_lat, last_lon);

			if (dist_between_waypoints > max_distance) {
				/* item is too far from home */
				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
						     "Distance between waypoints too far: %d meters, %d max.",
						     (int)dist_between_waypoints, (int)max_distance);

				_navigator->get_mission_result()->warning = true;
				return false;
			}
		}

		last_lat = mission_item.lat;
		last_lon = mission_item.lon;
	}

	/* We ran through all waypoints and have not found any distances between waypoints that are too far. */
	return true;
}

bool FixedwingLandDetector::_get_landed_state()
{
	// only trigger flight conditions if we are armed
	if (!_arming.armed) {
		return true;
	}

	bool landDetected = false;

	if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) {
		float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_controlState.x_vel *
				_controlState.x_vel + _controlState.y_vel * _controlState.y_vel);

		if (PX4_ISFINITE(val)) {
			_velocity_xy_filtered = val;
		}

		val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_controlState.z_vel);

		if (PX4_ISFINITE(val)) {
			_velocity_z_filtered = val;
		}

		_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;

		// a leaking lowpass prevents biases from building up, but
		// gives a mostly correct response for short impulses
		_accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f;

		// crude land detector for fixedwing
		if (_velocity_xy_filtered < _params.maxVelocity
		    && _velocity_z_filtered < _params.maxClimbRate
		    && _airspeed_filtered < _params.maxAirSpeed
		    && _accel_horz_lp < _params.maxIntVelocity) {

			landDetected = true;

		} else {
			landDetected = false;
		}

	} else {
		// Control state topic has timed out and we need to assume we're landed.
		landDetected = true;
	}

	return landDetected;
}

/*
 * Attitude rates controller.
 * Input: '_rates_sp' vector, '_thrust_sp'
 * Output: '_att_control' vector
 */
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */
	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
		_rates_int.zero();
	}

	/* current body angular rates */
	math::Vector<3> rates;
	rates(0) = _ctrl_state.roll_rate;
	rates(1) = _ctrl_state.pitch_rate;
	rates(2) = _ctrl_state.yaw_rate;

	/* angular rates error */
	math::Vector<3> rates_err = _rates_sp - rates;
	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
		       _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
	_rates_sp_prev = _rates_sp;
	_rates_prev = rates;

	/* update integral only if not saturated on low limit and if motor commands are not saturated */
	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
		for (int i = 0; i < 3; i++) {
			if (fabsf(_att_control(i)) < _thrust_sp) {
				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

				if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
					_rates_int(i) = rate_i;
				}
			}
		}
	}
}

bool MulticopterLandDetector::_is_climb_rate_enabled()
{
	bool has_updated = (_vehicleLocalPositionSetpoint.timestamp != 0)
			   && (hrt_elapsed_time(&_vehicleLocalPositionSetpoint.timestamp) < 500_ms);

	return (_control_mode.flag_control_climb_rate_enabled && has_updated && PX4_ISFINITE(_vehicleLocalPositionSetpoint.vz));
}

void Tailsitter::scale_mc_output()
{
	// scale around tuning airspeed
	float airspeed;
	calc_tot_airspeed();	// estimate air velocity seen by elevons

	// if airspeed is not updating, we assume the normal average speed
	if (bool nonfinite = !PX4_ISFINITE(_airspeed->true_airspeed_m_s) ||
			     hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
		airspeed = _params->mc_airspeed_trim;

		if (nonfinite) {
			perf_count(_nonfinite_input_perf);
		}

	} else {
		airspeed = _airspeed_tot;
		airspeed = math::constrain(airspeed, _params->mc_airspeed_min, _params->mc_airspeed_max);
	}

	_vtol_vehicle_status->airspeed_tot = airspeed;	// save value for logging
	/*
	 * For scaling our actuators using anything less than the min (close to stall)
	 * speed doesn't make any sense - its the strongest reasonable deflection we
	 * want to do in flight and its the baseline a human pilot would choose.
	 *
	 * Forcing the scaling to this value allows reasonable handheld tests.
	 */
	float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min :
				 airspeed);
	_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1] * airspeed_scaling * airspeed_scaling,
				       -1.0f, 1.0f);
}

void
Battery::estimateRemaining(float voltage_v, float throttle_normalized)
{
	// XXX this time constant needs to become tunable but really, the right fix are smart batteries.
	const float filtered_next = _throttle_filtered * 0.97f + throttle_normalized * 0.03f;

	if (PX4_ISFINITE(filtered_next)) {
		_throttle_filtered = filtered_next;
	}

	/* remaining charge estimate based on voltage and internal resistance (drop under load) */
	const float bat_v_empty_dynamic = _param_v_empty.get() - (_param_v_load_drop.get() * _throttle_filtered);
	/* the range from full to empty is the same for batteries under load and without load,
	 * since the voltage drop applies to both the full and empty state
	 */
	const float voltage_range = (_param_v_full.get() - _param_v_empty.get());
	const float remaining_voltage = (voltage_v - (_param_n_cells.get() * bat_v_empty_dynamic))
					/ (_param_n_cells.get() * voltage_range);

	if (_param_capacity.get() > 0.0f) {
		/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
		_remaining = fminf(remaining_voltage, 1.0f - _discharged_mah / _param_capacity.get());

	} else {
		/* else use voltage */
		_remaining = remaining_voltage;
	}

	/* limit to sane values */
	_remaining = (_remaining < 0.0f) ? 0.0f : _remaining;
	_remaining = (_remaining > 1.0f) ? 1.0f : _remaining;
}

void
Mission::altitude_sp_foh_update()
{
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	/* Don't change setpoint if last and current waypoint are not valid */
	if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
	    !PX4_ISFINITE(_mission_item_previous_alt)) {
		return;
	}

	/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
	if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < FLT_EPSILON) {
		return;
	}

	/* Don't do FOH for non-missions, landing and takeoff waypoints, the ground may be near
	 * and the FW controller has a custom landing logic
	 *
	 * NAV_CMD_LOITER_TO_ALT doesn't change altitude until reaching desired lat/lon
	 * */
	if (_mission_item.nav_cmd == NAV_CMD_LAND
	    || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
	    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF
	    || _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT
	    || !_navigator->is_planned_mission()) {
		return;
	}

	/* Calculate distance to current waypoint */
	float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
			  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);

	/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
	 * _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
	_min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy),
						_distance_current_previous);

	/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
	 * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
	if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
		pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);

	} else {
		/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
		* of the mission item as it is used to check if the mission item is reached
		* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
		* radius around the current waypoint
		**/
		float delta_alt = (get_absolute_altitude_for_item(_mission_item) - _mission_item_previous_alt);
		float grad = -delta_alt / (_distance_current_previous - _navigator->get_acceptance_radius(
						   _mission_item.acceptance_radius));
		float a = _mission_item_previous_alt - grad * _distance_current_previous;
		pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
	}

	// we set altitude directly so we can run this in parallel to the heading update
	_navigator->set_position_setpoint_triplet_updated();
}

void
Battery::filterVoltage(float voltage_v)
{
	// TODO: inspect that filter performance
	const float filtered_next = _voltage_filtered_v * 0.999f + voltage_v * 0.001f;

	if (PX4_ISFINITE(filtered_next)) {
		_voltage_filtered_v = filtered_next;
	}
}

bool FixedwingLandDetector::update()
{
	// First poll for new data from our subscriptions
	updateSubscriptions();

	const uint64_t now = hrt_absolute_time();
	bool landDetected = false;

	if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
		float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx *
					_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
		if (PX4_ISFINITE(val)) {
			_velocity_xy_filtered = val;
		}
		val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz);

		if (PX4_ISFINITE(val)) {
			_velocity_z_filtered = val;
		}
	}

	if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
		_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
	}

	// crude land detector for fixedwing
	if (_velocity_xy_filtered < _params.maxVelocity
	    && _velocity_z_filtered < _params.maxClimbRate
	    && _airspeed_filtered < _params.maxAirSpeed) {

		// these conditions need to be stable for a period of time before we trust them
		if (now > _landDetectTrigger) {
			landDetected = true;
		}

	} else {
		// reset land detect trigger
		_landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME;
	}

	return landDetected;
}

int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
{
	uint8_t x_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_X_VARIANCE;
	uint8_t y_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Y_VARIANCE;
	uint8_t z_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Z_VARIANCE;

	if (PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[x_variance])) {
		// check if the mocap data is valid based on the covariances
		_mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[x_variance],
					 _sub_mocap_odom.get().pose_covariance[y_variance]));
		_mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[z_variance]);
		_mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV;
		_mocap_z_valid = _mocap_epv <= EP_MAX_STD_DEV;

	} else {
		// if we don't have covariances, assume every reading
		_mocap_xy_valid = true;
		_mocap_z_valid = true;
	}

	if (!_mocap_xy_valid || !_mocap_z_valid) {
		_time_last_mocap = _sub_mocap_odom.get().timestamp;
		return -1;

	} else {
		_time_last_mocap = _sub_mocap_odom.get().timestamp;

		if (PX4_ISFINITE(_sub_mocap_odom.get().x)) {
			y.setZero();
			y(Y_mocap_x) = _sub_mocap_odom.get().x;
			y(Y_mocap_y) = _sub_mocap_odom.get().y;
			y(Y_mocap_z) = _sub_mocap_odom.get().z;
			_mocapStats.update(y);

			return OK;

		} else {
			return -1;
		}
	}
}

bool FlightTaskFailsafe::update()
{
	if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
		// stay at current position setpoint
		_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
		_thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;

	} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
		// don't move along xy
		_position_setpoint(0) = _position_setpoint(1) = NAN;
		_thrust_setpoint(0) = _thrust_setpoint(1) = NAN;
	}

	if (PX4_ISFINITE(_position(2))) {
		// stay at current altitude setpoint
		_velocity_setpoint(2) = 0.0f;
		_thrust_setpoint(2) = NAN;

	} else if (PX4_ISFINITE(_velocity(2))) {
		// land with landspeed
		_velocity_setpoint(2) = MPC_LAND_SPEED.get();
		_position_setpoint(2) = NAN;
		_thrust_setpoint(2) = NAN;
	}

	return true;

}

void BlockLocalPositionEstimator::publishLocalPos()
{
	const Vector<float, n_x> &xLP = _xLowPass.getState();

	// publish local position
	if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
	    && PX4_ISFINITE(_x(X_vz))) {
		_pub_lpos.get().timestamp = _timeStamp;
		_pub_lpos.get().xy_valid = _validXY;
		_pub_lpos.get().z_valid = _validZ;
		_pub_lpos.get().v_xy_valid = _validXY;
		_pub_lpos.get().v_z_valid = _validZ;
		_pub_lpos.get().x = xLP(X_x); 	// north
		_pub_lpos.get().y = xLP(X_y);  	// east
		_pub_lpos.get().z = xLP(X_z); 	// down
		_pub_lpos.get().vx = xLP(X_vx); // north
		_pub_lpos.get().vy = xLP(X_vy); // east
		_pub_lpos.get().vz = xLP(X_vz); // down
		_pub_lpos.get().yaw = _sub_att.get().yaw;
		_pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference
		_pub_lpos.get().z_global = _baroInitialized;
		_pub_lpos.get().ref_timestamp = _sub_home.get().timestamp;
		_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
		_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
		_pub_lpos.get().ref_alt = _sub_home.get().alt;
		_pub_lpos.get().dist_bottom = agl();
		_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
		_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
		_pub_lpos.get().dist_bottom_valid = _validTZ && _validZ;
		_pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
		_pub_lpos.get().epv = sqrtf(_P(X_z, X_z));
		_pub_lpos.update();
	}
}

void BlockLocalPositionEstimator::publishGlobalPos()
{
	// publish global position
	double lat = 0;
	double lon = 0;
	const Vector<float, n_x> &xLP = _xLowPass.getState();
	map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);
	float alt = -xLP(X_z) + _altHome;

	if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
	    PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
	    PX4_ISFINITE(xLP(X_vz))) {
		_pub_gpos.get().timestamp = _timeStamp;
		_pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec;
		_pub_gpos.get().lat = lat;
		_pub_gpos.get().lon = lon;
		_pub_gpos.get().alt = alt;
		_pub_gpos.get().vel_n = xLP(X_vx);
		_pub_gpos.get().vel_e = xLP(X_vy);
		_pub_gpos.get().vel_d = xLP(X_vz);
		_pub_gpos.get().yaw = _sub_att.get().yaw;
		_pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
		_pub_gpos.get().epv = sqrtf(_P(X_z, X_z));
		_pub_gpos.get().terrain_alt = _altHome - xLP(X_tz);
		_pub_gpos.get().terrain_alt_valid = _validTZ;
		_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
		_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0];
		_pub_gpos.update();
	}
}

	matrix::Vector<Type, M> update(const matrix::Matrix<Type, M, 1> &input)
	{
		for (size_t i = 0; i < M; i++) {
			if (!PX4_ISFINITE(getState()(i))) {
				setState(input);
			}
		}

		float b = 2 * float(M_PI) * getFCut() * getDt();
		float a = b / (1 + b);
		setState(input * a + getState() * (1 - a));
		return getState();
	}

void
Mission::heading_sp_update()
{
    if (_takeoff) {
        /* we don't want to be yawing during takeoff */
        return;
    }

    struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

    /* Don't change setpoint if last and current waypoint are not valid */
    if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
            !PX4_ISFINITE(_on_arrival_yaw)) {
        return;
    }

    /* Don't do FOH for landing and takeoff waypoints, the ground may be near
     * and the FW controller has a custom landing logic */
    if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
        return;
    }

    /* set yaw angle for the waypoint iff a loiter time has been specified */
    if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
        _mission_item.yaw = _on_arrival_yaw;
        /* always keep the front of the rotary wing pointing to the next waypoint */
    } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
        _mission_item.yaw = get_bearing_to_next_waypoint(
                                _navigator->get_global_position()->lat,
                                _navigator->get_global_position()->lon,
                                _mission_item.lat,
                                _mission_item.lon);
        /* always keep the back of the rotary wing pointing towards home */
    } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
        _mission_item.yaw = get_bearing_to_next_waypoint(
                                _navigator->get_global_position()->lat,
                                _navigator->get_global_position()->lon,
                                _navigator->get_home_position()->lat,
                                _navigator->get_home_position()->lon);
        /* always keep the back of the rotary wing pointing towards home */
    } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
        _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
                                         _navigator->get_global_position()->lat,
                                         _navigator->get_global_position()->lon,
                                         _navigator->get_home_position()->lat,
                                         _navigator->get_home_position()->lon) + M_PI_F);
    }

    mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
    _navigator->set_position_setpoint_triplet_updated();
}

void
Battery::filterCurrent(float current_a)
{
	if (_current_filtered_a < 0.0f) {
		_current_filtered_a = current_a;
	}

	// ADC poll is at 100Hz, this will perform a low pass over approx 500ms
	const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f;

	if (PX4_ISFINITE(filtered_next)) {
		_current_filtered_a = filtered_next;
	}
}

int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
			       float airspeedSp, unsigned mode, LimitOverride limitOverride)
{
	/* check if all input arguments are numbers and abort if not so */
	if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(altitude) ||
	    !PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) {
		return -1;
	}

	/* time measurement */
	updateTimeMeasurement();

	/* Filter altitude */
	float altitudeFiltered = _altitudeLowpass.update(altitude);


	/* calculate flight path angle setpoint from altitude setpoint */
	float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered);

	/* Debug output */
	if (_counter % 10 == 0) {
		debug("***");
		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f",
		      (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
	}

#if defined(__PX4_NUTTX)
	/* Write part of the status message */
	_status.get().altitudeSp = altitudeSp;
	_status.get().altitude_filtered = altitudeFiltered;
#endif // defined(__PX4_NUTTX)


	/* use flightpath angle setpoint for total energy control */
	return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed,
					  airspeedSp, mode, limitOverride);
}