From 61398f5c03ad8c894b4c3933adb0272dd9c7d8e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 22 Feb 2015 13:37:14 +0100 Subject: multiplatform port of #1741 and #1801: port attitude setpoint generation in manual to mc_pos_control_multiplatform --- .../mc_pos_control.cpp | 70 ++++++++++++++++++++-- .../mc_pos_control_multiplatform/mc_pos_control.h | 23 +++---- 2 files changed, 78 insertions(+), 15 deletions(-) (limited to 'src/modules') diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 1c0298a07..303123202 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -41,6 +41,9 @@ #include "mc_pos_control.h" #include "mc_pos_control_params.h" +/* The following inclue is needed because the pos controller depens on a parameter from attitude control to set a + * reasonable yaw setpoint in manual mode */ +#include #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f @@ -83,9 +86,10 @@ MulticopterPositionControl::MulticopterPositionControl() : .tilt_max_air = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT), .land_speed = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT), .tilt_max_land = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT), - .man_roll_max = px4::ParameterFloat("MC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT), - .man_pitch_max = px4::ParameterFloat("MC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT), - .man_yaw_max = px4::ParameterFloat("MC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT) + .man_roll_max = px4::ParameterFloat("MPC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT), + .man_pitch_max = px4::ParameterFloat("MPC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT), + .man_yaw_max = px4::ParameterFloat("MPC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT), + .mc_att_yaw_p = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT) }), _ref_alt(0.0f), _ref_timestamp(0), @@ -102,7 +106,6 @@ MulticopterPositionControl::MulticopterPositionControl() : * Do subscriptions */ _att = _n.subscribe(&MulticopterPositionControl::handle_vehicle_attitude, this, 0); - _v_att_sp = _n.subscribe(0); _control_mode = _n.subscribe(0); _parameter_update = _n.subscribe( &MulticopterPositionControl::handle_parameter_update, this, 1000); @@ -154,6 +157,8 @@ MulticopterPositionControl::parameters_update() _params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update()); _params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update()); + _params.mc_att_yaw_p = _params_handles.mc_att_yaw_p.update(); + _params.pos_p(0) = _params.pos_p(1) = _params_handles.xy_p.update(); _params.pos_p(2) = _params_handles.z_p.update(); _params.vel_p(0) = _params.vel_p(1) = _params_handles.xy_vel_p.update(); @@ -577,6 +582,7 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti static bool reset_int_z = true; static bool reset_int_z_manual = false; static bool reset_int_xy = true; + static bool reset_yaw_sp = true; static bool was_armed = false; static uint64_t t_prev = 0; @@ -590,8 +596,10 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; + reset_yaw_sp = true; } + /* Update previous arming state */ was_armed = _control_mode->data().flag_armed; update_ref(); @@ -975,6 +983,60 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti reset_int_xy = true; } + /* generate attitude setpoint from manual controls */ + if(_control_mode->data().flag_control_manual_enabled && _control_mode->data().flag_control_attitude_enabled) { + + /* reset yaw setpoint to current position if needed */ + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp_msg.data().yaw_body = _att->data().yaw; + } + + // do not move yaw while arming + else if (_manual_control_sp->data().z > 0.1f) + { + const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p; + + _att_sp_msg.data().yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max; + _att_sp_msg.data().yaw_body = _wrap_pi(_att_sp_msg.data().yaw_body + _att_sp_msg.data().yaw_sp_move_rate * dt); + float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw); + if (yaw_offs < - YAW_OFFSET_MAX) { + _att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - YAW_OFFSET_MAX); + + } else if (yaw_offs > YAW_OFFSET_MAX) { + _att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + YAW_OFFSET_MAX); + } + } + + /* Control roll and pitch directly if we no aiding velocity controller is active */ + if(!_control_mode->data().flag_control_velocity_enabled) { + _att_sp_msg.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max; + _att_sp_msg.data().pitch_body = -_manual_control_sp->data().x * _params.man_pitch_max; + } + + /* Control climb rate directly if no aiding altitude controller is active */ + if(!_control_mode->data().flag_control_climb_rate_enabled) { + _att_sp_msg.data().thrust = _manual_control_sp->data().z; + } + + /* Construct attitude setpoint rotation matrix */ + math::Matrix<3,3> R_sp; + R_sp.from_euler(_att_sp_msg.data().roll_body,_att_sp_msg.data().pitch_body,_att_sp_msg.data().yaw_body); + memcpy(&_att_sp_msg.data().R_body[0], R_sp.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().timestamp = get_time_micros(); + } + else { + reset_yaw_sp = true; + } + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise(); + } + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled; } diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 59b663b9f..54c6de155 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -88,19 +88,18 @@ protected: Publisher *_att_sp_pub; /**< attitude setpoint publication */ Publisher *_local_pos_sp_pub; /**< vehicle local position setpoint publication */ - Publisher *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + Publisher *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ - Subscriber *_att; /**< vehicle attitude */ - Subscriber *_v_att_sp; /**< vehicle attitude setpoint */ - Subscriber *_control_mode; /**< vehicle control mode */ - Subscriber *_parameter_update; /**< parameter update */ - Subscriber *_manual_control_sp; /**< manual control setpoint */ - Subscriber *_armed; /**< actuator arming status */ - Subscriber *_local_pos; /**< local position */ - Subscriber *_pos_sp_triplet; /**< local position */ - Subscriber *_local_pos_sp; /**< local position */ - Subscriber *_global_vel_sp; /**< local position */ + Subscriber *_att; /**< vehicle attitude */ + Subscriber *_control_mode; /**< vehicle control mode */ + Subscriber *_parameter_update; /**< parameter update */ + Subscriber *_manual_control_sp; /**< manual control setpoint */ + Subscriber *_armed; /**< actuator arming status */ + Subscriber *_local_pos; /**< local position */ + Subscriber *_pos_sp_triplet; /**< local position */ + Subscriber *_local_pos_sp; /**< local position */ + Subscriber *_global_vel_sp; /**< local position */ px4_vehicle_attitude_setpoint _att_sp_msg; px4_vehicle_local_position_setpoint _local_pos_sp_msg; @@ -129,6 +128,7 @@ protected: px4::ParameterFloat man_roll_max; px4::ParameterFloat man_pitch_max; px4::ParameterFloat man_yaw_max; + px4::ParameterFloat mc_att_yaw_p; // needed for calculating reasonable attitude setpoints in manual } _params_handles; /**< handles for interesting parameters */ struct { @@ -140,6 +140,7 @@ protected: float man_roll_max; float man_pitch_max; float man_yaw_max; + float mc_att_yaw_p; math::Vector<3> pos_p; math::Vector<3> vel_p; -- cgit v1.2.3