dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_pos_control: multiple small fixes in position controller we acumulated over time during our PX4 deployment and want to contribute back

This commit is contained in:
Matthias Grob 2017-10-06 14:41:39 +02:00 committed by Dennis Mannhart
parent c177d6491a
commit 2b7dcd3f34
2 changed files with 75 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/vehicle_attitude_setpoint.msg
View File

@ -1,3 +1,5 @@
int8 LANDING_GEAR_UP = 1 # landing gear up
int8 LANDING_GEAR_DOWN = -1 # landing gear down
float32 roll_body # body angle in NED frame
float32 pitch_body # body angle in NED frame

112
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -114,7 +114,7 @@ private:
static constexpr uint64_t DIRECTION_CHANGE_TRIGGER_TIME_US = 100000;
bool _task_should_exit = false; /**<true if task should exit */
bool _gear_state_initialized = false; /**<true if the gear state has been initialized */
bool _gear_state_initialized = false; /**<true if the gear state has been initialized */
bool _reset_pos_sp = true; /**<true if position setpoint needs a reset */
bool _reset_alt_sp = true; /**<true if altitude setpoint needs a reset */
bool _do_reset_alt_pos_flag = true; /**< TODO: check if we need this */
@ -176,8 +176,9 @@ private:
control::BlockParamFloat _velocity_hor_manual; /**< target velocity in manual controlled mode at full speed*/
control::BlockParamFloat _nav_rad; /**< radius that is used by navigator that defines when to update triplets */
control::BlockParamFloat _takeoff_ramp_time; /**< time contant for smooth takeoff ramp */
control::BlockParamFloat _jerk_hor_max; /**< maximum jerk in manual controlled mode when breaking to zero */
control::BlockParamFloat _jerk_hor_min; /**< minimum jerk in manual controlled mode when breaking to zero */
control::BlockParamFloat _jerk_hor_max; /**< maximum jerk in manual controlled mode when braking to zero */
control::BlockParamFloat _jerk_hor_min; /**< minimum jerk in manual controlled mode when braking to zero */
control::BlockParamFloat _mis_yaw_error; /**< yaw error threshold that is used in mission as update criteria */
control::BlockDerivative _vel_x_deriv;
control::BlockDerivative _vel_y_deriv;
control::BlockDerivative _vel_z_deriv;
@ -445,6 +446,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_takeoff_ramp_time(this, "TKO_RAMP_T", true),
_jerk_hor_max(this, "JERK_MAX", true),
_jerk_hor_min(this, "JERK_MIN", true),
_mis_yaw_error(this, "MIS_YAW_ERR", false),
_vel_x_deriv(this, "VELD"),
_vel_y_deriv(this, "VELD"),
_vel_z_deriv(this, "VELD"),
@ -471,7 +473,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vxy_reset_counter(0),
_heading_reset_counter(0)
{
// Make the quaternion valid for control state
/* Make the attitude quaternion valid */
_att.q[0] = 1.0f;
_ref_pos = {};
@ -493,7 +495,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel_err_d.zero();
_curr_pos_sp.zero();
_prev_pos_sp.zero();
_stick_input_xy_prev = {};
_stick_input_xy_prev.zero();
_R.identity();
_R_setpoint.identity();
@ -914,26 +916,32 @@ MulticopterPositionControl::reset_alt_sp()
void
MulticopterPositionControl::limit_altitude()
{
/* in altitude control, limit setpoint */
if (_run_alt_control && _pos_sp(2) <= -_vehicle_land_detected.alt_max) {
_pos_sp(2) = -_vehicle_land_detected.alt_max;
return;
}
if (_vehicle_land_detected.alt_max > 0.0f) {
/* in velocity control mode and want to fly upwards */
if (!_run_alt_control && _vel_sp(2) <= 0.0f) {
float altitude_above_home = -_pos(2) + _home_pos.z;
/* time to travel to reach zero velocity */
float delta_t = -_vel(2) / _acceleration_z_max_down.get();
/* predicted position */
float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f * _acceleration_z_max_down.get() * delta_t *delta_t;
if (pos_z_next <= -_vehicle_land_detected.alt_max) {
_pos_sp(2) = -_vehicle_land_detected.alt_max;
_run_alt_control = true;
/* in altitude control, lim_pos_sp(2)it setpoint */
if (_run_alt_control && altitude_above_home > _vehicle_land_detected.alt_max) {
_pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z;
return;
}
/* in velocity control mode and want to fly upwards */
if (!_run_alt_control && _vel_sp(2) <= 0.0f) {
/* time to travel to reach zero velocity */
float delta_t = -_vel(2) / _acceleration_z_max_down.get();
/* predicted position */
float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f *
_acceleration_z_max_down.get() * delta_t *delta_t;
if ((-pos_z_next + _home_pos.z) > _vehicle_land_detected.alt_max) {
_pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z;
_run_alt_control = true;
return;
}
}
}
}
@ -1028,7 +1036,7 @@ MulticopterPositionControl::get_cruising_speed_xy()
/*
* in mission the user can choose cruising speed different to default
*/
return ((PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) && (_pos_sp_triplet.current.cruising_speed > 0.1f)) ?
return ((PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) && !(_pos_sp_triplet.current.cruising_speed < 0.0f)) ?
_pos_sp_triplet.current.cruising_speed : _params.vel_cruise_xy);
}
@ -1860,6 +1868,17 @@ void MulticopterPositionControl::control_auto(float dt)
if (current_setpoint_valid &&
(_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) {
/* 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;
} else if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
float yaw_diff = _wrap_pi(_att_sp.yaw_body - _yaw);
/* only follow previous-current-line for specific triplet type */
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER ||
@ -2081,8 +2100,11 @@ void MulticopterPositionControl::control_auto(float dt)
* be used by auto and manual */
float acc_track = (final_cruise_speed - vel_sp_along_track_prev) / dt;
if (acc_track > _acceleration_hor.get()) {
vel_sp_along_track = _acceleration_hor.get() * dt + vel_sp_along_track_prev;
/* 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;
}
/* enforce minimum cruise speed */
@ -2211,6 +2233,24 @@ void MulticopterPositionControl::control_auto(float dt)
_pos_sp = pos_sp;
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_VELOCITY) {
float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1));
if (vel_xy_mag > SIGMA_NORM) {
_vel_sp(0) = _vel(0) / vel_xy_mag * get_cruising_speed_xy();
_vel_sp(1) = _vel(1) / vel_xy_mag * get_cruising_speed_xy();
} else {
/* TODO: we should go in the direction we are heading
* if current velocity is zero
*/
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
}
_run_pos_control = false;
} else {
/* just go to the target point */;
_pos_sp = _curr_pos_sp;
@ -2227,14 +2267,6 @@ void MulticopterPositionControl::control_auto(float dt)
_pos_sp = _curr_pos_sp;
}
/* 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;
} else if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
/*
* if we're already near the current takeoff setpoint don't reset in case we switch back to posctl.
@ -2261,14 +2293,14 @@ void MulticopterPositionControl::control_auto(float dt)
!_vehicle_land_detected.landed &&
high_enough_for_landing_gear) {
_att_sp.landing_gear = 1.0f;
_att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP;
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF ||
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND ||
!high_enough_for_landing_gear) {
// During takeoff and landing, we better put it down again.
_att_sp.landing_gear = -1.0f;
_att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN;
// For the rest of the setpoint types, just leave it as is.
}
@ -2391,7 +2423,10 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
}
}
limit_altitude();
/* in auto the setpoint is already limited by the navigator */
if (!_control_mode.flag_control_auto_enabled) {
limit_altitude();
}
if (_run_alt_control) {
if (PX4_ISFINITE(_pos_sp(2))) {
@ -2919,10 +2954,10 @@ MulticopterPositionControl::generate_attitude_setpoint(float dt)
// until he toggles the switch to avoid retracting the gear immediately on takeoff.
if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized &&
!_vehicle_land_detected.landed) {
_att_sp.landing_gear = 1.0f;
_att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP;
} else if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
_att_sp.landing_gear = -1.0f;
_att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN;
// Switching the gear off does put it into a safe defined state
_gear_state_initialized = true;
}
@ -2959,7 +2994,7 @@ MulticopterPositionControl::task_main()
hrt_abstime t_prev = 0;
// Let's be safe and have the landing gear down by default
_att_sp.landing_gear = -1.0f;
_att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN;
/* wakeup source */
px4_pollfd_struct_t fds[1];
@ -3124,7 +3159,6 @@ MulticopterPositionControl::task_main()
* attitude setpoints for the transition).
* - if not armed
*/
if (_control_mode.flag_armed &&
(!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||