From b52d0ed8a245cc25ffdaf3214f9cf793fe2f1ba6 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 13 Apr 2015 09:52:03 +0200 Subject: mc_att_control: implemented anti windup --- src/modules/mc_att_control/mc_att_control_main.cpp | 44 +++++++++++++++++++++- 1 file changed, 42 insertions(+), 2 deletions(-) (limited to 'src/modules/mc_att_control/mc_att_control_main.cpp') diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index d47731622..b8c143782 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -77,6 +77,8 @@ #include #include #include +#include +#include #include #include #include @@ -129,10 +131,12 @@ private: int _manual_control_sp_sub; /**< manual control setpoint subscription */ int _armed_sub; /**< arming status subscription */ int _vehicle_status_sub; /**< vehicle status subscription */ + int _motor_limits_sub; /**< motor limits subscription */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + orb_advert_t _controller_status_pub; /**< controller status publication */ orb_id_t _rates_sp_id; /**< pointer to correct rates setpoint uORB metadata structure */ orb_id_t _actuators_id; /**< pointer to correct actuator controls0 uORB metadata structure */ @@ -147,6 +151,8 @@ private: struct actuator_controls_s _actuators; /**< actuator controls */ struct actuator_armed_s _armed; /**< actuator arming status */ struct vehicle_status_s _vehicle_status; /**< vehicle status */ + struct multirotor_motor_limits_s _motor_limits; /**< motor limits */ + struct mc_att_ctrl_status_s _controller_status; /**< controller status */ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _controller_latency_perf; @@ -261,6 +267,11 @@ private: */ void vehicle_status_poll(); + /** + * Check for vehicle motor limits status. + */ + void vehicle_motor_limits_poll(); + /** * Shim for calling task_main from task_create. */ @@ -302,6 +313,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), + _controller_status_pub(-1), _rates_sp_id(0), _actuators_id(0), @@ -320,6 +332,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_actuators, 0, sizeof(_actuators)); memset(&_armed, 0, sizeof(_armed)); memset(&_vehicle_status, 0, sizeof(_vehicle_status)); + memset(&_motor_limits, 0, sizeof(_motor_limits)); + memset(&_controller_status,0,sizeof(_controller_status)); _vehicle_status.is_rotary_wing = true; _params.att_p.zero(); @@ -570,6 +584,18 @@ MulticopterAttitudeControl::vehicle_status_poll() } } +void +MulticopterAttitudeControl::vehicle_motor_limits_poll() +{ + /* check if there is a new message */ + bool updated; + orb_check(_motor_limits_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &_motor_limits); + } +} + /* * Attitude controller. * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) @@ -689,8 +715,8 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _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_prev = rates; - /* update integral only if not saturated on low limit */ - if (_thrust_sp > MIN_TAKEOFF_THRUST) { + /* 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; @@ -725,6 +751,7 @@ MulticopterAttitudeControl::task_main() _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); /* initialize parameters cache */ parameters_update(); @@ -777,6 +804,7 @@ MulticopterAttitudeControl::task_main() arming_status_poll(); vehicle_manual_poll(); vehicle_status_poll(); + vehicle_motor_limits_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -837,6 +865,11 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _v_att.timestamp; + _controller_status.roll_rate_integ = _rates_int(0); + _controller_status.pitch_rate_integ = _rates_int(1); + _controller_status.yaw_rate_integ = _rates_int(2); + _controller_status.timestamp = hrt_absolute_time(); + if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); @@ -847,6 +880,13 @@ MulticopterAttitudeControl::task_main() } } + + /* publish controller status */ + if(_controller_status_pub > 0) { + orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status); + } else { + _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); + } } } -- cgit v1.2.3