diff options
Diffstat (limited to 'src/modules/mc_att_control')
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 798 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_params.c | 15 |
2 files changed, 453 insertions, 360 deletions
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 245ac024b..db5e2e9bb 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -85,7 +85,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THROTTLE 0.3f -#define YAW_DEADZONE 0.01f +#define YAW_DEADZONE 0.05f #define RATES_I_LIMIT 0.5f class MulticopterAttitudeControl @@ -113,45 +113,66 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _att_sp_sub; /**< vehicle attitude setpoint */ - int _control_mode_sub; /**< vehicle control mode subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ - int _arming_sub; /**< arming status of outputs */ + int _v_att_sub; /**< vehicle attitude subscription */ + int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ + int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ + int _v_control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< parameter updates subscription */ + int _manual_control_sp_sub; /**< manual control setpoint subscription */ + int _armed_sub; /**< arming status subscription */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ - orb_advert_t _rates_sp_pub; /**< rate setpoint publication */ - orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_rates_setpoint_s _rates_sp; /**< vehicle rates setpoint */ + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ + struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ + struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator controls */ + struct actuator_armed_s _armed; /**< actuator arming status */ perf_counter_t _loop_perf; /**< loop performance counter */ - math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */ + math::Matrix<3, 3> _R; /**< rotation matrix for current state */ + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp; /**< angular rates setpoint */ + math::Vector<3> _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + math::Vector<3> _att_control; /**< attitude control vector */ + + math::Matrix<3, 3> I; /**< identity matrix */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ struct { - param_t att_p; + param_t roll_p; + param_t roll_rate_p; + param_t roll_rate_i; + param_t roll_rate_d; + param_t pitch_p; + param_t pitch_rate_p; + param_t pitch_rate_i; + param_t pitch_rate_d; param_t yaw_p; - param_t att_rate_p; - param_t att_rate_i; - param_t att_rate_d; param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; + param_t yaw_ff; + + param_t rc_scale_yaw; } _params_handles; /**< handles for interesting parameters */ struct { - math::Vector<3> p; /**< P gain for angular error */ + math::Vector<3> att_p; /**< P gain for angular error */ math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + + float rc_scale_yaw; } _params; /** @@ -160,9 +181,9 @@ private: int parameters_update(); /** - * Update control outputs + * Check for parameter update and handle it. */ - void control_update(); + void parameter_update_poll(); /** * Check for changes in vehicle control mode. @@ -175,9 +196,14 @@ private: void vehicle_manual_poll(); /** - * Check for set triplet updates. + * Check for attitude setpoint updates. */ - void vehicle_setpoint_poll(); + void vehicle_attitude_setpoint_poll(); + + /** + * Check for rates setpoint updates. + */ + void vehicle_rates_setpoint_poll(); /** * Check for arming status updates. @@ -185,6 +211,16 @@ private: void arming_status_poll(); /** + * Attitude controller. + */ + void control_attitude(float dt); + + /** + * Attitude rates controller. + */ + void control_attitude_rates(float dt); + + /** * Shim for calling task_main from task_create. */ static void task_main_trampoline(int argc, char *argv[]); @@ -195,7 +231,7 @@ private: void task_main() __attribute__((noreturn)); }; -namespace att_control +namespace mc_att_control { /* oddly, ERROR is not defined for c++ */ @@ -213,43 +249,58 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _control_task(-1), /* subscriptions */ - _att_sub(-1), - _att_sp_sub(-1), - _control_mode_sub(-1), + _v_att_sub(-1), + _v_att_sp_sub(-1), + _v_control_mode_sub(-1), _params_sub(-1), - _manual_sub(-1), - _arming_sub(-1), + _manual_control_sp_sub(-1), + _armed_sub(-1), /* publications */ _att_sp_pub(-1), - _rates_sp_pub(-1), + _v_rates_sp_pub(-1), _actuators_0_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) { - memset(&_att, 0, sizeof(_att)); - memset(&_att_sp, 0, sizeof(_att_sp)); - memset(&_manual, 0, sizeof(_manual)); - memset(&_control_mode, 0, sizeof(_control_mode)); - memset(&_arming, 0, sizeof(_arming)); + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_armed, 0, sizeof(_armed)); - _params.p.zero(); + _params.att_p.zero(); _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _R_sp.identity(); + _R.identity(); _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); - _params_handles.att_p = param_find("MC_ATT_P"); - _params_handles.yaw_p = param_find("MC_YAW_P"); - _params_handles.att_rate_p = param_find("MC_ATTRATE_P"); - _params_handles.att_rate_i = param_find("MC_ATTRATE_I"); - _params_handles.att_rate_d = param_find("MC_ATTRATE_D"); - _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + I.identity(); + + _params_handles.roll_p = param_find("MC_ROLL_P"); + _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(); @@ -276,7 +327,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } while (_control_task != -1); } - att_control::g_control = nullptr; + mc_att_control::g_control = nullptr; } int @@ -284,421 +335,460 @@ MulticopterAttitudeControl::parameters_update() { float v; - param_get(_params_handles.att_p, &v); - _params.p(0) = v; - _params.p(1) = v; - param_get(_params_handles.yaw_p, &v); - _params.p(2) = v; - - param_get(_params_handles.att_rate_p, &v); + /* roll */ + param_get(_params_handles.roll_p, &v); + _params.att_p(0) = v; + param_get(_params_handles.roll_rate_p, &v); _params.rate_p(0) = v; + param_get(_params_handles.roll_rate_i, &v); + _params.rate_i(0) = v; + param_get(_params_handles.roll_rate_d, &v); + _params.rate_d(0) = v; + + /* pitch */ + param_get(_params_handles.pitch_p, &v); + _params.att_p(1) = v; + param_get(_params_handles.pitch_rate_p, &v); _params.rate_p(1) = v; + param_get(_params_handles.pitch_rate_i, &v); + _params.rate_i(1) = v; + param_get(_params_handles.pitch_rate_d, &v); + _params.rate_d(1) = v; + + /* yaw */ + param_get(_params_handles.yaw_p, &v); + _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); _params.rate_p(2) = v; - - param_get(_params_handles.att_rate_i, &v); - _params.rate_i(0) = v; - _params.rate_i(1) = v; param_get(_params_handles.yaw_rate_i, &v); _params.rate_i(2) = v; - - param_get(_params_handles.att_rate_d, &v); - _params.rate_d(0) = v; - _params.rate_d(1) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; + param_get(_params_handles.yaw_ff, &_params.yaw_ff); + + param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + return OK; } void -MulticopterAttitudeControl::vehicle_control_mode_poll() +MulticopterAttitudeControl::parameter_update_poll() { - bool control_mode_updated; + bool updated; /* Check HIL state if vehicle status has changed */ - orb_check(_control_mode_sub, &control_mode_updated); + orb_check(_params_sub, &updated); + + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} + +void +MulticopterAttitudeControl::vehicle_control_mode_poll() +{ + bool updated; - if (control_mode_updated) { + /* Check HIL state if vehicle status has changed */ + orb_check(_v_control_mode_sub, &updated); - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); } } void MulticopterAttitudeControl::vehicle_manual_poll() { - bool manual_updated; + bool updated; /* get pilots inputs */ - orb_check(_manual_sub, &manual_updated); + orb_check(_manual_control_sp_sub, &updated); - if (manual_updated) { + if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); } } void -MulticopterAttitudeControl::vehicle_setpoint_poll() +MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() { /* check if there is a new setpoint */ - bool att_sp_updated; - orb_check(_att_sp_sub, &att_sp_updated); + bool updated; + orb_check(_v_att_sp_sub, &updated); - if (att_sp_updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); } } void -MulticopterAttitudeControl::arming_status_poll() +MulticopterAttitudeControl::vehicle_rates_setpoint_poll() { /* check if there is a new setpoint */ - bool arming_updated; - orb_check(_arming_sub, &arming_updated); + bool updated; + orb_check(_v_rates_sp_sub, &updated); - if (arming_updated) { - orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + if (updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); } } void -MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) +MulticopterAttitudeControl::arming_status_poll() { - att_control::g_control->task_main(); + /* check if there is a new setpoint */ + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } } +/* + * Attitude controller. + * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) + * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) + */ void -MulticopterAttitudeControl::task_main() +MulticopterAttitudeControl::control_attitude(float dt) { - /* inform about start */ - warnx("started"); - fflush(stdout); + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; - /* - * do subscriptions - */ - _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + if (_v_control_mode.flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ - /* rate limit attitude updates to 100Hz */ - orb_set_interval(_att_sub, 10); + if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + vehicle_attitude_setpoint_poll(); + } - parameters_update(); + if (!_v_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp.thrust = _manual_control_sp.throttle; + publish_att_sp = true; + } - /* initialize values of critical structs until first regular update */ - _arming.armed = false; + if (!_armed.armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } - /* get an initial update for all sensor and status data */ - vehicle_setpoint_poll(); - vehicle_control_mode_poll(); - vehicle_manual_poll(); - arming_status_poll(); + /* move yaw setpoint in all modes */ + if (_v_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; - /* setpoint rotation matrix */ - math::Matrix<3, 3> R_sp; - R_sp.identity(); + if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; - /* rotation matrix for current state */ - math::Matrix<3, 3> R; - R.identity(); + if (_manual_control_sp.yaw > 0.0f) { + yaw_sp_move_rate -= YAW_DEADZONE; - /* current angular rates */ - math::Vector<3> rates; - rates.zero(); + } else { + yaw_sp_move_rate += YAW_DEADZONE; + } - /* angular rates integral error */ - math::Vector<3> rates_int; - rates_int.zero(); + yaw_sp_move_rate *= _params.rc_scale_yaw; + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + } - /* identity matrix */ - math::Matrix<3, 3> I; - I.identity(); + /* reset yaw setpint to current position if needed */ + if (_reset_yaw_sp) { + _reset_yaw_sp = false; + _v_att_sp.yaw_body = _v_att.yaw; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } - math::Quaternion q; + if (!_v_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp.roll_body = _manual_control_sp.roll; + _v_att_sp.pitch_body = _manual_control_sp.pitch; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } - bool reset_yaw_sp = true; + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + vehicle_attitude_setpoint_poll(); - /* wakeup source(s) */ - struct pollfd fds[2]; + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } - /* Setup of loop */ - fds[0].fd = _params_sub; - fds[0].events = POLLIN; - fds[1].fd = _att_sub; - fds[1].events = POLLIN; + _thrust_sp = _v_att_sp.thrust; - while (!_task_should_exit) { + /* construct attitude setpoint rotation matrix */ + if (_v_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + _R_sp.set(&_v_att_sp.R_body[0][0]); - /* wait for up to 500ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + _R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) - continue; + /* copy rotation matrix back to setpoint struct */ + memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body)); + _v_att_sp.R_valid = true; + } - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } + /* publish the attitude setpoint if needed */ + if (publish_att_sp) { + _v_att_sp.timestamp = hrt_absolute_time(); - perf_begin(_loop_perf); + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); - /* only update parameters if they changed */ - if (fds[0].revents & POLLIN) { - /* copy the topic to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - parameters_update(); + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); } + } - /* only run controller if attitude changed */ - if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; - float dt = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); + /* rotation matrix for current state */ + _R.set(_v_att.R); - /* guard against too large dt's */ - if (dt > 0.02f) - dt = 0.02f; + /* all input data is ready, run controller itself */ - /* copy attitude topic */ - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2)); + math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2)); - vehicle_setpoint_poll(); - vehicle_control_mode_poll(); - arming_status_poll(); - vehicle_manual_poll(); + /* axis and sin(angle) of desired rotation */ + math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z); - float yaw_sp_move_rate = 0.0f; - bool publish_att_sp = false; + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; - /* define which input is the dominating control input */ - if (_control_mode.flag_control_manual_enabled) { - /* manual input */ - if (!_control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - _att_sp.thrust = _manual.throttle; - } + /* calculate weight for yaw control */ + float yaw_w = _R_sp(2, 2) * _R_sp(2, 2); - if (!_arming.armed) { - /* reset yaw setpoint when disarmed */ - reset_yaw_sp = true; - } + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; - if (_control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - - if (_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual.yaw; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); - _att_sp.R_valid = false; - publish_att_sp = true; - } - } - - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; - _att_sp.R_valid = false; - publish_att_sp = true; - } - - if (!_control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - _att_sp.roll_body = _manual.roll; - _att_sp.pitch_body = _manual.pitch; - _att_sp.R_valid = false; - publish_att_sp = true; - } + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; - } else { - /* manual rate inputs (ACRO) */ - // TODO - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; - } + e_R = e_R_z_axis * e_R_z_angle; - } else { - /* reset yaw setpoint after non-manual control */ - reset_yaw_sp = true; - } + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); - if (_att_sp.R_valid) { - /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_att_sp.R_body[0][0]); + /* rotation matrix for roll/pitch only rotation */ + R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); - } else { - /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); + } else { + /* zero roll/pitch rotation */ + R_rp = _R; + } - /* copy rotation matrix back to setpoint struct */ - memcpy(&_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_att_sp.R_body)); - _att_sp.R_valid = true; - } + /* R_rp and _R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0)); + math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + math::Quaternion q; + q.from_dcm(_R.transposed() * _R_sp); + math::Vector<3> e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } - if (publish_att_sp) { - /* publish the attitude setpoint */ - _att_sp.timestamp = hrt_absolute_time(); + /* calculate angular rates setpoint */ + _rates_sp = _params.att_p.emult(e_R); - if (_att_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; +} - } else { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); +/* + * Attitude rates controller. + * Input: '_rates_sp' vector, '_thrust_sp' + * Output: '_att_control' vector + */ +void +MulticopterAttitudeControl::control_attitude_rates(float dt) +{ + /* reset integral if disarmed */ + if (!_armed.armed) { + _rates_int.zero(); + } + + /* current body angular rates */ + math::Vector<3> rates; + rates(0) = _v_att.rollspeed; + rates(1) = _v_att.pitchspeed; + rates(2) = _v_att.yawspeed; + + /* angular rates error */ + math::Vector<3> rates_err = _rates_sp - rates; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > 0.1f) { + 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; + + if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; } } + } + } +} - /* rotation matrix for current state */ - R.set(_att.R); - - /* current body angular rates */ - rates(0) = _att.rollspeed; - rates(1) = _att.pitchspeed; - rates(2) = _att.yawspeed; +void +MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + mc_att_control::g_control->task_main(); +} - /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); - math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); +void +MulticopterAttitudeControl::task_main() +{ + warnx("started"); + fflush(stdout); - /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); + /* + * do subscriptions + */ + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - /* calculate angle error */ - float e_R_z_sin = e_R.length(); - float e_R_z_cos = R_z * R_sp_z; + /* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */ + orb_set_interval(_v_att_sub, 5); - /* calculate weight for yaw control */ - float yaw_w = R_sp(2, 2) * R_sp(2, 2); + /* initialize parameters cache */ + parameters_update(); - /* calculate rotation matrix after roll/pitch only rotation */ - math::Matrix<3, 3> R_rp; + /* wakeup source: vehicle attitude */ + struct pollfd fds[1]; - if (e_R_z_sin > 0.0f) { - /* get axis-angle representation */ - float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); - math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; + fds[0].fd = _v_att_sub; + fds[0].events = POLLIN; - e_R = e_R_z_axis * e_R_z_angle; + while (!_task_should_exit) { - /* cross product matrix for e_R_axis */ - math::Matrix<3, 3> e_R_cp; - e_R_cp.zero(); - e_R_cp(0, 1) = -e_R_z_axis(2); - e_R_cp(0, 2) = e_R_z_axis(1); - e_R_cp(1, 0) = e_R_z_axis(2); - e_R_cp(1, 2) = -e_R_z_axis(0); - e_R_cp(2, 0) = -e_R_z_axis(1); - e_R_cp(2, 1) = e_R_z_axis(0); + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - /* rotation matrix for roll/pitch only rotation */ - R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) + continue; - } else { - /* zero roll/pitch rotation */ - R_rp = R; - } + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(100000); + continue; + } - /* R_rp and R_sp has the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); - math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); - e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; - - if (e_R_z_cos < 0.0f) { - /* for large thrust vector rotations use another rotation method: - * calculate angle and axis for R -> R_sp rotation directly */ - q.from_dcm(R.transposed() * R_sp); - math::Vector<3> e_R_d = q.imag(); - e_R_d.normalize(); - e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); - - /* use fusion of Z axis based rotation and direct rotation */ - float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; - e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; - } + perf_begin(_loop_perf); - /* angular rates setpoint*/ - math::Vector<3> rates_sp = _params.p.emult(e_R); + /* run controller on attitude changes */ + if (fds[0].revents & POLLIN) { + static uint64_t last_run = 0; + float dt = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); - /* feed forward yaw setpoint rate */ - rates_sp(2) += yaw_sp_move_rate * yaw_w; + /* guard against too small (< 2ms) and too large (> 20ms) dt's */ + if (dt < 0.002f) { + dt = 0.002f; - /* reset integral if disarmed */ - // TODO add LANDED flag here - if (!_arming.armed) { - rates_int.zero(); + } else if (dt > 0.02f) { + dt = 0.02f; } - /* rate controller */ - math::Vector<3> rates_err = rates_sp - rates; - math::Vector<3> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; - _rates_prev = rates; + /* copy attitude topic */ + orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att); - /* update integral */ - for (int i = 0; i < 3; i++) { - float rate_i = rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + /* check for updates in other topics */ + parameter_update_poll(); + vehicle_control_mode_poll(); + arming_status_poll(); + vehicle_manual_poll(); - if (isfinite(rate_i)) { - if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) { - rates_int(i) = rate_i; - } - } - } + if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); - /* publish the attitude rates setpoint */ - _rates_sp.roll = rates_sp(0); - _rates_sp.pitch = rates_sp(1); - _rates_sp.yaw = rates_sp(2); - _rates_sp.thrust = _att_sp.thrust; - _rates_sp.timestamp = hrt_absolute_time(); + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); - if (_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } } else { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); + /* attitude controller disabled */ + // TODO poll 'attitude_rates_setpoint' topic + _rates_sp.zero(); + _thrust_sp = 0.0f; } - /* publish the attitude controls */ - if (_control_mode.flag_control_rates_enabled) { - _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; - _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; - _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; - _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; - _actuators.timestamp = hrt_absolute_time(); + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt); - } else { - /* controller disabled, publish zero attitude controls */ - _actuators.control[0] = 0.0f; - _actuators.control[1] = 0.0f; - _actuators.control[2] = 0.0f; - _actuators.control[3] = 0.0f; + /* publish actuator controls */ + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); - } - if (_actuators_0_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - } else { - /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } } } @@ -739,17 +829,17 @@ int mc_att_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { - if (att_control::g_control != nullptr) + if (mc_att_control::g_control != nullptr) errx(1, "already running"); - att_control::g_control = new MulticopterAttitudeControl; + mc_att_control::g_control = new MulticopterAttitudeControl; - if (att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) errx(1, "alloc failed"); - if (OK != att_control::g_control->start()) { - delete att_control::g_control; - att_control::g_control = nullptr; + if (OK != mc_att_control::g_control->start()) { + delete mc_att_control::g_control; + mc_att_control::g_control = nullptr; err(1, "start failed"); } @@ -757,16 +847,16 @@ int mc_att_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (att_control::g_control == nullptr) + if (mc_att_control::g_control == nullptr) errx(1, "not running"); - delete att_control::g_control; - att_control::g_control = nullptr; + delete mc_att_control::g_control; + mc_att_control::g_control = nullptr; exit(0); } if (!strcmp(argv[1], "status")) { - if (att_control::g_control) { + if (mc_att_control::g_control) { errx(0, "running"); } else { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index a170365ee..27a45b6bb 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -41,13 +41,16 @@ #include <systemlib/param/param.h> -PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f); -PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); -PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); |