mc att control: ran fix code style script

This commit is contained in:
Thomas Gubler 2014-12-05 09:02:06 +01:00
parent eafc4b5f9e
commit 1c19d75cf4
5 changed files with 71 additions and 53 deletions

View File

@ -83,10 +83,12 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
_I.identity(); _I.identity();
} }
MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase()
{
} }
void MulticopterAttitudeControlBase::control_attitude(float dt) { void MulticopterAttitudeControlBase::control_attitude(float dt)
{
float yaw_sp_move_rate = 0.0f; float yaw_sp_move_rate = 0.0f;
_publish_att_sp = false; _publish_att_sp = false;
@ -124,12 +126,14 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
if (yaw_offs < -yaw_offs_max) { if (yaw_offs < -yaw_offs_max) {
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
} else if (yaw_offs > yaw_offs_max) { } else if (yaw_offs > yaw_offs_max) {
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
} }
_v_att_sp.R_valid = false; _v_att_sp.R_valid = false;
_publish_att_sp = true; _publish_att_sp = true;
} }
@ -259,7 +263,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) {
_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
} }
void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
{
/* reset integral if disarmed */ /* reset integral if disarmed */
if (!_armed.armed) { if (!_armed.armed) {
_rates_int.zero(); _rates_int.zero();
@ -295,7 +300,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
} }
void MulticopterAttitudeControlBase::set_actuator_controls() { void MulticopterAttitudeControlBase::set_actuator_controls()
{
_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; _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[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;

View File

@ -64,7 +64,8 @@
#define MIN_TAKEOFF_THRUST 0.2f #define MIN_TAKEOFF_THRUST 0.2f
#define RATES_I_LIMIT 0.3f #define RATES_I_LIMIT 0.3f
class MulticopterAttitudeControlBase { class MulticopterAttitudeControlBase
{
public: public:
/** /**
* Constructor * Constructor

View File

@ -479,8 +479,9 @@ MulticopterAttitudeControl::task_main()
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */ /* timed out - periodic check for _task_should_exit */
if (pret == 0) if (pret == 0) {
continue; continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */ /* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) { if (pret < 0) {
@ -525,6 +526,7 @@ MulticopterAttitudeControl::task_main()
if (_att_sp_pub > 0) { if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
&_v_att_sp); &_v_att_sp);
} else { } else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
&_v_att_sp); &_v_att_sp);
@ -549,7 +551,8 @@ MulticopterAttitudeControl::task_main()
/* attitude controller disabled, poll rates setpoint topic */ /* attitude controller disabled, poll rates setpoint topic */
if (_v_control_mode.flag_control_manual_enabled) { if (_v_control_mode.flag_control_manual_enabled) {
/* manual rates control - ACRO mode */ /* manual rates control - ACRO mode */
_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
_manual_control_sp.r).emult(_params.acro_rate_max);
_thrust_sp = _manual_control_sp.z; _thrust_sp = _manual_control_sp.z;
/* reset yaw setpoint after ACRO */ /* reset yaw setpoint after ACRO */
@ -632,18 +635,21 @@ MulticopterAttitudeControl::start()
int mc_att_control_main(int argc, char *argv[]) int mc_att_control_main(int argc, char *argv[])
{ {
if (argc < 1) if (argc < 1) {
errx(1, "usage: mc_att_control {start|stop|status}"); errx(1, "usage: mc_att_control {start|stop|status}");
}
if (!strcmp(argv[1], "start")) { if (!strcmp(argv[1], "start")) {
if (mc_att_control::g_control != nullptr) if (mc_att_control::g_control != nullptr) {
errx(1, "already running"); errx(1, "already running");
}
mc_att_control::g_control = new MulticopterAttitudeControl; mc_att_control::g_control = new MulticopterAttitudeControl;
if (mc_att_control::g_control == nullptr) if (mc_att_control::g_control == nullptr) {
errx(1, "alloc failed"); errx(1, "alloc failed");
}
if (OK != mc_att_control::g_control->start()) { if (OK != mc_att_control::g_control->start()) {
delete mc_att_control::g_control; delete mc_att_control::g_control;
@ -655,8 +661,9 @@ int mc_att_control_main(int argc, char *argv[])
} }
if (!strcmp(argv[1], "stop")) { if (!strcmp(argv[1], "stop")) {
if (mc_att_control::g_control == nullptr) if (mc_att_control::g_control == nullptr) {
errx(1, "not running"); errx(1, "not running");
}
delete mc_att_control::g_control; delete mc_att_control::g_control;
mc_att_control::g_control = nullptr; mc_att_control::g_control = nullptr;

View File

@ -46,7 +46,8 @@
using namespace std; using namespace std;
#endif #endif
void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude) { void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude)
{
math::Quaternion quat; math::Quaternion quat;
quat(0) = (float)attitude.w(); quat(0) = (float)attitude.w();
quat(1) = (float)attitude.x(); quat(1) = (float)attitude.x();
@ -72,14 +73,16 @@ void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double>
_v_att.R_valid = true; _v_att.R_valid = true;
} }
void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d& angular_rate) { void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate)
{
// check if this is consistent !!! // check if this is consistent !!!
_v_att.rollspeed = angular_rate(0); _v_att.rollspeed = angular_rate(0);
_v_att.pitchspeed = angular_rate(1); _v_att.pitchspeed = angular_rate(1);
_v_att.yawspeed = angular_rate(2); _v_att.yawspeed = angular_rate(2);
} }
void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference)
{
_v_att_sp.roll_body = control_attitude_thrust_reference(0); _v_att_sp.roll_body = control_attitude_thrust_reference(0);
_v_att_sp.pitch_body = control_attitude_thrust_reference(1); _v_att_sp.pitch_body = control_attitude_thrust_reference(1);
_v_att_sp.yaw_body = control_attitude_thrust_reference(2); _v_att_sp.yaw_body = control_attitude_thrust_reference(2);
@ -99,7 +102,8 @@ void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d
_v_att_sp.R_body[2][2] = Rot_sp(2, 2); _v_att_sp.R_body[2][2] = Rot_sp(2, 2);
} }
void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d& motor_inputs) { void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs)
{
motor_inputs(0) = _actuators.control[0]; motor_inputs(0) = _actuators.control[0];
motor_inputs(1) = _actuators.control[1]; motor_inputs(1) = _actuators.control[1];
motor_inputs(2) = _actuators.control[2]; motor_inputs(2) = _actuators.control[2];