diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-11-08 23:00:41 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-11-08 23:00:41 +0400 |
commit | d5ae5f237db466ddc1ec7b097738a52671c54aef (patch) | |
tree | 061f3d28db455c152ff464d2b7cc5a6951f5bfb2 /src/modules | |
parent | e8224376ca4d32e948cbee75bfecf8a30f3e98ea (diff) | |
download | px4-firmware-d5ae5f237db466ddc1ec7b097738a52671c54aef.tar.gz px4-firmware-d5ae5f237db466ddc1ec7b097738a52671c54aef.tar.bz2 px4-firmware-d5ae5f237db466ddc1ec7b097738a52671c54aef.zip |
mc_att_control_vector fixes
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp | 13 | ||||
-rw-r--r-- | src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp | 78 |
2 files changed, 36 insertions, 55 deletions
diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp index 252cdafab..589c31c0d 100644 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp @@ -77,12 +77,13 @@ void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, // desired thrust in body frame // avoid division by zero // compensates for thrust loss due to roll/pitch - if (F_des(2) >= 0.1f) { - thrust = F_des(2) / R_bn(2, 2); - } else { - F_des(2) = 0.1f; - thrust= F_des(2) / R_bn(2, 2); - } + //XXX disable this for first time + //if (F_des(2) >= 0.1f) { + // thrust = F_des(2) / R_bn(2, 2); + //} else { + // F_des(2) = 0.1f; + // thrust= F_des(2) / R_bn(2, 2); + //} math::Vector3 x_B_des; math::Vector3 y_B_des; diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 368d343bd..e0b36067c 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -62,17 +62,16 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/actuator_armed.h> #include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> -//#include <ecl/attitude_mc/ecl_mc_att_control_vector.h> #include "ecl_mc_att_control_vector.h" /** @@ -111,7 +110,7 @@ private: int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ - int _vstatus_sub; /**< vehicle status subscription */ + 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 */ @@ -123,7 +122,7 @@ private: struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct vehicle_status_s _vstatus; /**< vehicle status */ + 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 */ @@ -178,9 +177,9 @@ private: void control_update(); /** - * Check for changes in vehicle status. + * Check for changes in vehicle control mode. */ - void vehicle_status_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in manual inputs. @@ -233,7 +232,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* subscriptions */ _att_sub(-1), _accel_sub(-1), - _vstatus_sub(-1), + _control_mode_sub(-1), _params_sub(-1), _manual_sub(-1), _arming_sub(-1), @@ -320,16 +319,16 @@ MulticopterAttitudeControl::parameters_update() } void -MulticopterAttitudeControl::vehicle_status_poll() +MulticopterAttitudeControl::vehicle_control_mode_poll() { - bool vstatus_updated; + bool control_mode_updated; /* Check HIL state if vehicle status has changed */ - orb_check(_vstatus_sub, &vstatus_updated); + orb_check(_control_mode_sub, &control_mode_updated); - if (vstatus_updated) { + if (control_mode_updated) { - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } } @@ -404,14 +403,13 @@ MulticopterAttitudeControl::task_main() _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _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)); - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); - orb_set_interval(_att_sub, 100); + /* rate limit attitude updates to 100Hz */ + orb_set_interval(_att_sub, 10); parameters_update(); @@ -421,7 +419,7 @@ MulticopterAttitudeControl::task_main() /* get an initial update for all sensor and status data */ vehicle_setpoint_poll(); vehicle_accel_poll(); - vehicle_status_poll(); + vehicle_control_mode_poll(); vehicle_manual_poll(); arming_status_poll(); @@ -463,8 +461,6 @@ MulticopterAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { - - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -477,15 +473,9 @@ MulticopterAttitudeControl::task_main() orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); vehicle_setpoint_poll(); - vehicle_accel_poll(); - - /* check vehicle status for changes to publication state */ - vehicle_status_poll(); - - /* check for arming status changes */ + vehicle_control_mode_poll(); arming_status_poll(); - vehicle_manual_poll(); /* lock integrator until armed */ @@ -497,33 +487,22 @@ MulticopterAttitudeControl::task_main() } /* decide if in auto or full manual control */ - float roll_sp, pitch_sp; + float roll_sp = 0.0f; + float pitch_sp = 0.0f; float throttle_sp = 0.0f; float yaw_sp = 0.0f; - if (_vstatus.state_machine == SYSTEM_STATE_MANUAL || - (_vstatus.state_machine == SYSTEM_STATE_STABILIZED)) { - - /* - * Scale down roll and pitch as the setpoints are radians - * and a typical remote can only do 45 degrees, the mapping is - * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. - * - * With this mapping the stick angle is a 1:1 representation of - * the commanded attitude. If more than 45 degrees are desired, - * a scaling parameter can be applied to the remote. - */ - roll_sp = _manual.roll * 0.75f; - pitch_sp = _manual.pitch * 0.75f; - yaw_sp = _manual.yaw; + if (_control_mode.flag_control_manual_enabled) { + roll_sp = _manual.roll; + pitch_sp = _manual.pitch; + yaw_sp = _manual.yaw; // XXX move yaw setpoint throttle_sp = _manual.throttle; - } else if (_vstatus.state_machine == SYSTEM_STATE_AUTO) { - - roll_sp = _att_sp.roll_body; - pitch_sp = _att_sp.pitch_body; - yaw_sp = _att_sp.yaw_body; - throttle_sp = _att_sp.thrust; + } else if (_control_mode.flag_control_auto_enabled) { + roll_sp = _att_sp.roll_body; + pitch_sp = _att_sp.pitch_body; + yaw_sp = _att_sp.yaw_body; + throttle_sp = _att_sp.thrust; } // XXX take rotation matrix directly from att_sp for auto mode @@ -532,6 +511,7 @@ MulticopterAttitudeControl::task_main() math::Vector3 rates_des; math::Dcm R_nb(_att.R); + // XXX actually it's not euler angles rates, but body rates, rename it (?) math::Vector3 angular_rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); _att_control.control(deltaT, R_nb, _att.yaw, F_des, |