Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

mc_pos_control: use global _dt member from the control block architecture

Browse Source
This commit is contained in:
Matthias Grob
2018-01-13 02:20:13 +01:00
parent a25ca0c37d
commit 931482941c
1 changed files with 57 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+57 -57
View File
@@ -345,31 +345,31 @@ private:
/**
* Set position setpoint using manual control
*/
void control_manual(float dt);
void control_manual();
void control_non_manual(float dt);
void control_non_manual();
/**
* Set position setpoint using offboard control
*/
void control_offboard(float dt);
void control_offboard();
/**
* Set position setpoint for AUTO
*/
void control_auto(float dt);
void control_auto();
void control_position(float dt);
void calculate_velocity_setpoint(float dt);
void calculate_thrust_setpoint(float dt);
void control_position();
void calculate_velocity_setpoint();
void calculate_thrust_setpoint();
void vel_sp_slewrate(float dt);
void vel_sp_slewrate();
void update_velocity_derivative();
void do_control(float dt);
void do_control();
void generate_attitude_setpoint(float dt);
void generate_attitude_setpoint();
float get_cruising_speed_xy();
@@ -377,9 +377,9 @@ private:
float get_vel_close(const matrix::Vector2f &unit_prev_to_current, const matrix::Vector2f &unit_current_to_next);
void set_manual_acceleration_xy(matrix::Vector2f &stick_input_xy_NED, const float dt);
void set_manual_acceleration_xy(matrix::Vector2f &stick_input_xy_NED);
void set_manual_acceleration_z(float &max_acc_z, const float stick_input_z_NED, const float dt);
void set_manual_acceleration_z(float &max_acc_z, const float stick_input_z_NED);
/**
* limit altitude based on several conditions
@@ -1043,7 +1043,7 @@ MulticopterPositionControl::get_cruising_speed_xy()
}
void
MulticopterPositionControl::set_manual_acceleration_z(float &max_acceleration, const float stick_z, const float dt)
MulticopterPositionControl::set_manual_acceleration_z(float &max_acceleration, const float stick_z)
{
@@ -1087,10 +1087,10 @@ MulticopterPositionControl::set_manual_acceleration_z(float &max_acceleration, c
if (_user_intention_z == brake) {
/* limit jerk when braking to zero */
float jerk = (_acceleration_z_max_up.get() - _acceleration_state_dependent_z) / dt;
float jerk = (_acceleration_z_max_up.get() - _acceleration_state_dependent_z) / _dt;
if (jerk > _manual_jerk_limit_z) {
_acceleration_state_dependent_z = _manual_jerk_limit_z * dt + _acceleration_state_dependent_z;
_acceleration_state_dependent_z = _manual_jerk_limit_z * _dt + _acceleration_state_dependent_z;
} else {
_acceleration_state_dependent_z = _acceleration_z_max_up.get();
@@ -1104,7 +1104,7 @@ MulticopterPositionControl::set_manual_acceleration_z(float &max_acceleration, c
}
void
MulticopterPositionControl::set_manual_acceleration_xy(matrix::Vector2f &stick_xy, const float dt)
MulticopterPositionControl::set_manual_acceleration_xy(matrix::Vector2f &stick_xy)
{
/*
@@ -1259,10 +1259,10 @@ MulticopterPositionControl::set_manual_acceleration_xy(matrix::Vector2f &stick_x
case brake: {
/* limit jerk when braking to zero */
float jerk = (_acceleration_hor_max.get() - _acceleration_state_dependent_xy) / dt;
float jerk = (_acceleration_hor_max.get() - _acceleration_state_dependent_xy) / _dt;
if (jerk > _manual_jerk_limit_xy) {
_acceleration_state_dependent_xy = _manual_jerk_limit_xy * dt + _acceleration_state_dependent_xy;
_acceleration_state_dependent_xy = _manual_jerk_limit_xy * _dt + _acceleration_state_dependent_xy;
} else {
_acceleration_state_dependent_xy = _acceleration_hor_max.get();
@@ -1314,7 +1314,7 @@ MulticopterPositionControl::set_manual_acceleration_xy(matrix::Vector2f &stick_x
}
void
MulticopterPositionControl::control_manual(float dt)
MulticopterPositionControl::control_manual()
{
/* Entering manual control from non-manual control mode, reset alt/pos setpoints */
if (_mode_auto) {
@@ -1367,10 +1367,10 @@ MulticopterPositionControl::control_manual(float dt)
/* adjust acceleration based on stick input */
matrix::Vector2f stick_xy(man_vel_sp(0), man_vel_sp(1));
set_manual_acceleration_xy(stick_xy, dt);
set_manual_acceleration_xy(stick_xy);
float stick_z = man_vel_sp(2);
float max_acc_z;
set_manual_acceleration_z(max_acc_z, stick_z, dt);
set_manual_acceleration_z(max_acc_z, stick_z);
/* prepare cruise speed (m/s) vector to scale the velocity setpoint */
float vel_mag = (_velocity_hor_manual.get() < _vel_max_xy) ? _velocity_hor_manual.get() : _vel_max_xy;
@@ -1463,16 +1463,16 @@ MulticopterPositionControl::control_manual(float dt)
_vel_sp(1) = man_vel_sp(1);
}
control_position(dt);
control_position();
}
void
MulticopterPositionControl::control_non_manual(float dt)
MulticopterPositionControl::control_non_manual()
{
/* select control source */
if (_control_mode.flag_control_offboard_enabled) {
/* offboard control */
control_offboard(dt);
control_offboard();
_mode_auto = false;
} else {
@@ -1480,7 +1480,7 @@ MulticopterPositionControl::control_non_manual(float dt)
_hold_offboard_z = false;
/* AUTO */
control_auto(dt);
control_auto();
}
// guard against any bad velocity values
@@ -1545,12 +1545,12 @@ MulticopterPositionControl::control_non_manual(float dt)
_att_sp.timestamp = hrt_absolute_time();
} else {
control_position(dt);
control_position();
}
}
void
MulticopterPositionControl::control_offboard(float dt)
MulticopterPositionControl::control_offboard()
{
if (_pos_sp_triplet.current.valid) {
@@ -1637,7 +1637,7 @@ MulticopterPositionControl::control_offboard(float dt)
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_pos_sp_triplet.current.yawspeed_valid) {
float yaw_target = _wrap_pi(_att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt);
float yaw_target = _wrap_pi(_att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * _dt);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
_params.global_yaw_max;
@@ -1661,21 +1661,21 @@ MulticopterPositionControl::control_offboard(float dt)
}
void
MulticopterPositionControl::vel_sp_slewrate(float dt)
MulticopterPositionControl::vel_sp_slewrate()
{
matrix::Vector2f vel_sp_xy(_vel_sp(0), _vel_sp(1));
matrix::Vector2f vel_sp_prev_xy(_vel_sp_prev(0), _vel_sp_prev(1));
matrix::Vector2f acc_xy = (vel_sp_xy - vel_sp_prev_xy) / dt;
matrix::Vector2f acc_xy = (vel_sp_xy - vel_sp_prev_xy) / _dt;
/* limit total horizontal acceleration */
if (acc_xy.length() > _acceleration_state_dependent_xy) {
vel_sp_xy = _acceleration_state_dependent_xy * acc_xy.normalized() * dt + vel_sp_prev_xy;
vel_sp_xy = _acceleration_state_dependent_xy * acc_xy.normalized() * _dt + vel_sp_prev_xy;
_vel_sp(0) = vel_sp_xy(0);
_vel_sp(1) = vel_sp_xy(1);
}
/* limit vertical acceleration */
float acc_z = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
float acc_z = (_vel_sp(2) - _vel_sp_prev(2)) / _dt;
float max_acc_z;
if (_control_mode.flag_control_manual_enabled) {
@@ -1686,7 +1686,7 @@ MulticopterPositionControl::vel_sp_slewrate(float dt)
}
if (fabsf(acc_z) > fabsf(max_acc_z)) {
_vel_sp(2) = max_acc_z * dt + _vel_sp_prev(2);
_vel_sp(2) = max_acc_z * _dt + _vel_sp_prev(2);
}
}
@@ -1740,7 +1740,7 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, c
}
}
void MulticopterPositionControl::control_auto(float dt)
void MulticopterPositionControl::control_auto()
{
/* reset position setpoint on AUTO mode activation or if we are not in MC mode */
if (!_mode_auto || !_vehicle_status.is_rotary_wing) {
@@ -1868,7 +1868,7 @@ void MulticopterPositionControl::control_auto(float dt)
/* update yaw setpoint if needed */
if (_pos_sp_triplet.current.yawspeed_valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) {
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * _dt;
} else if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
@@ -1943,11 +1943,11 @@ void MulticopterPositionControl::control_auto(float dt)
} else if (dist_to_prev_z < target_threshold_z) {
/* we want to accelerate */
float acc_z = (vel_sp_z - fabsf(_vel_sp(2))) / dt;
float acc_z = (vel_sp_z - fabsf(_vel_sp(2))) / _dt;
float acc_max = (flying_upward) ? (_acceleration_z_max_up.get() * 0.5f) : (_acceleration_z_max_down.get() * 0.5f);
if (acc_z > acc_max) {
vel_sp_z = _acceleration_z_max_up.get() * dt + fabsf(_vel_sp(2));
vel_sp_z = _acceleration_z_max_up.get() * _dt + fabsf(_vel_sp(2));
}
}
@@ -2095,13 +2095,13 @@ void MulticopterPositionControl::control_auto(float dt)
/* we want to accelerate not too fast
* TODO: change the name acceleration_hor_man to something that can
* be used by auto and manual */
float acc_track = (final_cruise_speed - vel_sp_along_track_prev) / dt;
float acc_track = (final_cruise_speed - vel_sp_along_track_prev) / _dt;
/* if yaw offset is large, only accelerate with 0.5m/s^2 */
float acc = (fabsf(yaw_diff) > math::radians(_mis_yaw_error.get())) ? 0.5f : _acceleration_hor.get();
if (acc_track > acc) {
vel_sp_along_track = acc * dt + vel_sp_along_track_prev;
vel_sp_along_track = acc * _dt + vel_sp_along_track_prev;
}
/* enforce minimum cruise speed */
@@ -2372,7 +2372,7 @@ MulticopterPositionControl::update_velocity_derivative()
}
void
MulticopterPositionControl::do_control(float dt)
MulticopterPositionControl::do_control()
{
/* by default, run position/altitude controller. the control_* functions
* can disable this and run velocity controllers directly in this cycle */
@@ -2381,7 +2381,7 @@ MulticopterPositionControl::do_control(float dt)
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
control_manual(dt);
control_manual();
_mode_auto = false;
/* we set triplets to false
@@ -2399,18 +2399,18 @@ MulticopterPositionControl::do_control(float dt)
/* reset acceleration to default */
_acceleration_state_dependent_xy = _acceleration_hor_max.get();
_acceleration_state_dependent_z = _acceleration_z_max_up.get();
control_non_manual(dt);
control_non_manual();
}
}
void
MulticopterPositionControl::control_position(float dt)
MulticopterPositionControl::control_position()
{
calculate_velocity_setpoint(dt);
calculate_velocity_setpoint();
if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled) {
calculate_thrust_setpoint(dt);
calculate_thrust_setpoint();
} else {
_reset_int_z = true;
@@ -2418,7 +2418,7 @@ MulticopterPositionControl::control_position(float dt)
}
void
MulticopterPositionControl::calculate_velocity_setpoint(float dt)
MulticopterPositionControl::calculate_velocity_setpoint()
{
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control) {
@@ -2492,14 +2492,14 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
/* apply slewrate (aka acceleration limit) for smooth flying */
if (!_control_mode.flag_control_auto_enabled && !_in_smooth_takeoff) {
vel_sp_slewrate(dt);
vel_sp_slewrate();
}
/* special velocity setpoint limitation for smooth takeoff (after slewrate!) */
if (_in_smooth_takeoff) {
_in_smooth_takeoff = _takeoff_vel_limit < -_vel_sp(2);
/* ramp vertical velocity limit up to takeoff speed */
_takeoff_vel_limit += -_vel_sp(2) * dt / _takeoff_ramp_time.get();
_takeoff_vel_limit += -_vel_sp(2) * _dt / _takeoff_ramp_time.get();
/* limit vertical velocity to the current ramp value */
_vel_sp(2) = math::max(_vel_sp(2), -_takeoff_vel_limit);
}
@@ -2518,7 +2518,7 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
}
void
MulticopterPositionControl::calculate_thrust_setpoint(float dt)
MulticopterPositionControl::calculate_thrust_setpoint()
{
/* reset integrals if needed */
if (_control_mode.flag_control_climb_rate_enabled) {
@@ -2730,12 +2730,12 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
/* update integrals */
if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
_thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
_thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
_thrust_int(0) += vel_err(0) * _params.vel_i(0) * _dt;
_thrust_int(1) += vel_err(1) * _params.vel_i(1) * _dt;
}
if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
_thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
_thrust_int(2) += vel_err(2) * _params.vel_i(2) * _dt;
}
/* calculate attitude setpoint from thrust vector */
@@ -2819,7 +2819,7 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
}
void
MulticopterPositionControl::generate_attitude_setpoint(float dt)
MulticopterPositionControl::generate_attitude_setpoint()
{
// yaw setpoint is integrated over time, but we don't want to integrate the offset's
_att_sp.yaw_body -= _man_yaw_offset;
@@ -2841,7 +2841,7 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt)
const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * _dt);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
@@ -3042,7 +3042,7 @@ MulticopterPositionControl::task_main()
parameters_update(false);
hrt_abstime t = hrt_absolute_time();
float dt = t_prev != 0 ? (t - t_prev) / 1e6f : 0.004f;
const float dt = t_prev != 0 ? (t - t_prev) / 1e6f : 0.004f;
t_prev = t;
/* set dt for control blocks */
@@ -3130,7 +3130,7 @@ MulticopterPositionControl::task_main()
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled) {
do_control(dt);
do_control();
/* fill local position, velocity and thrust setpoint */
_local_pos_sp.timestamp = hrt_absolute_time();
@@ -3166,7 +3166,7 @@ MulticopterPositionControl::task_main()
/* generate attitude setpoint from manual controls */
if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
generate_attitude_setpoint(dt);
generate_attitude_setpoint();
} else {
_reset_yaw_sp = true;