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

mc_att_control: implemented anti windup

This commit is contained in:
Roman Bapst 2015-04-13 09:52:03 +02:00 committed by Lorenz Meier
parent f23bc38d3e
commit b52d0ed8a2
3 changed files with 49 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
msg/mc_att_ctrl_status.msg Normal file
View File

@ -0,0 +1,5 @@
uint64 timestamp # in microseconds since system start
float32 roll_rate_integ # roll rate inegrator
float32 pitch_rate_integ # pitch rate integrator
float32 yaw_rate_integ # yaw rate integrator

44
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -77,6 +77,8 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/mc_att_ctrl_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
@ -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);
}
}
}

2
src/modules/uORB/objects_common.cpp
View File

@ -254,3 +254,5 @@ ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s);
#include "topics/time_offset.h"
ORB_DEFINE(time_offset, struct time_offset_s);
#include "topics/mc_att_ctrl_status.h"
ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s);