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);
}
float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&
PX4_ISFINITE(ctl_data.yaw))) {
return _rate_setpoint;
}
/* Calculate the error */
float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw;
/* shortest angle (wrap around) */
yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F;
/*warnx("yaw_error: %.4f", (double)yaw_error);*/
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = yaw_error / _tc;
/* limit the rate */
if (_max_rate > 0.01f) {
if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
} else {
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
}
return _rate_setpoint;
}
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);
}
int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, 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(flightPathAngleSp) ||
!PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) {
return -1;
}
/* time measurement */
updateTimeMeasurement();
/* Filter airspeed */
float airspeedFiltered = _airspeedLowpass.update(airspeed);
/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered);
/* Debug output */
if (_counter % 10 == 0) {
debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f,"
"accelerationLongitudinalSp%.4f",
(double)airspeedSp, (double)airspeed,
(double)airspeedFiltered, (double)accelerationLongitudinalSp);
}
#if defined(__PX4_NUTTX)
/* Write part of the status message */
_status.get().airspeedSp = airspeedSp;
_status.get().airspeed_filtered = airspeedFiltered;
#endif // defined(__PX4_NUTTX)
/* use longitudinal acceleration setpoint for total energy control */
return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered,
accelerationLongitudinalSp, mode, limitOverride);
}
float LowPassFilter2p::reset(float sample)
{
const float dval = sample / (_b0 + _b1 + _b2);
if (PX4_ISFINITE(dval)) {
_delay_element_1 = dval;
_delay_element_2 = dval;
} else {
_delay_element_1 = sample;
_delay_element_2 = sample;
}
return apply(sample);
}
void
Battery::computeScale()
{
const float voltage_range = (_param_v_full.get() - _param_v_empty.get());
// reusing capacity calculation to get single cell voltage before drop
const float bat_v = _param_v_empty.get() + (voltage_range * _remaining_voltage);
_scale = _param_v_full.get() / bat_v;
if (_scale > 1.3f) { // Allow at most 30% compensation
_scale = 1.3f;
} else if (!PX4_ISFINITE(_scale) || _scale < 1.0f) { // Shouldn't ever be more than the power at full battery
_scale = 1.0f;
}
}
// Returns calibrate_return_error if any parameter is not finite
// Logs if parameters are out of range
static calibrate_return check_calibration_result(float offset_x, float offset_y, float offset_z,
float sphere_radius,
float diag_x, float diag_y, float diag_z,
float offdiag_x, float offdiag_y, float offdiag_z,
orb_advert_t *mavlink_log_pub, size_t cur_mag)
{
float must_be_finite[] = {offset_x, offset_y, offset_z,
sphere_radius,
diag_x, diag_y, diag_z,
offdiag_x, offdiag_y, offdiag_z};
float should_be_not_huge[] = {offset_x, offset_y, offset_z};
float should_be_positive[] = {sphere_radius, diag_x, diag_y, diag_z};
// Make sure every parameter is finite
const int num_finite = sizeof(must_be_finite) / sizeof(*must_be_finite);
for (unsigned i = 0; i < num_finite; ++i) {
if (!PX4_ISFINITE(must_be_finite[i])) {
calibration_log_emergency(mavlink_log_pub,
"ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
return calibrate_return_error;
}
}
// Notify if offsets are too large
const int num_not_huge = sizeof(should_be_not_huge) / sizeof(*should_be_not_huge);
for (unsigned i = 0; i < num_not_huge; ++i) {
if (fabsf(should_be_not_huge[i]) > MAG_MAX_OFFSET_LEN) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets",
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
break;
}
}
// Notify if a parameter which should be positive is non-positive
const int num_positive = sizeof(should_be_positive) / sizeof(*should_be_positive);
for (unsigned i = 0; i < num_positive; ++i) {
if (should_be_positive[i] <= 0.0f) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with non-positive scale",
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
break;
}
}
return calibrate_return_ok;
}
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) + _altOrigin;
// lie about eph/epv to allow visual odometry only navigation when velocity est. good
float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));
float epv = sqrtf(_P(X_z, X_z));
float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
float eph_thresh = 3.0f;
float epv_thresh = 3.0f;
if (vxy_stddev < _vxy_pub_thresh.get()) {
if (eph > eph_thresh) {
eph = eph_thresh;
}
if (epv > epv_thresh) {
epv = epv_thresh;
}
}
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().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);
// this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity
_pub_gpos.get().pos_d_deriv = xLP(X_vz);
_pub_gpos.get().yaw = _eul(2);
_pub_gpos.get().eph = eph;
_pub_gpos.get().epv = epv;
_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter;
_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);
_pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ;
_pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY);
_pub_gpos.update();
// TODO provide calculated values for these
_pub_gpos.get().evh = 0.0f;
_pub_gpos.get().evv = 0.0f;
}
}
float LowPassFilter2p::apply(float sample)
{
// do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (!PX4_ISFINITE(delay_element_0)) {
// don't allow bad values to propagate via the filter
delay_element_0 = sample;
}
const float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
_delay_element_2 = _delay_element_1;
_delay_element_1 = delay_element_0;
// return the value. Should be no need to check limits
return output;
}
/*
* matrix inverse code for any square matrix using LU decomposition
* inv = inv(U)*inv(L)*P, where L and U are triagular matrices and P the pivot matrix
* ref: http://www.cl.cam.ac.uk/teaching/1314/NumMethods/supporting/mcmaster-kiruba-ludecomp.pdf
* @param m, input 4x4 matrix
* @param inv, Output inverted 4x4 matrix
* @param n, dimension of square matrix
* @returns false = matrix is Singular, true = matrix inversion successful
*/
bool mat_inverse(float *A, float *inv, uint8_t n)
{
float *L, *U, *P;
bool ret = true;
L = new float[n * n];
U = new float[n * n];
P = new float[n * n];
mat_LU_decompose(A, L, U, P, n);
float *L_inv = new float[n * n];
float *U_inv = new float[n * n];
memset(L_inv, 0, n * n * sizeof(float));
mat_forward_sub(L, L_inv, n);
memset(U_inv, 0, n * n * sizeof(float));
mat_back_sub(U, U_inv, n);
// decomposed matrices no longer required
delete[] L;
delete[] U;
float *inv_unpivoted = mat_mul(U_inv, L_inv, n);
float *inv_pivoted = mat_mul(inv_unpivoted, P, n);
//check sanity of results
for (uint8_t i = 0; i < n; i++) {
for (uint8_t j = 0; j < n; j++) {
if (!PX4_ISFINITE(inv_pivoted[i * n + j])) {
ret = false;
}
}
}
memcpy(inv, inv_pivoted, n * n * sizeof(float));
//free memory
delete[] inv_pivoted;
delete[] inv_unpivoted;
delete[] P;
delete[] U_inv;
delete[] L_inv;
return ret;
}
/*
* 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;
/* throttle pid attenuation factor */
float tpa = fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint)));
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
_params.rate_ff.emult(_rates_sp);
_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 = AXIS_INDEX_ROLL; i < AXIS_COUNT; 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 &&
/* if the axis is the yaw axis, do not update the integral if the limit is hit */
!((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) {
_rates_int(i) = rate_i;
}
}
}
}
}
void FollowTarget::update_position_sp(bool use_velocity, bool use_position, float yaw_rate)
{
// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// activate line following in pos control if position is valid
pos_sp_triplet->previous.valid = use_position;
pos_sp_triplet->previous = pos_sp_triplet->current;
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
pos_sp_triplet->current.position_valid = use_position;
pos_sp_triplet->current.velocity_valid = use_velocity;
pos_sp_triplet->current.vx = _current_vel(0);
pos_sp_triplet->current.vy = _current_vel(1);
pos_sp_triplet->next.valid = false;
pos_sp_triplet->current.yawspeed_valid = PX4_ISFINITE(yaw_rate);
pos_sp_triplet->current.yawspeed = yaw_rate;
_navigator->set_position_setpoint_triplet_updated();
}