zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

MulticopterPositionControl: rename local_pos -> vehicle_local_position

main
Matthias Grob 3 years ago
parent
4ffe796b4d
commit
8180f931de
1 changed files with 44 additions and 41 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 85
      src/modules/mc_pos_control/MulticopterPositionControl.cpp

85
src/modules/mc_pos_control/MulticopterPositionControl.cpp
Unescape View File

@ -241,32 +241,35 @@ void MulticopterPositionControl::parameters_update(bool force)
} }
} }
PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehicle_local_position_s &local_pos) PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehicle_local_position_s
&vehicle_local_position)
{ {
PositionControlStates states; PositionControlStates states;
// only set position states if valid and finite // only set position states if valid and finite
if (PX4_ISFINITE(local_pos.x) && PX4_ISFINITE(local_pos.y) && local_pos.xy_valid) { if (PX4_ISFINITE(vehicle_local_position.x) && PX4_ISFINITE(vehicle_local_position.y)
states.position(0) = local_pos.x; && vehicle_local_position.xy_valid) {
states.position(1) = local_pos.y; states.position(0) = vehicle_local_position.x;
states.position(1) = vehicle_local_position.y;
} else { } else {
states.position(0) = NAN; states.position(0) = NAN;
states.position(1) = NAN; states.position(1) = NAN;
} }
if (PX4_ISFINITE(local_pos.z) && local_pos.z_valid) { if (PX4_ISFINITE(vehicle_local_position.z) && vehicle_local_position.z_valid) {
states.position(2) = local_pos.z; states.position(2) = vehicle_local_position.z;
} else { } else {
states.position(2) = NAN; states.position(2) = NAN;
} }
if (PX4_ISFINITE(local_pos.vx) && PX4_ISFINITE(local_pos.vy) && local_pos.v_xy_valid) { if (PX4_ISFINITE(vehicle_local_position.vx) && PX4_ISFINITE(vehicle_local_position.vy)
states.velocity(0) = local_pos.vx; && vehicle_local_position.v_xy_valid) {
states.velocity(1) = local_pos.vy; states.velocity(0) = vehicle_local_position.vx;
states.acceleration(0) = _vel_x_deriv.update(local_pos.vx); states.velocity(1) = vehicle_local_position.vy;
states.acceleration(1) = _vel_y_deriv.update(local_pos.vy); states.acceleration(0) = _vel_x_deriv.update(vehicle_local_position.vx);
states.acceleration(1) = _vel_y_deriv.update(vehicle_local_position.vy);
} else { } else {
states.velocity(0) = NAN; states.velocity(0) = NAN;
@ -279,8 +282,8 @@ PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehic
_vel_y_deriv.reset(); _vel_y_deriv.reset();
} }
if (PX4_ISFINITE(local_pos.vz) && local_pos.v_z_valid) { if (PX4_ISFINITE(vehicle_local_position.vz) && vehicle_local_position.v_z_valid) {
states.velocity(2) = local_pos.vz; states.velocity(2) = vehicle_local_position.vz;
states.acceleration(2) = _vel_z_deriv.update(states.velocity(2)); states.acceleration(2) = _vel_z_deriv.update(states.velocity(2));
} else { } else {
@ -291,7 +294,7 @@ PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehic
_vel_z_deriv.reset(); _vel_z_deriv.reset();
} }
states.yaw = local_pos.heading; states.yaw = vehicle_local_position.heading;
return states; return states;
} }
@ -310,10 +313,10 @@ void MulticopterPositionControl::Run()
parameters_update(false); parameters_update(false);
perf_begin(_cycle_perf); perf_begin(_cycle_perf);
vehicle_local_position_s local_pos; vehicle_local_position_s vehicle_local_position;
if (_local_pos_sub.update(&local_pos)) { if (_local_pos_sub.update(&vehicle_local_position)) {
const hrt_abstime time_stamp_now = local_pos.timestamp_sample; const hrt_abstime time_stamp_now = vehicle_local_position.timestamp_sample;
const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f); const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f);
_time_stamp_last_loop = time_stamp_now; _time_stamp_last_loop = time_stamp_now;
@ -349,48 +352,48 @@ void MulticopterPositionControl::Run()
const bool is_trajectory_setpoint_updated = _trajectory_setpoint_sub.update(&_setpoint); const bool is_trajectory_setpoint_updated = _trajectory_setpoint_sub.update(&_setpoint);
// adjust existing (or older) setpoint with any EKF reset deltas // adjust existing (or older) setpoint with any EKF reset deltas
if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < local_pos.timestamp)) { if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) {
if (local_pos.vxy_reset_counter != _vxy_reset_counter) { if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
_setpoint.vx += local_pos.delta_vxy[0]; _setpoint.vx += vehicle_local_position.delta_vxy[0];
_setpoint.vy += local_pos.delta_vxy[1]; _setpoint.vy += vehicle_local_position.delta_vxy[1];
} }
if (local_pos.vz_reset_counter != _vz_reset_counter) { if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
_setpoint.vz += local_pos.delta_vz; _setpoint.vz += vehicle_local_position.delta_vz;
} }
if (local_pos.xy_reset_counter != _xy_reset_counter) { if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) {
_setpoint.x += local_pos.delta_xy[0]; _setpoint.x += vehicle_local_position.delta_xy[0];
_setpoint.y += local_pos.delta_xy[1]; _setpoint.y += vehicle_local_position.delta_xy[1];
} }
if (local_pos.z_reset_counter != _z_reset_counter) { if (vehicle_local_position.z_reset_counter != _z_reset_counter) {
_setpoint.z += local_pos.delta_z; _setpoint.z += vehicle_local_position.delta_z;
} }
if (local_pos.heading_reset_counter != _heading_reset_counter) { if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
_setpoint.yaw += local_pos.delta_heading; _setpoint.yaw += vehicle_local_position.delta_heading;
} }
} }
if (local_pos.vxy_reset_counter != _vxy_reset_counter) { if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
_vel_x_deriv.reset(); _vel_x_deriv.reset();
_vel_y_deriv.reset(); _vel_y_deriv.reset();
} }
if (local_pos.vz_reset_counter != _vz_reset_counter) { if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
_vel_z_deriv.reset(); _vel_z_deriv.reset();
} }
// save latest reset counters // save latest reset counters
_vxy_reset_counter = local_pos.vxy_reset_counter; _vxy_reset_counter = vehicle_local_position.vxy_reset_counter;
_vz_reset_counter = local_pos.vz_reset_counter; _vz_reset_counter = vehicle_local_position.vz_reset_counter;
_xy_reset_counter = local_pos.xy_reset_counter; _xy_reset_counter = vehicle_local_position.xy_reset_counter;
_z_reset_counter = local_pos.z_reset_counter; _z_reset_counter = vehicle_local_position.z_reset_counter;
_heading_reset_counter = local_pos.heading_reset_counter; _heading_reset_counter = vehicle_local_position.heading_reset_counter;
PositionControlStates states{set_vehicle_states(local_pos)}; PositionControlStates states{set_vehicle_states(vehicle_local_position)};
if (_vehicle_control_mode.flag_multicopter_position_control_enabled) { if (_vehicle_control_mode.flag_multicopter_position_control_enabled) {
@ -400,7 +403,7 @@ void MulticopterPositionControl::Run()
&& (time_stamp_now > _time_position_control_enabled)) { && (time_stamp_now > _time_position_control_enabled)) {
failsafe(time_stamp_now, _setpoint, states); failsafe(time_stamp_now, _setpoint, states);
_setpoint.timestamp = local_pos.timestamp; _setpoint.timestamp = vehicle_local_position.timestamp;
} }
} }
@ -498,14 +501,14 @@ void MulticopterPositionControl::Run()
// update states // update states
if (!PX4_ISFINITE(_setpoint.z) if (!PX4_ISFINITE(_setpoint.z)
&& PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON) && PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON)
&& PX4_ISFINITE(local_pos.z_deriv) && local_pos.z_valid && local_pos.v_z_valid) { && PX4_ISFINITE(vehicle_local_position.z_deriv) && vehicle_local_position.z_valid && vehicle_local_position.v_z_valid) {
// A change in velocity is demanded and the altitude is not controlled. // A change in velocity is demanded and the altitude is not controlled.
// Set velocity to the derivative of position // Set velocity to the derivative of position
// because it has less bias but blend it in across the landing speed range // because it has less bias but blend it in across the landing speed range
// < MPC_LAND_SPEED: ramp up using altitude derivative without a step // < MPC_LAND_SPEED: ramp up using altitude derivative without a step
// >= MPC_LAND_SPEED: use altitude derivative // >= MPC_LAND_SPEED: use altitude derivative
float weighting = fminf(fabsf(_setpoint.vz) / _param_mpc_land_speed.get(), 1.f); float weighting = fminf(fabsf(_setpoint.vz) / _param_mpc_land_speed.get(), 1.f);
states.velocity(2) = local_pos.z_deriv * weighting + local_pos.vz * (1.f - weighting); states.velocity(2) = vehicle_local_position.z_deriv * weighting + vehicle_local_position.vz * (1.f - weighting);
} }
_control.setState(states); _control.setState(states);

Write Preview
Loading…
Cancel
Save